diff --git a/gz_black_hole/src/gz_black_hole.cpp b/gz_black_hole/src/gz_black_hole.cpp
index b871501327d9176e1f4192964c9cebacd2933dd8..e64e26c94a96ee38e203a7e2d158cfeb83b7312e 100644
--- a/gz_black_hole/src/gz_black_hole.cpp
+++ b/gz_black_hole/src/gz_black_hole.cpp
@@ -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());
+ }
}
}
}
diff --git a/gz_gripper_plugin/CMakeLists.txt b/gz_gripper_plugin/CMakeLists.txt
index 5dff16c70916f87b0cead65c435b904fd6c6b3d6..602ddcdf4b0b09d9a4184151fafcd5de330b091d 100644
--- a/gz_gripper_plugin/CMakeLists.txt
+++ b/gz_gripper_plugin/CMakeLists.txt
@@ -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
diff --git a/gz_gripper_plugin/launch/iri_wam.launch b/gz_gripper_plugin/launch/iri_wam.launch
new file mode 100644
index 0000000000000000000000000000000000000000..3b6b8f83fc4fd332a0a826c2c07f48161e55be0c
--- /dev/null
+++ b/gz_gripper_plugin/launch/iri_wam.launch
@@ -0,0 +1,54 @@
+<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>
+
diff --git a/gz_gripper_plugin/package.xml b/gz_gripper_plugin/package.xml
index 71430c2fa32d46ed8afe0b4b4a9f00e31b62731c..3ef5438309997113aff843a8792335bf3d509c4a 100644
--- a/gz_gripper_plugin/package.xml
+++ b/gz_gripper_plugin/package.xml
@@ -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>
diff --git a/gz_gripper_plugin/src/gz_gripper_plugin.cpp b/gz_gripper_plugin/src/gz_gripper_plugin.cpp
index 5d6217ccba5308375a4f225f860db47e404aa76d..87717591f6859be0881612299b82fb527924545d 100644
--- a/gz_gripper_plugin/src/gz_gripper_plugin.cpp
+++ b/gz_gripper_plugin/src/gz_gripper_plugin.cpp
@@ -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();
diff --git a/gz_gripper_plugin/srv/CheckGrasped.srv b/gz_gripper_plugin/srv/CheckGrasped.srv
index 6bfb6d0a17033cc9b5103e281b9fcb95eb61527b..d76d9d2f6b31d73eeeb02191c93aba26118512cf 100644
--- a/gz_gripper_plugin/srv/CheckGrasped.srv
+++ b/gz_gripper_plugin/srv/CheckGrasped.srv
@@ -1,2 +1,2 @@
---
-std_msgs/String grasped
+string grasped
diff --git a/gz_gripper_plugin/xacro/gripper.urdf.xacro b/gz_gripper_plugin/xacro/gripper.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..7d7ea961b367c3c23cad18ed5b712a111bba49d3
--- /dev/null
+++ b/gz_gripper_plugin/xacro/gripper.urdf.xacro
@@ -0,0 +1,44 @@
+<?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>
+
diff --git a/gz_gripper_plugin/xacro/iri_wam.urdf.xacro b/gz_gripper_plugin/xacro/iri_wam.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..9741f681a4f9d9008c95d797f150bc6ba7fbb14c
--- /dev/null
+++ b/gz_gripper_plugin/xacro/iri_wam.urdf.xacro
@@ -0,0 +1,59 @@
+<?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>
+