Skip to content
Snippets Groups Projects
Commit 8b8be58a authored by Alejandro Suarez Hernandez's avatar Alejandro Suarez Hernandez
Browse files

example of usage of gripper plugin with WAM robot

parent f133d6a9
No related branches found
No related tags found
No related merge requests found
......@@ -19,6 +19,7 @@ class BlackHolePlugin : public ModelPlugin
private:
transport::SubscriberPtr contactSub_;
physics::ModelPtr model_;
physics::LinkPtr link_;
physics::ContactManager* contact_manager_;
math::Pose trash_;
transport::NodePtr node_;
......@@ -27,8 +28,15 @@ class BlackHolePlugin : public ModelPlugin
void read_options(sdf::ElementPtr sdf)
{
// Read gripper link
sdf::ElementPtr elem;
// Read black hole link
if (sdf->HasElement("link"))
{
elem = sdf->GetElement("link");
std::string link_name = elem->GetValue()->GetAsString();
link_ = model_->GetLink(link_name);
}
if (sdf->HasElement("star"))
{
......@@ -87,14 +95,25 @@ class BlackHolePlugin : public ModelPlugin
*/
void obtain_collision_names()
{
collision_names_.clear();
physics::Link_V links = model_->GetLinks();
for (int i = 0; i < links.size(); ++i)
//collision_names_.clear();
if (link_)
{
auto collisions = link_->GetCollisions();
for (auto collision : collisions)
{
collision_names_.push_back(collision->GetScopedName());
}
}
else
{
physics::Collision_V collisions = links[i]->GetCollisions();
for (int j = 0; j < collisions.size(); ++j)
physics::Link_V links = model_->GetLinks();
for (auto link : links)
{
collision_names_.push_back(collisions[j]->GetScopedName());
physics::Collision_V collisions = link->GetCollisions();
for (auto collision : collisions)
{
collision_names_.push_back(collision->GetScopedName());
}
}
}
}
......
......@@ -109,7 +109,7 @@ generate_messages(
catkin_package(
# INCLUDE_DIRS include
LIBRARIES gz_gripper_plugin
CATKIN_DEPENDS actionlib gazebo_plugins gazebo_ros iri_common_drivers_msgs tf std_msgs message_runtime
CATKIN_DEPENDS actionlib gazebo_plugins gazebo_ros iri_common_drivers_msgs tf std_msgs message_runtime iri_wam_description iri_wam_gazebo
# DEPENDS gazebo
)
......@@ -132,6 +132,9 @@ add_library(gz_gripper_plugin
src/gz_gripper_plugin.cpp
)
add_dependencies(gz_gripper_plugin gz_gripper_plugin_generate_messages_cpp)
add_dependencies(gz_gripper_plugin iri_common_drivers_msgs_generate_messages_cpp)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
......
<launch>
<arg name="ROBOT" default="iri_wam"/>
<arg name="IK" default="true"/>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="world" default="empty"/>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find iri_wam_gazebo)/worlds/iri_wam_$(arg world).world"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<param name="use_gui" value="$(arg gui)" />
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description"
command="$(find xacro)/xacro --inorder '$(find gz_gripper_plugin)/xacro/iri_wam.urdf.xacro'" />
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model iri_wam -param robot_description"/>
<!-- ros_control iri_wam launch file -->
<include file="$(find iri_wam_gazebo)/launch/iri_wam_control.launch" />
<group ns="$(arg ROBOT)">
<!-- Publish Robot Inverse Kinematics -->
<include file="$(find iri_wam_ik)/launch/iri_wam_ik.launch" if="$(arg IK)" >
<arg name="ROBOT" value="$(arg ROBOT)" />
</include>
<!-- Publish TCP Robot Inverse Kinematics -->
<node name="$(arg ROBOT)_tcp_ik" pkg="iri_wam_tcp_ik" type="iri_wam_tcp_ik" if="$(arg IK)" >
<param name="robot_base" type="str" value="/$(arg ROBOT)_link_base" />
<!-- Here, robot_tcp and tool_tcp are the same because if not the algorithm would take
the link_7 to link_tcp transformation twice -->
<param name="robot_tcp" type="str" value="/$(arg ROBOT)_link_tcp" />
<param name="tool_tcp" type="str" value="/$(arg ROBOT)_link_tcp" />
<remap from="$(arg ROBOT)_tcp_ik/wamik" to="$(arg ROBOT)_ik/get_wam_ik" />
</node>
</group>
</launch>
......@@ -57,7 +57,8 @@
<run_depend>tf</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>iri_wam_description</run_depend>
<run_depend>iri_wam_gazebo</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
......
......@@ -8,6 +8,7 @@
#include <tf/transform_listener.h>
#include <iri_common_drivers_msgs/tool_closeAction.h>
#include <iri_common_drivers_msgs/tool_openAction.h>
#include <gz_gripper_plugin/CheckGrasped.h>
namespace gazebo
{
......@@ -43,6 +44,9 @@ class GripperPlugin : public ModelPlugin
actionlib::SimpleActionServer<iri_common_drivers_msgs::tool_openAction>*
open_as_;
// service servers
ros::ServiceServer check_ss_;
template <typename T>
static void set_option(sdf::ElementPtr sdf, const std::string& option,
T& value, const T& default_)
......@@ -294,6 +298,13 @@ class GripperPlugin : public ModelPlugin
close_as_->setSucceeded(result);
}
bool check_grasped(gz_gripper_plugin::CheckGrasped::Request&,
gz_gripper_plugin::CheckGrasped::Response& res)
{
res.grasped = attached_graspable_ ? attached_graspable_->GetScopedName() : "";
return true;
}
public:
virtual void Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
{
......@@ -329,6 +340,10 @@ class GripperPlugin : public ModelPlugin
iri_common_drivers_msgs::tool_closeAction>(*nh_, "gripper/close_tool",
boost::bind(&GripperPlugin::close_callback, this, _1), false);
check_ss_ = nh_->advertiseService("check_grasped",
&GripperPlugin::check_grasped,
this);
open_as_->start();
close_as_->start();
......
---
std_msgs/String grasped
string grasped
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="M_PI" value="3.1415926535897931" />
<xacro:macro name="gripper" params="prefix parent">
<!--<link name="${prefix}_gripper">-->
<!--<visual>-->
<!--<geometry>-->
<!--<sphere radius="0.04"/>-->
<!--</geometry>-->
<!--</visual>-->
<!--</link>-->
<!--<joint name="${prefix}_gripper_j" type="fixed">-->
<!--<parent link="${parent}" />-->
<!--<child link="${prefix}_gripper" />-->
<!--<origin xyz="0 0 0.04" rpy="0 0 0" />-->
<!--</joint>-->
<link name="${prefix}_gripper_tcp" />
<joint name="${prefix}_gripper_tcp_j" type="fixed" >
<parent link="${parent}" />
<child link="${prefix}_gripper_tcp" />
<origin xyz="0.0 0.0 0.115" rpy="0 0 0" />
</joint>
<gazebo>
<plugin filename="libgz_gripper_plugin.so" name="gripper_plugin">
<gripper_link>${prefix}_link_7</gripper_link>
<gripper_tf>${prefix}_gripper_tcp</gripper_tf>
</plugin>
</gazebo>
<gazebo reference="${prefix}_gripper">
<material>Gazebo/Grey</material>
</gazebo>
</xacro:macro>
</robot>
<?xml version="1.0"?>
<robot name="iri_wam" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Set up definitions of parts of the robot body -->
<xacro:include filename="$(find iri_wam_description)/xacro/iri_wam_base.urdf.xacro" />
<xacro:include filename="$(find iri_wam_description)/xacro/iri_wam_1.urdf.xacro" />
<xacro:include filename="$(find iri_wam_description)/xacro/iri_wam_2.urdf.xacro" />
<xacro:include filename="$(find iri_wam_description)/xacro/iri_wam_3.urdf.xacro" />
<xacro:include filename="$(find iri_wam_description)/xacro/iri_wam_4.urdf.xacro" />
<xacro:include filename="$(find iri_wam_description)/xacro/iri_wam_5.urdf.xacro" />
<xacro:include filename="$(find iri_wam_description)/xacro/iri_wam_6.urdf.xacro" />
<xacro:include filename="$(find iri_wam_description)/xacro/iri_wam_7.urdf.xacro" />
<xacro:include filename="$(find iri_wam_description)/xacro/iri_wam_tcp.urdf.xacro" />
<xacro:include filename="$(find gz_gripper_plugin)/xacro/gripper.urdf.xacro" />
<!-- The first use of a macro. This one was defined in wambase.urdf.xacro above.
A macro like this will expand to a set of link and joint definitions, and to additional
Gazebo-related extensions (sensor plugins, etc). The macro takes an argument, name,
that equals "wam", and uses it to generate names for its component links and joints
(e.g., wam_link_base). The included origin block is also an argument to the macro. By convention,
the origin block defines where the component is w.r.t its parent (in this case the parent
is the world frame). For more, see http://www.ros.org/wiki/xacro -->
<xacro:property name="robot_name" value="iri_wam"/>
<xacro:property name="JointInterface" value="hardware_interface/EffortJointInterface"/>
<!-- <xacro:property name="JointInterface" value="PositionJointInterface"/> -->
<!-- This will attach the robot to the world link -->
<link name="world"/>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="${robot_name}_link_footprint"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<xacro:wambase prefix="${robot_name}"/>
<xacro:wam1 prefix="${robot_name}" parent="${robot_name}_link_base" controller="${JointInterface}"/>
<xacro:wam2 prefix="${robot_name}" parent="${robot_name}_link_1" controller="${JointInterface}"/>
<xacro:wam3 prefix="${robot_name}" parent="${robot_name}_link_2" controller="${JointInterface}"/>
<xacro:wam4 prefix="${robot_name}" parent="${robot_name}_link_3" controller="${JointInterface}"/>
<xacro:wam5 prefix="${robot_name}" parent="${robot_name}_link_4" controller="${JointInterface}"/>
<xacro:wam6 prefix="${robot_name}" parent="${robot_name}_link_5" controller="${JointInterface}"/>
<xacro:wam7 prefix="${robot_name}" parent="${robot_name}_link_6" controller="${JointInterface}"/>
<xacro:wamtcp prefix="${robot_name}" parent="${robot_name}_link_7"/>
<xacro:gripper prefix="${robot_name}" parent="${robot_name}_link_tcp"/>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/${robot_name}</robotNamespace>
<!--<robotParam>/robot_description</robotParam>-->
<!--<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>-->
<!--<controlPeriod>0.002</controlPeriod>-->
<eStopTopic>e_stop</eStopTopic>
</plugin>
</gazebo>
</robot>
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment