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(
"simple_obstacle_pub")
28 root_frame =
"/odom_combined" 30 pub = rospy.Publisher(
"obstacle_distance/registerObstacle", CollisionObject, queue_size = 1)
32 while pub.get_num_connections() < 1:
33 if rospy.is_shutdown():
35 rospy.logwarn(
"Please create a subscriber to 'obstacle_distance/registerObstacle' topic (Type: moveit_msgs/CollisionObject)")
38 rospy.loginfo(
"Continue ...")
43 x.header.frame_id = root_frame
44 x.operation = CollisionObject.ADD
46 sphere = SolidPrimitive()
47 sphere.type = SolidPrimitive.SPHERE
48 sphere.dimensions.append(0.1)
49 x.primitives.append(sphere)
52 pose.position.x = 0.35
53 pose.position.y = -0.45
55 pose.orientation.x = 0.0;
56 pose.orientation.y = 0.0;
57 pose.orientation.z = 0.0;
58 pose.orientation.w = 1.0;
59 x.primitive_poses.append(pose)
66 y.header.frame_id = root_frame
68 y.type.db =
"package://cob_gazebo_objects/Media/models/cabinet_ikea_kallax_4x2.stl" 70 y.operation = CollisionObject.ADD
77 pose.orientation.x = 0.0
78 pose.orientation.y = 0.0
79 pose.orientation.z = 0.0
80 pose.orientation.w = 1.0
81 y.mesh_poses.append(pose)
85 rospy.loginfo(
"Done!")