Initial commit

CMakeLists.txt

@@ -0,0 +1,72 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gazebo_ros_link_attacher)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS 
+  roscpp 
+  gazebo_ros
+  std_msgs
+  message_runtime
+  message_generation
+)
+
+# Depend on system install of Gazebo
+find_package(gazebo REQUIRED)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  Attach.msg
+)
+
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(CATKIN_DEPENDS message_runtime)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include)
+link_directories(${GAZEBO_LIBRARY_DIRS})
+include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
+
+## Declare a cpp library
+add_library(${PROJECT_NAME} src/gazebo_ros_link_attacher.cpp)
+
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+
+
+#############
+## Install ##
+#############
+
+install(PROGRAMS
+  scripts/demo.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark executables and/or libraries for installation
+install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+
+foreach (dir launch)
+  install(DIRECTORY ${dir}/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)

README.md

@@ -0,0 +1,44 @@
+Attach two Gazebo models with a virtual joint in a generalized grasp hack
+
+# Launch
+
+    roslaunch gazebo_ros_link_attacher test_attacher.launch
+
+Empty world with the plugin `libgazebo_ros_link_attacher.so` loaded.
+
+Which provides the `/link_attacher_node/attach_models` topic to specify two models and their links to be attached.
+
+![gazebo screenshot](ss.png)
+
+# Run demo
+
+    rosrun gazebo_ros_link_attacher demo.py
+
+This demo will spawn two boxes and link them.
+
+You can also spawn two items with the GUI and run a rostopic pub:
+````
+rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'unit_box_1'
+link_name_1: 'link'
+model_name_2: 'unit_sphere_1'
+link_name_2: 'link'"
+````
+
+# Current status
+
+Currently it crashes with:
+
+````
+***** Internal Program Error - assertion (self->inertial != __null) failed in static void gazebo::physics::ODELink::MoveCallback(dBodyID):
+/tmp/buildd/gazebo4-4.1.3/gazebo/physics/ode/ODELink.cc(178): Inertial pointer is NULL
+Aborted (core dumped)
+````
+
+Which I've only seen this other useful information: [Bitbucket gazebo removing moving model with ode friction fails](https://bitbucket.org/osrf/gazebo/issues/1177/removing-moving-model-with-ode-friction). But it didn't help me solve my crash. I guess when attaching one model to the other it removes the second one to re-create it attached to the first or something like that.
+
+
+The method to attach the links is based on the grasp hack of the Gripper in gazebo/physics:
+[Gripper.hh](https://bitbucket.org/osrf/gazebo/src/1d1e3a542af81670f43a120e1df7190592bc4c0f/gazebo/physics/Gripper.hh?at=default&fileviewer=file-view-default)
+[Gripper.cc](https://bitbucket.org/osrf/gazebo/src/1d1e3a542af81670f43a120e1df7190592bc4c0f/gazebo/physics/Gripper.cc?at=default&fileviewer=file-view-default)
+
+Which is to create a revolute joint on the first model (with range of motion 0) linking a link on the first model and a link on the second model.

include/gazebo_ros_link_attacher.h

@@ -0,0 +1,78 @@
+/*
+ * Desc: Gazebo link attacher plugin.
+ * Author: Sammy Pfeiffer ([email protected])
+ * Date: 05/04/2016
+ */
+
+#ifndef GAZEBO_ROS_LINK_ATTACHER_HH
+#define GAZEBO_ROS_LINK_ATTACHER_HH
+
+#include <ros/ros.h>
+
+#include <sdf/sdf.hh>
+#include "gazebo/gazebo.hh"
+#include <gazebo/physics/physics.hh>
+#include "gazebo/physics/PhysicsTypes.hh"
+#include <gazebo/transport/TransportTypes.hh>
+#include <gazebo/common/Time.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/common/Events.hh>
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/transport/transport.hh"
+
+#include <std_msgs/String.h>
+#include "gazebo_ros_link_attacher/Attach.h"
+#include <geometry_msgs/Point.h>
+
+namespace gazebo
+{
+
+   class GazeboRosLinkAttacher : public WorldPlugin
+   {
+      public:
+        /// \brief Constructor
+        GazeboRosLinkAttacher();
+
+        /// \brief Destructor
+        virtual ~GazeboRosLinkAttacher();
+
+        /// \brief Load the controller
+        void Load( physics::WorldPtr _world, sdf::ElementPtr /*_sdf*/ );
+
+        /// \brief Attach with a revolute joint link1 to link2
+        bool attach(std::string model1, std::string link1,
+                    std::string model2, std::string link2);
+
+        /// \brief Detach link1 from link2
+        bool detach();
+
+
+      private:
+        ros::NodeHandle nh_;
+        ros::Subscriber attach_by_name_subscriber_;
+        ros::Subscriber detach_subscriber_;
+
+        void attach_callback(const gazebo_ros_link_attacher::Attach msg);
+        void detach_callback(const std_msgs::String msg);
+
+        bool attached;
+        physics::JointPtr fixedJoint;
+
+        physics::LinkPtr link1;
+        physics::LinkPtr link2;
+
+        /// \brief Model that contains this
+        physics::ModelPtr model;
+
+        /// \brief The physics engine.
+        physics::PhysicsEnginePtr physics;
+
+        /// \brief Pointer to the world.
+        physics::WorldPtr world;
+
+   };
+
+}
+
+#endif
+

launch/test_attacher.launch

@@ -0,0 +1,9 @@
+<launch>
+  <!-- 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 gazebo_ros_link_attacher)/worlds/test_attacher.world"/>
+    <arg name="paused" value="false"/>
+    <!-- more default parameters can be changed here -->
+  </include>
+
+</launch>

msg/Attach.msg

@@ -0,0 +1,4 @@
+string model_name_1
+string link_name_1
+string model_name_2
+string link_name_2

package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package>
+  <name>gazebo_ros_link_attacher</name>
+  <version>0.0.0</version>
+  <description>The gazebo_ros_link_attacher package lets you attach
+    by a virtual joint temporarily two links in a Gazebo simulation.</description>
+
+  <maintainer email="[email protected]">Sam Pfeiffer</maintainer>
+
+
+  <license>BSD</license>
+
+
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>gazebo_ros</build_depend>
+  <build_depend>gazebo</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+</package>

scripts/demo.py

@@ -0,0 +1,151 @@
+#!/usr/bin/env python
+
+import rospy
+from gazebo_ros_link_attacher.msg import Attach
+from gazebo_msgs.srv import SpawnModel, SpawnModelRequest, SpawnModelResponse
+from copy import deepcopy
+from tf.transformations import quaternion_from_euler
+
+sdf_cube = """<?xml version="1.0" ?>
+<sdf version="1.4">
+  <model name="MODELNAME">
+    <static>0</static>
+    <link name="link">
+      <inertial>
+        <mass>1.0</mass>
+        <inertia>
+          <ixx>0.01</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.01</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.01</izz>
+        </inertia>
+      </inertial>
+      <collision name="stairs_collision0">
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>SIZEXYZ</size>
+          </box>
+        </geometry>
+        <surface>
+          <bounce />
+          <friction>
+            <ode>
+              <mu>1.0</mu>
+              <mu2>1.0</mu2>
+            </ode>
+          </friction>
+          <contact>
+            <ode>
+              <kp>10000000.0</kp>
+              <kd>1.0</kd>
+              <min_depth>0.0</min_depth>
+              <max_vel>0.0</max_vel>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+      <visual name="stairs_visual0">
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>SIZEXYZ</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Wood</name>
+          </script>
+        </material>
+      </visual>
+      <velocity_decay>
+        <linear>0.000000</linear>
+        <angular>0.000000</angular>
+      </velocity_decay>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <gravity>1</gravity>
+    </link>
+  </model>
+</sdf>
+"""
+
+
+def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz):
+    """Create a SpawnModelRequest with the parameters of the cube given.
+    modelname: name of the model for gazebo
+    px py pz: position of the cube (and it's collision cube)
+    rr rp ry: rotation (roll, pitch, yaw) of the model
+    sx sy sz: size of the cube"""
+    cube = deepcopy(sdf_cube)
+    # Replace size of model
+    size_str = str(round(sx, 3)) + " " + \
+        str(round(sy, 3)) + " " + str(round(sz, 3))
+    cube = cube.replace('SIZEXYZ', size_str)
+    # Replace modelname
+    cube = cube.replace('MODELNAME', str(modelname))
+
+    req = SpawnModelRequest()
+    req.model_name = modelname
+    req.model_xml = cube
+    req.initial_pose.position.x = px
+    req.initial_pose.position.y = py
+    req.initial_pose.position.z = pz
+
+    q = quaternion_from_euler(rr, rp, ry)
+    req.initial_pose.orientation.x = q[0]
+    req.initial_pose.orientation.y = q[1]
+    req.initial_pose.orientation.z = q[2]
+    req.initial_pose.orientation.w = q[3]
+
+    return req
+
+
+if __name__ == '__main__':
+    rospy.init_node('demo_attach_links')
+    attach_pub = rospy.Publisher('/link_attacher_node/attach_models',
+                                 Attach, queue_size=1)
+    rospy.loginfo("Created publisher to /link_attacher_node/attach_models")
+    spawn_srv = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
+    rospy.loginfo("Waiting for /gazebo/spawn_sdf_model service...")
+    spawn_srv.wait_for_service()
+    rospy.loginfo("Connected to service!")
+
+    # Spawn object 1
+    rospy.loginfo("Spawning cube1")
+    req1 = create_cube_request("cube1",
+                              0.0, 0.0, 0.51,  # position
+                              0.0, 0.0, 0.0,  # rotation
+                              1.0, 1.0, 1.0)  # size
+    spawn_srv.call(req1)
+    rospy.sleep(1.0)
+
+    # Spawn object 2
+    rospy.loginfo("Spawning cube2")
+    req2 = create_cube_request("cube2",
+                              0.0, 1.1, 0.41,  # position
+                              0.0, 0.0, 0.0,  # rotation
+                              0.8, 0.8, 0.8)  # size
+    spawn_srv.call(req2)
+    rospy.sleep(1.0)
+
+    # Link them
+    rospy.loginfo("Attaching cube1 and cube2")
+    amsg = Attach()
+    amsg.model_name_1 = "cube1"
+    amsg.link_name_1 = "link"
+    amsg.model_name_2 = "cube2"
+    amsg.link_name_2 = "link"
+
+    attach_pub.publish(amsg)
+    # From the shell:
+    """
+rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'cube1'
+link_name_1: 'link'
+model_name_2: 'cube2'
+link_name_2: 'link'"
+    """
+    rospy.loginfo("Published into linking service!")