21 from copy
import deepcopy
25 from std_srvs.srv
import Empty
26 from cob_srvs.srv
import SetString
27 from geometry_msgs.msg
import Point, PoseStamped
28 from visualization_msgs.msg
import Marker, InteractiveMarker, InteractiveMarkerControl
39 if rospy.has_param(
'cartesian_controller/chain_tip_link'):
40 self.
active_frame = rospy.get_param(
"cartesian_controller/chain_tip_link")
42 rospy.logerr(
"No chain_tip_link specified. Aborting!")
44 if rospy.has_param(
'cartesian_controller/tracking_frame'):
47 rospy.logerr(
"No tracking_frame specified. Aborting!")
49 if rospy.has_param(
'cartesian_controller/root_frame'):
50 self.
root_frame = rospy.get_param(
"cartesian_controller/root_frame")
52 rospy.logerr(
"No root_frame specified. Setting to 'base_link'!")
55 if rospy.has_param(
'cartesian_controller/movable_trans'):
56 self.
movable_trans = rospy.get_param(
"cartesian_controller/movable_trans")
58 rospy.logerr(
"No movable_trans specified. Setting True!")
60 if rospy.has_param(
'cartesian_controller/movable_rot'):
61 self.
movable_rot = rospy.get_param(
"cartesian_controller/movable_rot")
63 rospy.logerr(
"No movable_rot specified. Setting True!")
67 print(
"Waiting for StartTrackingServer...")
68 rospy.wait_for_service(
'frame_tracker/start_tracking')
72 print(
"Waiting for StopTrackingServer...")
73 rospy.wait_for_service(
'frame_tracker/stop_tracking')
89 transform_available =
False
90 while not transform_available:
97 except rospy.ROSInterruptException
as e:
101 transform_available =
True
120 box_marker = Marker()
121 box_marker.type = Marker.CUBE
122 box_marker.scale.x = 0.1
123 box_marker.scale.y = 0.1
124 box_marker.scale.z = 0.1
125 box_marker.color.r = 0.0
126 box_marker.color.g = 0.5
127 box_marker.color.b = 0.5
128 box_marker.color.a = 1.0
129 control_3d = InteractiveMarkerControl()
130 control_3d.always_visible =
True
131 control_3d.name =
"move_rotate_3D"
132 control_3d.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
133 control_3d.markers.append( box_marker )
136 control = InteractiveMarkerControl()
137 control.always_visible =
True
138 control.orientation.w = 1
139 control.orientation.x = 1
140 control.orientation.y = 0
141 control.orientation.z = 0
143 control.name =
"move_x"
144 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
145 self.
int_marker.controls.append(deepcopy(control))
146 control.name =
"move_y"
147 control.orientation.x = 0
148 control.orientation.y = 1
149 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
150 self.
int_marker.controls.append(deepcopy(control))
151 control.name =
"move_z"
152 control.orientation.y = 0
153 control.orientation.z = 1
154 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
155 self.
int_marker.controls.append(deepcopy(control))
157 control.orientation.w = 1
158 control.orientation.x = 1
159 control.orientation.y = 0
160 control.orientation.z = 0
161 control.name =
"rotate_x"
162 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
163 self.
int_marker.controls.append(deepcopy(control))
164 control.name =
"rotate_y"
165 control.orientation.x = 0
166 control.orientation.y = 1
167 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
168 self.
int_marker.controls.append(deepcopy(control))
169 control.name =
"rotate_z"
170 control.orientation.y = 0
171 control.orientation.z = 1
172 control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
173 self.
int_marker.controls.append(deepcopy(control))
188 control = InteractiveMarkerControl()
189 control.interaction_mode = InteractiveMarkerControl.MENU
190 control.name =
"menu_control"
191 control.description=
"InteractiveTargetMenu"
203 except rospy.ServiceException
as e:
204 print(
"Service call failed: %s"%e)
213 except rospy.ServiceException
as e:
214 print(
"Service call failed: %s"%e)
223 transform_available =
False
224 while not transform_available:
228 rospy.logwarn(
"Waiting for transform...")
231 except rospy.ROSInterruptException
as e:
235 transform_available =
True
237 reset_pose = PoseStamped()
239 reset_pose.header.stamp = rospy.Time.now()
240 reset_pose.pose.position.x = trans[0]
241 reset_pose.pose.position.y = trans[1]
242 reset_pose.pose.position.z = trans[2]
243 reset_pose.pose.orientation.x = rot[0]
244 reset_pose.pose.orientation.y = rot[1]
245 reset_pose.pose.orientation.z = rot[2]
246 reset_pose.pose.orientation.w = rot[3]
258 self.target_pose.header = fb.header
259 self.target_pose.pose = fb.pose
260 self.ia_server.applyChanges()
266 self.
br.sendTransform(
272 if __name__ ==
"__main__":
273 rospy.init_node(
"interactive_frame_target")
278 while not rospy.is_shutdown():
282 except rospy.ROSInterruptException
as e: