Updated testing scripts to service

scripts/attach.py

@@ -1,53 +1,48 @@
 #!/usr/bin/env python
 
 import rospy
-from gazebo_ros_link_attacher.msg import Attach
+from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse
 
 
 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")
-    rospy.sleep(1.0)
+    rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach")
+    attach_srv = rospy.ServiceProxy('/link_attacher_node/attach',
+                                    Attach)
+    attach_srv.wait_for_service()
+    rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach")
 
     # 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)
-    rospy.sleep(1.0)
+    req = AttachRequest()
+    req.model_name_1 = "cube1"
+    req.link_name_1 = "link"
+    req.model_name_2 = "cube2"
+    req.link_name_2 = "link"
+
+    attach_srv.call(req)
     # From the shell:
     """
-rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'cube1'
+rosservice call /link_attacher_node/attach "model_name_1: 'cube1'
 link_name_1: 'link'
 model_name_2: 'cube2'
 link_name_2: 'link'"
     """
-    rospy.loginfo("Published into linking service!")
-
 
     rospy.loginfo("Attaching cube2 and cube3")
-    amsg = Attach()
-    amsg.model_name_1 = "cube2"
-    amsg.link_name_1 = "link"
-    amsg.model_name_2 = "cube3"
-    amsg.link_name_2 = "link"
-
-    attach_pub.publish(amsg)
-    rospy.sleep(1.0)
+    req = AttachRequest()
+    req.model_name_1 = "cube2"
+    req.link_name_1 = "link"
+    req.model_name_2 = "cube3"
+    req.link_name_2 = "link"
 
+    attach_srv.call(req)
 
     rospy.loginfo("Attaching cube3 and cube1")
-    amsg = Attach()
-    amsg.model_name_1 = "cube3"
-    amsg.link_name_1 = "link"
-    amsg.model_name_2 = "cube1"
-    amsg.link_name_2 = "link"
-
-    attach_pub.publish(amsg)
-    rospy.sleep(2.0)
+    req = AttachRequest()
+    req.model_name_1 = "cube3"
+    req.link_name_1 = "link"
+    req.model_name_2 = "cube1"
+    req.link_name_2 = "link"
+
+    attach_srv.call(req)

scripts/detach.py

@@ -1,52 +1,48 @@
 #!/usr/bin/env python
 
 import rospy
-from gazebo_ros_link_attacher.msg import Attach
+from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse
+
 
 if __name__ == '__main__':
-    rospy.init_node('demo_attach_links')
-    detach_pub = rospy.Publisher('/link_attacher_node/detach',
-                                 Attach, queue_size=1)
-    rospy.loginfo("Created publisher to /link_attacher_node/detach")
-    rospy.sleep(1.0)
+    rospy.init_node('demo_detach_links')
+    rospy.loginfo("Creating ServiceProxy to /link_attacher_node/detach")
+    attach_srv = rospy.ServiceProxy('/link_attacher_node/detach',
+                                    Attach)
+    attach_srv.wait_for_service()
+    rospy.loginfo("Created ServiceProxy to /link_attacher_node/detach")
 
     # Link them
     rospy.loginfo("Detaching 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"
-
-    detach_pub.publish(amsg)
-    rospy.sleep(1.0)
+    req = AttachRequest()
+    req.model_name_1 = "cube1"
+    req.link_name_1 = "link"
+    req.model_name_2 = "cube2"
+    req.link_name_2 = "link"
+
+    attach_srv.call(req)
     # From the shell:
     """
-rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'cube1'
+rosservice call /link_attacher_node/detach "model_name_1: 'cube1'
 link_name_1: 'link'
 model_name_2: 'cube2'
 link_name_2: 'link'"
     """
-    rospy.loginfo("Published into linking service!")
-
-
-    rospy.loginfo("Detaching cube2 and cube3")
-    amsg = Attach()
-    amsg.model_name_1 = "cube2"
-    amsg.link_name_1 = "link"
-    amsg.model_name_2 = "cube3"
-    amsg.link_name_2 = "link"
 
-    detach_pub.publish(amsg)
-    rospy.sleep(1.0)
+    rospy.loginfo("Attaching cube2 and cube3")
+    req = AttachRequest()
+    req.model_name_1 = "cube2"
+    req.link_name_1 = "link"
+    req.model_name_2 = "cube3"
+    req.link_name_2 = "link"
 
+    attach_srv.call(req)
 
-    rospy.loginfo("Detaching cube3 and cube1")
-    amsg = Attach()
-    amsg.model_name_1 = "cube3"
-    amsg.link_name_1 = "link"
-    amsg.model_name_2 = "cube1"
-    amsg.link_name_2 = "link"
+    rospy.loginfo("Attaching cube3 and cube1")
+    req = AttachRequest()
+    req.model_name_1 = "cube3"
+    req.link_name_1 = "link"
+    req.model_name_2 = "cube1"
+    req.link_name_2 = "link"
 
-    detach_pub.publish(amsg)
-    rospy.sleep(2.0)
+    attach_srv.call(req)

scripts/spawn_models.py

@@ -1,7 +1,6 @@
 #!/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