20 from copy
import deepcopy
25 from geometry_msgs.msg
import Pose, PoseStamped
26 from moveit_msgs.msg
import CollisionObject
27 from shape_msgs.msg
import SolidPrimitive
28 from visualization_msgs.msg
import Marker, InteractiveMarker, InteractiveMarkerControl
39 self.
pub = rospy.Publisher(
"obstacle_distance/registerObstacle", CollisionObject, queue_size=1, latch=
True)
42 while self.pub.get_num_connections() < 1:
43 rospy.logwarn(
"Please create a subscriber to '" + rospy.get_namespace() +
"/obstacle_distance/registerObstacle' topic (Type: moveit_msgs/CollisionObject)")
46 rospy.loginfo(
"Continue ...")
49 self.
co = CollisionObject()
50 self.co.id =
"Interactive Box" 52 self.co.operation = CollisionObject.ADD
54 primitive = SolidPrimitive()
55 primitive.type = SolidPrimitive.SPHERE
56 primitive.dimensions = [0.1]
57 self.co.primitives.append(primitive)
60 pose.orientation.w = 1.0;
61 self.co.primitive_poses.append(pose)
65 self.interactive_obstacle_pose.header.stamp = rospy.Time.now()
66 self.interactive_obstacle_pose.header.frame_id = self.
root_frame 68 self.interactive_obstacle_pose.pose.position.x = -0.5
69 self.interactive_obstacle_pose.pose.position.y = 0.3
70 self.interactive_obstacle_pose.pose.position.z = 0.8
71 self.interactive_obstacle_pose.pose.orientation.w = 1.0
75 self.int_marker.header.frame_id = self.
root_frame 76 self.int_marker.pose = self.interactive_obstacle_pose.pose
77 self.int_marker.name =
"interactive_obstacle_marker" 78 self.int_marker.scale = 0.5
81 control_3d = InteractiveMarkerControl()
82 control_3d.always_visible =
True 83 control_3d.name =
"move_rotate_3D" 84 control_3d.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
85 self.int_marker.controls.append(control_3d)
87 control = InteractiveMarkerControl()
88 control.always_visible =
True 89 control.orientation.w = 1
90 control.orientation.x = 1
91 control.orientation.y = 0
92 control.orientation.z = 0
93 control.name =
"move_x" 94 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
95 self.int_marker.controls.append(deepcopy(control))
96 control.name =
"move_y" 97 control.orientation.x = 0
98 control.orientation.y = 1
99 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
100 self.int_marker.controls.append(deepcopy(control))
101 control.name =
"move_z" 102 control.orientation.y = 0
103 control.orientation.z = 1
104 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
105 self.int_marker.controls.append(deepcopy(control))
106 control.orientation.w = 1
107 control.orientation.x = 1
108 control.orientation.y = 0
109 control.orientation.z = 0
110 control.name =
"rotate_x" 111 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
112 self.int_marker.controls.append(deepcopy(control))
113 control.name =
"rotate_y" 114 control.orientation.x = 0
115 control.orientation.y = 1
116 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
117 self.int_marker.controls.append(deepcopy(control))
118 control.name =
"rotate_z" 119 control.orientation.y = 0
120 control.orientation.z = 1
121 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
122 self.int_marker.controls.append(deepcopy(control))
125 self.ia_server.applyChanges()
128 self.br.sendTransform(
129 (self.interactive_obstacle_pose.pose.position.x, self.interactive_obstacle_pose.pose.position.y, self.interactive_obstacle_pose.pose.position.z),
130 (self.interactive_obstacle_pose.pose.orientation.x, self.interactive_obstacle_pose.pose.orientation.y, self.interactive_obstacle_pose.pose.orientation.z, self.interactive_obstacle_pose.pose.orientation.w),
131 rospy.Time.now(), self.
obstacle_frame, self.interactive_obstacle_pose.header.frame_id)
133 self.pub.publish(self.
co)
139 self.interactive_obstacle_pose.header = fb.header
140 self.interactive_obstacle_pose.pose = fb.pose
141 self.ia_server.applyChanges()
144 self.br.sendTransform(
145 (self.interactive_obstacle_pose.pose.position.x, self.interactive_obstacle_pose.pose.position.y, self.interactive_obstacle_pose.pose.position.z),
146 (self.interactive_obstacle_pose.pose.orientation.x, self.interactive_obstacle_pose.pose.orientation.y, self.interactive_obstacle_pose.pose.orientation.z, self.interactive_obstacle_pose.pose.orientation.w),
147 rospy.Time.now(), self.
obstacle_frame, self.interactive_obstacle_pose.header.frame_id)
149 self.co.operation = CollisionObject.MOVE
150 self.pub.publish(self.
co)
153 if __name__ ==
"__main__":
154 rospy.init_node(
"interactive_obstacle_node")
159 while not rospy.is_shutdown():
163 except rospy.ROSInterruptException
as e:
interactive_obstacle_pose