Skip to content

Commit 77a80df

Browse files
awesomebytesawesomebytes
committed
Initial commit
0 parents  commit 77a80df

File tree

10 files changed

+521
-0
lines changed

10 files changed

+521
-0
lines changed

‎CMakeLists.txt‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(gazebo_ros_link_attacher)
3+
4+
# Load catkin and all dependencies required for this package
5+
find_package(catkin REQUIRED COMPONENTS
6+
roscpp
7+
gazebo_ros
8+
std_msgs
9+
message_runtime
10+
message_generation
11+
)
12+
13+
# Depend on system install of Gazebo
14+
find_package(gazebo REQUIRED)
15+
16+
## Generate messages in the 'msg' folder
17+
add_message_files(
18+
FILES
19+
Attach.msg
20+
)
21+
22+
23+
## Generate added messages and services with any dependencies listed here
24+
generate_messages(
25+
DEPENDENCIES
26+
std_msgs
27+
)
28+
29+
###################################
30+
## catkin specific configuration ##
31+
###################################
32+
catkin_package(CATKIN_DEPENDS message_runtime)
33+
34+
###########
35+
## Build ##
36+
###########
37+
38+
## Specify additional locations of header files
39+
## Your package locations should be listed before other locations
40+
include_directories(include)
41+
link_directories(${GAZEBO_LIBRARY_DIRS})
42+
include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
43+
44+
## Declare a cpp library
45+
add_library(${PROJECT_NAME} src/gazebo_ros_link_attacher.cpp)
46+
47+
48+
## Specify libraries to link a library or executable target against
49+
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
50+
51+
52+
#############
53+
## Install ##
54+
#############
55+
56+
install(PROGRAMS
57+
scripts/demo.py
58+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
59+
)
60+
61+
## Mark executables and/or libraries for installation
62+
install(TARGETS ${PROJECT_NAME}
63+
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
64+
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
65+
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
66+
)
67+
68+
69+
foreach (dir launch)
70+
install(DIRECTORY ${dir}/
71+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
72+
endforeach(dir)

‎README.md‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
Attach two Gazebo models with a virtual joint in a generalized grasp hack
2+
3+
# Launch
4+
5+
roslaunch gazebo_ros_link_attacher test_attacher.launch
6+
7+
Empty world with the plugin `libgazebo_ros_link_attacher.so` loaded.
8+
9+
Which provides the `/link_attacher_node/attach_models` topic to specify two models and their links to be attached.
10+
11+
![gazebo screenshot](ss.png)
12+
13+
# Run demo
14+
15+
rosrun gazebo_ros_link_attacher demo.py
16+
17+
This demo will spawn two boxes and link them.
18+
19+
You can also spawn two items with the GUI and run a rostopic pub:
20+
````
21+
rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'unit_box_1'
22+
link_name_1: 'link'
23+
model_name_2: 'unit_sphere_1'
24+
link_name_2: 'link'"
25+
````
26+
27+
# Current status
28+
29+
Currently it crashes with:
30+
31+
````
32+
***** Internal Program Error - assertion (self->inertial != __null) failed in static void gazebo::physics::ODELink::MoveCallback(dBodyID):
33+
/tmp/buildd/gazebo4-4.1.3/gazebo/physics/ode/ODELink.cc(178): Inertial pointer is NULL
34+
Aborted (core dumped)
35+
````
36+
37+
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.
38+
39+
40+
The method to attach the links is based on the grasp hack of the Gripper in gazebo/physics:
41+
[Gripper.hh](https://bitbucket.org/osrf/gazebo/src/1d1e3a542af81670f43a120e1df7190592bc4c0f/gazebo/physics/Gripper.hh?at=default&fileviewer=file-view-default)
42+
[Gripper.cc](https://bitbucket.org/osrf/gazebo/src/1d1e3a542af81670f43a120e1df7190592bc4c0f/gazebo/physics/Gripper.cc?at=default&fileviewer=file-view-default)
43+
44+
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‎

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
/*
2+
* Desc: Gazebo link attacher plugin.
3+
* Author: Sammy Pfeiffer ([email protected])
4+
* Date: 05/04/2016
5+
*/
6+
7+
#ifndef GAZEBO_ROS_LINK_ATTACHER_HH
8+
#define GAZEBO_ROS_LINK_ATTACHER_HH
9+
10+
#include <ros/ros.h>
11+
12+
#include <sdf/sdf.hh>
13+
#include "gazebo/gazebo.hh"
14+
#include <gazebo/physics/physics.hh>
15+
#include "gazebo/physics/PhysicsTypes.hh"
16+
#include <gazebo/transport/TransportTypes.hh>
17+
#include <gazebo/common/Time.hh>
18+
#include <gazebo/common/Plugin.hh>
19+
#include <gazebo/common/Events.hh>
20+
#include "gazebo/msgs/msgs.hh"
21+
#include "gazebo/transport/transport.hh"
22+
23+
#include <std_msgs/String.h>
24+
#include "gazebo_ros_link_attacher/Attach.h"
25+
#include <geometry_msgs/Point.h>
26+
27+
namespace gazebo
28+
{
29+
30+
class GazeboRosLinkAttacher : public WorldPlugin
31+
{
32+
public:
33+
/// \brief Constructor
34+
GazeboRosLinkAttacher();
35+
36+
/// \brief Destructor
37+
virtual ~GazeboRosLinkAttacher();
38+
39+
/// \brief Load the controller
40+
void Load( physics::WorldPtr _world, sdf::ElementPtr /*_sdf*/ );
41+
42+
/// \brief Attach with a revolute joint link1 to link2
43+
bool attach(std::string model1, std::string link1,
44+
std::string model2, std::string link2);
45+
46+
/// \brief Detach link1 from link2
47+
bool detach();
48+
49+
50+
private:
51+
ros::NodeHandle nh_;
52+
ros::Subscriber attach_by_name_subscriber_;
53+
ros::Subscriber detach_subscriber_;
54+
55+
void attach_callback(const gazebo_ros_link_attacher::Attach msg);
56+
void detach_callback(const std_msgs::String msg);
57+
58+
bool attached;
59+
physics::JointPtr fixedJoint;
60+
61+
physics::LinkPtr link1;
62+
physics::LinkPtr link2;
63+
64+
/// \brief Model that contains this
65+
physics::ModelPtr model;
66+
67+
/// \brief The physics engine.
68+
physics::PhysicsEnginePtr physics;
69+
70+
/// \brief Pointer to the world.
71+
physics::WorldPtr world;
72+
73+
};
74+
75+
}
76+
77+
#endif
78+

‎launch/test_attacher.launch‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<launch>
2+
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
3+
<include file="$(find gazebo_ros)/launch/empty_world.launch">
4+
<arg name="world_name" value="$(find gazebo_ros_link_attacher)/worlds/test_attacher.world"/>
5+
<arg name="paused" value="false"/>
6+
<!-- more default parameters can be changed here -->
7+
</include>
8+
9+
</launch>

‎msg/Attach.msg‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
string model_name_1
2+
string link_name_1
3+
string model_name_2
4+
string link_name_2

‎package.xml‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<?xml version="1.0"?>
2+
<package>
3+
<name>gazebo_ros_link_attacher</name>
4+
<version>0.0.0</version>
5+
<description>The gazebo_ros_link_attacher package lets you attach
6+
by a virtual joint temporarily two links in a Gazebo simulation.</description>
7+
8+
<maintainer email="[email protected]">Sam Pfeiffer</maintainer>
9+
10+
11+
<license>BSD</license>
12+
13+
14+
<build_depend>message_generation</build_depend>
15+
<build_depend>roscpp</build_depend>
16+
<build_depend>gazebo_ros</build_depend>
17+
<build_depend>gazebo</build_depend>
18+
<build_depend>std_msgs</build_depend>
19+
20+
<run_depend>message_runtime</run_depend>
21+
<run_depend>roscpp</run_depend>
22+
<run_depend>gazebo_ros</run_depend>
23+
<run_depend>std_msgs</run_depend>
24+
25+
<buildtool_depend>catkin</buildtool_depend>
26+
27+
28+
</package>

‎scripts/demo.py‎

Lines changed: 151 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,151 @@
1+
#!/usr/bin/env python
2+
3+
import rospy
4+
from gazebo_ros_link_attacher.msg import Attach
5+
from gazebo_msgs.srv import SpawnModel, SpawnModelRequest, SpawnModelResponse
6+
from copy import deepcopy
7+
from tf.transformations import quaternion_from_euler
8+
9+
sdf_cube = """<?xml version="1.0" ?>
10+
<sdf version="1.4">
11+
<model name="MODELNAME">
12+
<static>0</static>
13+
<link name="link">
14+
<inertial>
15+
<mass>1.0</mass>
16+
<inertia>
17+
<ixx>0.01</ixx>
18+
<ixy>0.0</ixy>
19+
<ixz>0.0</ixz>
20+
<iyy>0.01</iyy>
21+
<iyz>0.0</iyz>
22+
<izz>0.01</izz>
23+
</inertia>
24+
</inertial>
25+
<collision name="stairs_collision0">
26+
<pose>0 0 0 0 0 0</pose>
27+
<geometry>
28+
<box>
29+
<size>SIZEXYZ</size>
30+
</box>
31+
</geometry>
32+
<surface>
33+
<bounce />
34+
<friction>
35+
<ode>
36+
<mu>1.0</mu>
37+
<mu2>1.0</mu2>
38+
</ode>
39+
</friction>
40+
<contact>
41+
<ode>
42+
<kp>10000000.0</kp>
43+
<kd>1.0</kd>
44+
<min_depth>0.0</min_depth>
45+
<max_vel>0.0</max_vel>
46+
</ode>
47+
</contact>
48+
</surface>
49+
</collision>
50+
<visual name="stairs_visual0">
51+
<pose>0 0 0 0 0 0</pose>
52+
<geometry>
53+
<box>
54+
<size>SIZEXYZ</size>
55+
</box>
56+
</geometry>
57+
<material>
58+
<script>
59+
<uri>file://media/materials/scripts/gazebo.material</uri>
60+
<name>Gazebo/Wood</name>
61+
</script>
62+
</material>
63+
</visual>
64+
<velocity_decay>
65+
<linear>0.000000</linear>
66+
<angular>0.000000</angular>
67+
</velocity_decay>
68+
<self_collide>0</self_collide>
69+
<kinematic>0</kinematic>
70+
<gravity>1</gravity>
71+
</link>
72+
</model>
73+
</sdf>
74+
"""
75+
76+
77+
def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz):
78+
"""Create a SpawnModelRequest with the parameters of the cube given.
79+
modelname: name of the model for gazebo
80+
px py pz: position of the cube (and it's collision cube)
81+
rr rp ry: rotation (roll, pitch, yaw) of the model
82+
sx sy sz: size of the cube"""
83+
cube = deepcopy(sdf_cube)
84+
# Replace size of model
85+
size_str = str(round(sx, 3)) + " " + \
86+
str(round(sy, 3)) + " " + str(round(sz, 3))
87+
cube = cube.replace('SIZEXYZ', size_str)
88+
# Replace modelname
89+
cube = cube.replace('MODELNAME', str(modelname))
90+
91+
req = SpawnModelRequest()
92+
req.model_name = modelname
93+
req.model_xml = cube
94+
req.initial_pose.position.x = px
95+
req.initial_pose.position.y = py
96+
req.initial_pose.position.z = pz
97+
98+
q = quaternion_from_euler(rr, rp, ry)
99+
req.initial_pose.orientation.x = q[0]
100+
req.initial_pose.orientation.y = q[1]
101+
req.initial_pose.orientation.z = q[2]
102+
req.initial_pose.orientation.w = q[3]
103+
104+
return req
105+
106+
107+
if __name__ == '__main__':
108+
rospy.init_node('demo_attach_links')
109+
attach_pub = rospy.Publisher('/link_attacher_node/attach_models',
110+
Attach, queue_size=1)
111+
rospy.loginfo("Created publisher to /link_attacher_node/attach_models")
112+
spawn_srv = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
113+
rospy.loginfo("Waiting for /gazebo/spawn_sdf_model service...")
114+
spawn_srv.wait_for_service()
115+
rospy.loginfo("Connected to service!")
116+
117+
# Spawn object 1
118+
rospy.loginfo("Spawning cube1")
119+
req1 = create_cube_request("cube1",
120+
0.0, 0.0, 0.51, # position
121+
0.0, 0.0, 0.0, # rotation
122+
1.0, 1.0, 1.0) # size
123+
spawn_srv.call(req1)
124+
rospy.sleep(1.0)
125+
126+
# Spawn object 2
127+
rospy.loginfo("Spawning cube2")
128+
req2 = create_cube_request("cube2",
129+
0.0, 1.1, 0.41, # position
130+
0.0, 0.0, 0.0, # rotation
131+
0.8, 0.8, 0.8) # size
132+
spawn_srv.call(req2)
133+
rospy.sleep(1.0)
134+
135+
# Link them
136+
rospy.loginfo("Attaching cube1 and cube2")
137+
amsg = Attach()
138+
amsg.model_name_1 = "cube1"
139+
amsg.link_name_1 = "link"
140+
amsg.model_name_2 = "cube2"
141+
amsg.link_name_2 = "link"
142+
143+
attach_pub.publish(amsg)
144+
# From the shell:
145+
"""
146+
rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'cube1'
147+
link_name_1: 'link'
148+
model_name_2: 'cube2'
149+
link_name_2: 'link'"
150+
"""
151+
rospy.loginfo("Published into linking service!")

0 commit comments

Comments
 (0)