Skip to content

Commit f1e4825

Browse files
author
Sammy Pfeiffer
committed
Merge pull request #1 from pal-robotics/proper_vector_managing
Proper vector managing
2 parents ef85182 + 81b1550 commit f1e4825

File tree

9 files changed

+583
-79
lines changed

9 files changed

+583
-79
lines changed

‎CMakeLists.txt‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,10 +12,9 @@ find_package(catkin REQUIRED COMPONENTS
1212
# Depend on system install of Gazebo
1313
find_package(gazebo REQUIRED)
1414

15-
## Generate messages in the 'msg' folder
16-
add_message_files(
15+
add_service_files(
1716
FILES
18-
Attach.msg
17+
Attach.srv
1918
)
2019

2120

‎README.md‎

Lines changed: 24 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -19,26 +19,45 @@ source devel/setup.bash
1919

2020
Empty world with the plugin `libgazebo_ros_link_attacher.so` loaded (in the *worlds* folder).
2121

22-
Which provides the `/link_attacher_node/attach_models` topic to specify two models and their links to be attached.
22+
Which provides the `/link_attacher_node/attach` service to specify two models and their links to be attached.
23+
24+
And `/link_attacher_node/detach` service to specify two models and their links to be detached.
2325

2426
![gazebo screenshot](ss.png)
2527

2628
# Run demo
2729

2830
In another shell, be sure to do `source devel/setup.bash` of your workspace.
2931

30-
rosrun gazebo_ros_link_attacher demo.py
32+
rosrun gazebo_ros_link_attacher spawn.py
33+
34+
Three cubes will be spawned.
35+
36+
rosrun gazebo_ros_link_attacher attach.py
37+
38+
The cubes will be attached all between themselves as (1,2), (2,3), (3,1). You can move them with the GUI and you'll see they will move together.
39+
40+
rosrun gazebo_ros_link_attacher detach.py
41+
42+
The cubes will be detached and you can move them separately again.
3143

32-
This demo will spawn two boxes and link them.
44+
You can also spawn items with the GUI and run a rosservice call:
45+
````
46+
rosservice call /link_attacher_node/attach "model_name_1: 'unit_box_1'
47+
link_name_1: 'link'
48+
model_name_2: 'unit_sphere_1'
49+
link_name_2: 'link'"
50+
````
3351

34-
You can also spawn two items with the GUI and run a rostopic pub:
52+
And same thing to detach:
3553
````
36-
rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'unit_box_1'
54+
rosservice call /link_attacher_node/detach "model_name_1: 'unit_box_1'
3755
link_name_1: 'link'
3856
model_name_2: 'unit_sphere_1'
3957
link_name_2: 'link'"
4058
````
4159

60+
4261
# Current status
4362
It works!
4463

‎include/gazebo_ros_link_attacher.h‎

Lines changed: 29 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -19,10 +19,9 @@
1919
#include <gazebo/common/Events.hh>
2020
#include "gazebo/msgs/msgs.hh"
2121
#include "gazebo/transport/transport.hh"
22-
23-
#include <std_msgs/String.h>
2422
#include "gazebo_ros_link_attacher/Attach.h"
25-
#include <geometry_msgs/Point.h>
23+
#include "gazebo_ros_link_attacher/AttachRequest.h"
24+
#include "gazebo_ros_link_attacher/AttachResponse.h"
2625

2726
namespace gazebo
2827
{
@@ -39,30 +38,40 @@ namespace gazebo
3938
/// \brief Load the controller
4039
void Load( physics::WorldPtr _world, sdf::ElementPtr /*_sdf*/ );
4140

42-
/// \brief Attach with a revolute joint link1 to link2
41+
/// \brief Attach with a revolute joint
4342
bool attach(std::string model1, std::string link1,
4443
std::string model2, std::string link2);
4544

46-
/// \brief Detach link1 from link2
47-
bool detach();
45+
/// \brief Detach
46+
bool detach(std::string model1, std::string link1,
47+
std::string model2, std::string link2);
4848

49-
50-
private:
49+
/// \brief Internal representation of a fixed joint
50+
struct fixedJoint{
51+
std::string model1;
52+
physics::ModelPtr m1;
53+
std::string link1;
54+
physics::LinkPtr l1;
55+
std::string model2;
56+
physics::ModelPtr m2;
57+
std::string link2;
58+
physics::LinkPtr l2;
59+
physics::JointPtr joint;
60+
};
61+
62+
bool getJoint(std::string model1, std::string link1, std::string model2, std::string link2, fixedJoint &joint);
63+
64+
private:
5165
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;
66+
ros::ServiceServer attach_service_;
67+
ros::ServiceServer detach_service_;
6068

61-
physics::LinkPtr link1;
62-
physics::LinkPtr link2;
69+
bool attach_callback(gazebo_ros_link_attacher::Attach::Request &req,
70+
gazebo_ros_link_attacher::Attach::Response &res);
71+
bool detach_callback(gazebo_ros_link_attacher::Attach::Request &req,
72+
gazebo_ros_link_attacher::Attach::Response &res);
6373

64-
/// \brief Model that contains this
65-
physics::ModelPtr model;
74+
std::vector<fixedJoint> joints;
6675

6776
/// \brief The physics engine.
6877
physics::PhysicsEnginePtr physics;

‎scripts/attach.py‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
#!/usr/bin/env python
2+
3+
import rospy
4+
from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse
5+
6+
7+
if __name__ == '__main__':
8+
rospy.init_node('demo_attach_links')
9+
rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach")
10+
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach',
11+
Attach)
12+
attach_srv.wait_for_service()
13+
rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach")
14+
15+
# Link them
16+
rospy.loginfo("Attaching cube1 and cube2")
17+
req = AttachRequest()
18+
req.model_name_1 = "cube1"
19+
req.link_name_1 = "link"
20+
req.model_name_2 = "cube2"
21+
req.link_name_2 = "link"
22+
23+
attach_srv.call(req)
24+
# From the shell:
25+
"""
26+
rosservice call /link_attacher_node/attach "model_name_1: 'cube1'
27+
link_name_1: 'link'
28+
model_name_2: 'cube2'
29+
link_name_2: 'link'"
30+
"""
31+
32+
rospy.loginfo("Attaching cube2 and cube3")
33+
req = AttachRequest()
34+
req.model_name_1 = "cube2"
35+
req.link_name_1 = "link"
36+
req.model_name_2 = "cube3"
37+
req.link_name_2 = "link"
38+
39+
attach_srv.call(req)
40+
41+
rospy.loginfo("Attaching cube3 and cube1")
42+
req = AttachRequest()
43+
req.model_name_1 = "cube3"
44+
req.link_name_1 = "link"
45+
req.model_name_2 = "cube1"
46+
req.link_name_2 = "link"
47+
48+
attach_srv.call(req)

‎scripts/demo_multiple.py‎

Lines changed: 183 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,183 @@
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+
# Spawn object 3
136+
rospy.loginfo("Spawning cube3")
137+
req3 = create_cube_request("cube3",
138+
0.0, 2.1, 0.41, # position
139+
0.0, 0.0, 0.0, # rotation
140+
0.4, 0.4, 0.4) # size
141+
spawn_srv.call(req3)
142+
rospy.sleep(1.0)
143+
144+
# Link them
145+
rospy.loginfo("Attaching cube1 and cube2")
146+
amsg = Attach()
147+
amsg.model_name_1 = "cube1"
148+
amsg.link_name_1 = "link"
149+
amsg.model_name_2 = "cube2"
150+
amsg.link_name_2 = "link"
151+
152+
attach_pub.publish(amsg)
153+
rospy.sleep(1.0)
154+
# From the shell:
155+
"""
156+
rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'cube1'
157+
link_name_1: 'link'
158+
model_name_2: 'cube2'
159+
link_name_2: 'link'"
160+
"""
161+
rospy.loginfo("Published into linking service!")
162+
163+
164+
rospy.loginfo("Attaching cube2 and cube3")
165+
amsg = Attach()
166+
amsg.model_name_1 = "cube2"
167+
amsg.link_name_1 = "link"
168+
amsg.model_name_2 = "cube3"
169+
amsg.link_name_2 = "link"
170+
171+
attach_pub.publish(amsg)
172+
rospy.sleep(1.0)
173+
174+
175+
rospy.loginfo("Attaching cube3 and cube1")
176+
amsg = Attach()
177+
amsg.model_name_1 = "cube3"
178+
amsg.link_name_1 = "link"
179+
amsg.model_name_2 = "cube1"
180+
amsg.link_name_2 = "link"
181+
182+
attach_pub.publish(amsg)
183+
rospy.sleep(2.0)

0 commit comments

Comments
 (0)