def interactive_marker(name, pose, scale):
int_marker = ims.InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.pose.position.x = pose[0][0]
int_marker.pose.position.y = pose[0][1]
int_marker.pose.position.z = pose[0][2]
int_marker.pose.orientation.x = pose[1][0]
int_marker.pose.orientation.y = pose[1][1]
int_marker.pose.orientation.z = pose[1][2]
int_marker.pose.orientation.w = pose[1][3]
int_marker.scale = scale
int_marker.name = name
int_marker.description = name
return int_marker
def __init__(self, tf, height, radius, name, color, im_server, listwidget,
remove_callback):
self.tf = tf.copy()
self.height = height
self.radius = radius
self.color = color
self.name = name
# Users provide feedback to refine the mesh
# position + configuration through RViz
# InteractiveMarkers
self.im_marker = ros_im.InteractiveMarker()
self.im_marker.header.frame_id = "base"
self.im_marker.name = name
self.im_marker.description = "Hypothesized Carrot"
self.im_marker.scale = 0.15
self.im_marker.pose = ros_utils.ROSPoseMsgFromPose(
spartanUtils.dict_from_homogenous_transform(tf))
# Visualize current carrot mesh
self.mesh_control = None
self._regenerateMesh()
# Add some control widgets
self._add6DofControls()
self.im_server = im_server
self.im_server.insert(self.im_marker, self._processImMarkerFeedbackCb)
self.im_server.applyChanges()
# Make control interface + add to list widget
self.control_widget = CarrotHypothesisWidget(name=name,
radius=radius,
height=height,
owner_hypothesis=self)
self.control_widget_listwidgetitem = QListWidgetItem(listwidget)
self.control_widget_listwidgetitem.setSizeHint(
self.control_widget.sizeHint())
self.listwidget = listwidget
self.listwidget.addItem(self.control_widget_listwidgetitem)
self.listwidget.setItemWidget(self.control_widget_listwidgetitem,
self.control_widget)
self.remove_callback = remove_callback
def _AddWaypointCallback(self, pose):
name = 'waypoint_%d' % self._current_waypoint_index
description = 'waypoint %d' % self._current_waypoint_index
self._current_waypoint_index += 1
self._AddWaypoint(name, pose)
waypoint_int_marker = interactive_marker_server.InteractiveMarker()
waypoint_int_marker.header.frame_id = pose.header.frame_id
waypoint_int_marker.name = name
waypoint_int_marker.description = description
waypoint_int_marker.pose = copy.deepcopy(pose.pose)
waypoint_int_marker.pose.position.z += 0.01
waypoint_marker = interactive_marker_server.Marker()
waypoint_marker.type = interactive_marker_server.Marker.ARROW
waypoint_marker.scale.x = 0.8
waypoint_marker.scale.y = 0.8
waypoint_marker.scale.z = 0.8
waypoint_marker.color.r = 0.0
waypoint_marker.color.g = 0.8
waypoint_marker.color.b = 0.0
waypoint_marker.color.a = 1.0
waypoint_control = interactive_marker_server.InteractiveMarkerControl()
waypoint_control.always_visible = True
waypoint_control.markers.append(waypoint_marker)
waypoint_int_marker.controls.append(waypoint_control)
translate_control = interactive_marker_server.InteractiveMarkerControl(
)
translate_control.name = 'move_rotate_plane'
translate_control.interaction_mode = interactive_marker_server.InteractiveMarkerControl.MOVE_ROTATE
translate_control.orientation.x = 0
translate_control.orientation.y = 1
translate_control.orientation.z = 0
translate_control.orientation.w = 1
translate_control.always_visible = True
waypoint_int_marker.controls.append(translate_control)
self.server.insert(waypoint_int_marker, self._MarkerFeedbackCallback)
self.menu_handler.apply(self.server, name)
self.server.applyChanges()
def make_interactive_marker(name, pose, frame='base', color=(0.5, 0.5, 0.5)):
rospy.loginfo("Creating imarker...")
int_marker = interactive_marker_server.InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.header.stamp = rospy.Time.now()
int_marker.scale = 0.25
int_marker.name = name
int_marker.description = "interactive marker"
cylinder_marker = Marker()
cylinder_marker.type = Marker.CYLINDER
cylinder_marker.scale.x = 0.25
cylinder_marker.scale.y = 0.25
cylinder_marker.scale.z = 0.25
cylinder_marker.color.r = color[0]
cylinder_marker.color.g = color[1]
cylinder_marker.color.b = color[2]
cylinder_marker.color.a = 0.5
cylinder_control = InteractiveMarkerControl()
cylinder_control.always_visible = True
cylinder_control.markers.append(cylinder_marker)
int_marker.controls.append(cylinder_control)
for axis, orientation in zip(['x', 'y', 'z'], [(1, 1, 0, 0), (1, 0, 1, 0),
(1, 0, 0, 1)]):
control = InteractiveMarkerControl()
for attr, val in zip(['w', 'x', 'y', 'z'], orientation):
setattr(control.orientation, attr, val)
control.name = "rotate_{}".format(axis)
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control.name = "move_{}".format(axis)
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
int_marker.pose = pose
rospy.loginfo("Imarker created")
return int_marker