21 from geometry_msgs.msg
import Pose
22 from moveit_msgs.msg
import CollisionObject
23 from shape_msgs.msg
import SolidPrimitive, Mesh
26 if __name__==
"__main__":
27 rospy.init_node(
"example_obstacle_publisher")
30 frame_id = rospy.get_param(
'root_frame')
32 pub = rospy.Publisher(
"obstacle_distance/registerObstacle", CollisionObject, queue_size=1, latch=
True)
34 while pub.get_num_connections() < 1:
35 if rospy.is_shutdown():
37 rospy.logwarn(
"Please create a subscriber to '" + rospy.get_namespace() +
"/obstacle_distance/registerObstacle' topic (Type: moveit_msgs/CollisionObject)")
40 rospy.loginfo(
"Continue ...")
45 x.header.frame_id = frame_id
46 x.operation = CollisionObject.ADD
47 sphere = SolidPrimitive()
48 sphere.type = SolidPrimitive.SPHERE
49 sphere.dimensions.append(0.1)
50 x.primitives.append(sphere)
53 pose.position.x = 0.35
54 pose.position.y = -0.35
56 pose.orientation.x = 0.0;
57 pose.orientation.y = 0.0;
58 pose.orientation.z = 0.0;
59 pose.orientation.w = 1.0;
60 x.primitive_poses.append(pose)
67 y.header.frame_id = frame_id
68 y.type.db =
"package://cob_gazebo_objects/Media/models/milk.dae" 69 y.operation = CollisionObject.ADD
72 pose.position.x = -0.35
73 pose.position.y = -0.35
75 pose.orientation.x = 0.0;
76 pose.orientation.y = 0.0;
77 pose.orientation.z = 0.0;
78 pose.orientation.w = 1.0;
79 y.mesh_poses.append(pose)