def make_marker(self):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.pose = self.pose
int_marker.scale = 1
int_marker.name = self.marker_name
control = InteractiveMarkerControl()
control.orientation.w = math.sqrt(2) / 2
control.orientation.x = 0
control.orientation.y = math.sqrt(2) / 2
control.orientation.z = 0
control.interaction_mode = self.interaction_mode
int_marker.controls.append(copy.deepcopy(control))
# make a box which also moves in the plane
markers = self.make_individual_markers(int_marker)
for marker in markers:
control.markers.append(marker)
control.always_visible = True
int_marker.controls.append(control)
# we want to use our special callback function
self.server.insert(int_marker, self.feedback)
def _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE,
id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=2001,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose)
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + 0.06)
button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=index, scale=Vector3(0, 0, 0.03),
text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
def _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
scale=Vector3(0, 0, 0.03), text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
def make_def(p, color=(0.5, 0.5, 0.5), desc=None):
im = InteractiveMarker()
im.header.frame_id = p.pose.header.frame_id
im.pose = p.pose.pose
im.name = p.name
if desc is None:
im.description = p.name
im.scale = 1.2 * max(p.bbox.dimensions)
marker = Marker()
marker.type = Marker.CUBE
marker.scale.x = p.bbox.dimensions[0]
marker.scale.y = p.bbox.dimensions[1]
marker.scale.z = p.bbox.dimensions[2]
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = 1.0
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.markers.append(marker)
im.controls.append(control)
return im
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(
pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def getMarkersFromPose(poseStamped):
int_marker = InteractiveMarker()
int_marker.header.frame_id = poseStamped.header.frame_id
int_marker.pose = poseStamped.pose
int_control = InteractiveMarkerControl()
int_control.markers = createMarker()
int_marker.controls.append(int_control)
return int_marker
def ScaleMarker(marker_template, control_scale=None, visual_scale=None):
"""Scale InteractiveMarker and/or a visual Marker associated with the InteractiveMarker.
@type marker_template: subclass of MarkerTemplate()
@param marker_template: The template object containing InteractiveMarkers.
@type control_scale: float
@param control_scale: The scale factor for the InteractiveMarker.
@type visual_scale: geometry_msgs/Vector3
@param visual_scale: The scale factor for the visualization Marker in the template.
"""
server = marker_template.server
menu_handler = marker_template.menu_handler
marker_name = marker_template.key
if server:
current_marker = server.get(marker_name)
if current_marker:
# rescale marker
marker = Marker()
marker = GetVisualMarker(current_marker)
if visual_scale is not None:
marker.scale = visual_scale
# push marker into visual control
visual = InteractiveMarkerControl()
visual.name = "visual"
visual.always_visible = GetVisualControl(
current_marker).always_visible
visual.interaction_mode = GetVisualControl(
current_marker).interaction_mode
visual.orientation = GetVisualControl(current_marker).orientation
visual.markers.append(marker)
new_marker = InteractiveMarker()
new_marker.header.frame_id = current_marker.header.frame_id
new_marker.name = current_marker.name
new_marker.description = current_marker.description
new_marker.pose = current_marker.pose
new_marker.scale = current_marker.scale
if control_scale is not None:
new_marker.scale = control_scale
new_marker.controls.append(visual)
for control in current_marker.controls:
if 'Translate' in control.name or 'Rotate' in control.name:
# todo rename Plane Translate so we don't need to do this extra check
if control.name not in [
'TranslateXY', 'TranslateYZ', 'TranslateXZ'
]:
new_marker.controls.append(
CreateTransRotControl(control.name))
# insert the updated marker into the server
server.insert(new_marker)
menu_handler.apply(server, marker_name)
def _get_object_marker(self, index, mesh=None):
"""Generate and return a marker for world objects.
Args:
index (int): ID for the new marker.
mesh (Mesh, optional): Mesh to use for the marker. Only
utilized if not None. Defaults to None.
Returns:
InteractiveMarker
"""
int_marker = InteractiveMarker()
int_marker.name = self._objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = self._objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE,
id=index,
lifetime=MARKER_DURATION,
scale=self._objects[index].object.dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_OBJ,
pose=self._objects[index].object.pose)
self.create_sides(index)
if mesh is not None:
object_marker = _get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
else:
for item in range((6 * (index)), (6 * (index)) + 6):
rospy.loginfo("Item: " + str(item) + "\nlen mark cont: " +
str(len(self._marker_controllers)))
int_marker.controls.append(self._marker_controllers[item])
text_pos = Point()
text_pos.x = self._objects[index].object.pose.position.x
text_pos.y = self._objects[index].object.pose.position.y
text_pos.z = (self._objects[index].object.pose.position.z +
self._objects[index].object.dimensions.z / 2 +
OFFSET_OBJ_TEXT_Z)
button_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=index,
scale=SCALE_TEXT,
text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
#rospy.loginfo("Int_marker controls: " + str(int_marker.controls))
return int_marker
def _get_object_marker(self, index, mesh=None):
'''Generate and return a marker for world objects.
Args:
index (int): ID for the new marker.
mesh (Mesh, optional): Mesh to use for the marker. Only
utilized if not None. Defaults to None.
Returns:
InteractiveMarker
'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=index,
lifetime=MARKER_DURATION,
scale=World.objects[index].object.dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_OBJ,
pose=World.objects[index].object.pose
)
if mesh is not None:
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (
World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z)
button_control.markers.append(
Marker(
type=Marker.TEXT_VIEW_FACING,
id=index,
scale=SCALE_TEXT,
text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))
)
)
int_marker.controls.append(button_control)
return int_marker
def ScaleMarker(marker_template, control_scale=None, visual_scale=None):
"""Scale InteractiveMarker and/or a visual Marker associated with the InteractiveMarker.
@type marker_template: subclass of MarkerTemplate()
@param marker_template: The template object containing InteractiveMarkers.
@type control_scale: float
@param control_scale: The scale factor for the InteractiveMarker.
@type visual_scale: geometry_msgs/Vector3
@param visual_scale: The scale factor for the visualization Marker in the template.
"""
server = marker_template.server
menu_handler = marker_template.menu_handler
marker_name = marker_template.key
if server:
current_marker = server.get(marker_name)
if current_marker:
# rescale marker
marker = Marker()
marker = GetVisualMarker(current_marker)
if visual_scale is not None:
marker.scale = visual_scale
# push marker into visual control
visual = InteractiveMarkerControl()
visual.name = "visual"
visual.always_visible = GetVisualControl(current_marker).always_visible
visual.interaction_mode = GetVisualControl(current_marker).interaction_mode
visual.orientation = GetVisualControl(current_marker).orientation
visual.markers.append(marker)
new_marker = InteractiveMarker()
new_marker.header.frame_id = current_marker.header.frame_id
new_marker.name = current_marker.name
new_marker.description = current_marker.description
new_marker.pose = current_marker.pose
new_marker.scale = current_marker.scale
if control_scale is not None:
new_marker.scale = control_scale
new_marker.controls.append(visual)
for control in current_marker.controls:
if 'Translate' in control.name or 'Rotate' in control.name:
# todo rename Plane Translate so we don't need to do this extra check
if control.name not in ['TranslateXY', 'TranslateYZ','TranslateXZ']:
new_marker.controls.append(CreateTransRotControl(control.name))
# insert the updated marker into the server
server.insert(new_marker)
menu_handler.apply(server, marker_name)
def _make_marker(self, name, pose):
"""Creates a new interactive marker.
Args:
name: The name of the marker.
pose: The geometry_msgs/Pose of the marker.
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = 'map'
int_marker.name = name
int_marker.description = name
int_marker.pose = pose
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 1
arrow_marker.scale.x = 0.45
arrow_marker.scale.y = 0.05
arrow_marker.scale.z = 0.05
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
text_marker = Marker()
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.orientation.w = 1
text_marker.pose.position.z = 1.5
text_marker.scale.x = 0.2
text_marker.scale.y = 0.2
text_marker.scale.z = 0.2
text_marker.color.r = 0.5
text_marker.color.g = 0.5
text_marker.color.b = 0.5
text_marker.color.a = 1
text_marker.text = name
arrow_control = InteractiveMarkerControl()
arrow_control.orientation.w = 1
arrow_control.orientation.y = 1
arrow_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
arrow_control.markers.append(arrow_marker)
arrow_control.markers.append(text_marker)
arrow_control.always_visible = True
int_marker.controls.append(arrow_control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
return int_marker
def _marker_style(self, name, pose):
int_marker = InteractiveMarker()
int_marker.header.frame_id = 'map'
int_marker.name = name
int_marker.description = name
int_marker.pose = pose
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 1
arrow_marker.scale.x = 0.5
arrow_marker.scale.y = 0.07
arrow_marker.scale.z = 0.05
arrow_marker.color.r = 0.5
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
# text_marker = Marker()
# text_marker.type = Marker.TEXT_VIEW_FACING
# text_marker.pose.orientation.w = 1
# text_marker.pose.position.z = 1.5
# text_marker.scale.x = 0.2
# text_marker.scale.y = 0.2
# text_marker.scale.z = 0.2
# text_marker.color.r = 0.5
# text_marker.color.g = 0.5
# text_marker.color.b = 0.5
# text_marker.color.a = 1
# text_marker.text = name
arrow_control = InteractiveMarkerControl()
arrow_control.orientation.w = 1
arrow_control.orientation.y = 1
arrow_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
arrow_control.markers.append(arrow_marker)
# arrow_control.markers.append(text_marker)
arrow_control.always_visible = True
int_marker.controls.append(arrow_control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
return int_marker
def create_im(self, marker, pose, name):
# create the new interactive marker
int_marker = InteractiveMarker()
int_marker.pose = copy.deepcopy(pose)
int_marker.header.frame_id = 'base_link'
int_marker.name = name
# move freely on the X-Y plane
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.markers.append(marker)
control.always_visible = True
int_marker.controls.append(control)
return int_marker
def create_im(self, marker, pose, name):
# create the new interactive marker
int_marker = InteractiveMarker()
int_marker.pose = copy.deepcopy(pose)
int_marker.header.frame_id = 'base_link'
int_marker.name = name
# move freely on the X-Y plane
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.markers.append(marker)
control.always_visible = True
int_marker.controls.append(control)
return int_marker
def update(self):
'''Updates marker/arm loop'''
self._menu_handler = MenuHandler()
# Inset main menu entries.
self._menu_handler.insert('Move gripper here',
callback=self._move_to_cb)
self._menu_handler.insert('Move marker to current gripper pose',
callback=self._move_pose_to_cb)
if self._is_hand_open():
self._menu_handler.insert('Close gripper',
callback=self._close_gripper_cb)
else:
self._menu_handler.insert('Open gripper',
callback=self._open_gripper_cb)
frame_id = REF_FRAME
pose = self.get_pose()
# if self._marker_moved() or self._menu_control is None:
rospy.loginfo("Marker moved")
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
self._menu_control = menu_control
# Make and add interactive marker.
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose.pose
int_marker.scale = INT_MARKER_SCALE
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(self._menu_control)
ArmControlMarker._im_server.insert(int_marker,
self._marker_feedback_cb)
self._menu_handler.apply(ArmControlMarker._im_server, self._get_name())
ArmControlMarker._im_server.applyChanges()
def _get_surface_marker(pose, dimensions):
'''Returns a surface marker with provided pose and dimensions.
Args:
pose (Pose)
dimensions (Vector3)
Returns:
InteractiveMarker
'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = BASE_LINK
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=2000,
lifetime=MARKER_DURATION,
scale=dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_SURFACE,
pose=pose
)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(
type=Marker.TEXT_VIEW_FACING,
id=2001,
scale=SCALE_TEXT, text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))
)
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
def interactive_gripper_marker(pose_stamped, finger_distance):
# gripper marker
gripper_marker = Marker()
gripper_marker.pose.position.x = 0.166 # wrist_roll_link/gripper_link Translation: [0.166, 0.000, 0.000]
gripper_marker.pose.orientation.w = 1 # wrist_roll_link/gripper_link Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
gripper_marker.type = Marker.MESH_RESOURCE
gripper_marker.mesh_resource = GRIPPER_MESH
gripper_marker.mesh_use_embedded_materials = True
finger_distance = max(CLOSE_FINGER_POS,
min(finger_distance, OPEN_FINGER_POS))
# left finger marker
l_marker = Marker()
l_marker.pose.position.x = 0.166
l_marker.pose.position.y = -finger_distance / 2.0
l_marker.pose.orientation.w = 1
l_marker.type = Marker.MESH_RESOURCE
l_marker.mesh_resource = L_FINGER_MESH
l_marker.mesh_use_embedded_materials = True
# right finger marker
r_marker = Marker()
r_marker.pose.position.x = 0.166
r_marker.pose.position.y = finger_distance / 2.0
r_marker.pose.orientation.w = 1
r_marker.type = Marker.MESH_RESOURCE
r_marker.mesh_resource = R_FINGER_MESH
r_marker.mesh_use_embedded_materials = True
control = InteractiveMarkerControl()
control.orientation.w = 1
control.interaction_mode = InteractiveMarkerControl.NONE
control.always_visible = True
control.markers.append(gripper_marker)
control.markers.append(l_marker)
control.markers.append(r_marker)
interactive_marker = InteractiveMarker()
interactive_marker.header = pose_stamped.header
interactive_marker.pose = pose_stamped.pose
interactive_marker.controls.append(control)
interactive_marker.scale = 0.3
return interactive_marker
def build_landmark_marker(landmark):
"""Generate and return a marker for world landmarks.
Args:
landmark (WorldLandmark): The landmark to generate a marker for.
Returns:
InteractiveMarker
"""
int_marker = InteractiveMarker()
int_marker.name = landmark.name()
int_marker.header.frame_id = BASE_LINK
int_marker.pose = landmark.object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE,
lifetime=MARKER_DURATION,
scale=landmark.object.dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_OBJ,
pose=landmark.object.pose)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = landmark.object.pose.position.x
text_pos.y = landmark.object.pose.position.y
text_pos.z = (landmark.object.pose.position.z +
landmark.object.dimensions.z / 2 + OFFSET_OBJ_TEXT_Z)
button_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
scale=SCALE_TEXT,
text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
def __init__(self, server, name, pose_set):
# ... Initialization, marker creation, etc. ...
self.pose = pose_set
if pose_set == None:
self.pose = geometry_msgs.msg.Pose()
int_marker = InteractiveMarker()
int_marker.header.frame_id = "odom"
int_marker.name = name
int_marker.description = name
int_marker.pose = self.pose
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 1
arrow_marker.scale.x = 0.7
arrow_marker.scale.y = 0.07
arrow_marker.scale.z = 0.07
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
arrow_marker.pose.position.z = 0.1
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.always_visible = True
control.markers.append(arrow_marker)
int_marker.controls.append(control)
#int_marker_control.markers.append(arrow_marker)
self._server = server
self._server.insert(int_marker, self._callback)
self._server.applyChanges()
def gripper_interactive_marker(pose_stamped, finger_distance):
gripper_marker = Marker()
gripper_marker.pose.position.x = 0.166
gripper_marker.pose.orientation.w = 1
gripper_marker.type = Marker.MESH_RESOURCE
gripper_marker.mesh_resource = GRIPPER_MESH
gripper_marker.mesh_use_embedded_materials = True
finger_distance = max(0, min(finger_distance, 0.1))
l_marker = Marker()
l_marker.pose.position.x = 0.166
l_marker.pose.position.y = finger_distance / 2.0 - 0.1
l_marker.pose.orientation.w = 1
l_marker.type = Marker.MESH_RESOURCE
l_marker.mesh_resource = L_FINGER_MESH
l_marker.mesh_use_embedded_materials = True
r_marker = Marker()
r_marker.pose.position.x = 0.166
r_marker.pose.position.y = -finger_distance / 2.0 + 0.1
r_marker.pose.orientation.w = 1
r_marker.type = Marker.MESH_RESOURCE
r_marker.mesh_resource = R_FINGER_MESH
r_marker.mesh_use_embedded_materials = True
control = InteractiveMarkerControl()
control.orientation.w = 1
control.interaction_mode = InteractiveMarkerControl.NONE
control.always_visible = True
control.markers.append(gripper_marker)
control.markers.append(l_marker)
control.markers.append(r_marker)
interactive_marker = InteractiveMarker()
interactive_marker.header = pose_stamped.header
interactive_marker.pose = pose_stamped.pose
interactive_marker.controls.append(control)
interactive_marker.scale = 0.25
return interactive_marker
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE,
id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose)
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + 0.06)
button_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=index,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
def __init__(self, server):
self.marker_name = "IMU"
self.imu_publisher = self.create_publisher(Imu, "/imu/data", 1)
self.server = server
self.pose = Pose()
self.pose.orientation.w = 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = "imu_frame"
int_marker.pose = self.pose
int_marker.scale = 1
int_marker.name = self.marker_name
int_marker.description = "Rotate 2DOF to simulate IMU orientation to ground"
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
normalize_quaternion(control.orientation)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.always_visible = True
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
normalize_quaternion(control.orientation)
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# we want to use our special callback function
self.server.insert(int_marker, self.process_feedback)
def __create_int_marker__(self, name, pose):
print("creating int marker: " + name)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = name
int_marker.description = name
int_marker.pose = pose
# Move it 0.25 meters up to make it easier to click
int_marker.pose.position.z = 0.25
text_marker = Marker()
text_marker.text = name
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.position.z = 2
text_marker.scale.x = 0.4
text_marker.scale.y = 0.4
text_marker.scale.z = 0.4
text_marker.color.r = 0.0
text_marker.color.g = 0.5
text_marker.color.b = 0.5
text_marker.color.a = 1.0
text_control = InteractiveMarkerControl()
text_control.name = "text_control"
text_control.markers.append(text_marker)
text_control.always_visible = True
text_control.interaction_mode = InteractiveMarkerControl.NONE
int_marker.controls.append(text_control)
rotation_ring_control = InteractiveMarkerControl()
rotation_ring_control.name = "position_control"
rotation_ring_control.always_visible = True
rotation_ring_control.orientation.x = 0
rotation_ring_control.orientation.w = 1
rotation_ring_control.orientation.y = 1
rotation_ring_control.orientation.z = 0
rotation_ring_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(rotation_ring_control)
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 1
arrow_marker.pose.position.z = 0.15
arrow_marker.pose.position.x = -0.5
arrow_marker.scale.x = 1
arrow_marker.scale.y = 0.25
arrow_marker.scale.z = 0.25
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
position_control = InteractiveMarkerControl()
position_control.name = "rotation_control"
position_control.always_visible = True
position_control.markers.append(arrow_marker)
position_control.orientation.w = 1
position_control.orientation.x = 0
position_control.orientation.y = 1
position_control.orientation.z = 0
position_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
int_marker.controls.append(position_control)
self._int_marker_server.insert(int_marker, self.__update_marker_pose__)
self._int_marker_server.applyChanges()
def getMarkersFromPose(poseStamped, preGraspStatus):
int_marker = InteractiveMarker()
int_marker.name = 'gripper'
int_marker.header.frame_id = poseStamped.header.frame_id
int_marker.pose = poseStamped.pose
int_control = InteractiveMarkerControl()
int_control.always_visible = True
int_control.orientation.w = 1
int_control.orientation.x = 0
int_control.orientation.y = 1
int_control.orientation.z = 0
# ADD GRIPPER CONTROLS
gripper_markers = createMarker(0, 0)
int_control.interaction_mode = InteractiveMarkerControl.MENU
int_control.markers.extend(gripper_markers)
int_marker.scale = 0.35
# ADD AUTO PICKING
if preGraspStatus:
#pre-grasp
int_control.markers.extend(createMarker(OFFSET_X, 0))
#lift-grasp
int_control.markers.extend(createMarker(0, OFFSET_Z))
#sample-box
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.pose.position.x += GRIPPER_OFFSET
box_marker.scale.x = 0.05
box_marker.scale.y = 0.05
box_marker.scale.z = 0.05
box_marker.color.r = 1
box_marker.color.g = 1.0
box_marker.color.b = 0.0
box_marker.color.a = 0.5
int_control.markers.append(box_marker)
int_marker.controls.append(int_control)
# ADD DOF
dof_controls = make_6dof_controls()
int_marker.controls.extend(dof_controls)
# ADD MENU ITEMS
menu_entries = []
open_gripper = MenuEntry()
open_gripper.id = GRIPPER_OPEN
open_gripper.command_type = MenuEntry.FEEDBACK
open_gripper.title = 'OPEN GRIPPER'
menu_entries.append(open_gripper)
close_gripper = MenuEntry()
close_gripper.id = GRIPPER_CLOSE
close_gripper.command_type = MenuEntry.FEEDBACK
close_gripper.title = 'CLOSE GRIPPER'
menu_entries.append(close_gripper)
move_gripper = MenuEntry()
move_gripper.id = GRIPPER_MOVETO
move_gripper.command_type = MenuEntry.FEEDBACK
if preGraspStatus:
move_gripper.title = 'Commence AutoPick'
else:
move_gripper.title = 'MOVE TO THIS POSE'
menu_entries.append(move_gripper)
int_marker.menu_entries.extend(menu_entries)
return int_marker
def _update_viz_core(self):
'''Updates visualization after a change'''
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
if (self.action_step.type == ActionStep.ARM_TARGET):
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.02, 0.02),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.4, 0.0, 0.8),
points=point_list)
menu_control.markers.append(main_marker)
menu_control.markers.append(ActionStepMarker.make_sphere_marker(
self.get_uid() + 2000,
self._get_traj_pose(0), frame_id, 0.05))
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(ActionStepMarker.make_sphere_marker(
self.get_uid() + 3000, self._get_traj_pose(last_index),
frame_id, 0.05))
else:
rospy.logerr('Non-handled action step type '
+ str(self.action_step.type))
ref_frame = World.get_ref_from_name(frame_id)
if (ref_frame == ArmState.OBJECT):
menu_control.markers.append(Marker(type=Marker.ARROW,
id=(1000 + self.get_uid()),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.03, 0.04),
header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.8, 0.2, 0.5),
points=[pose.position, Point(0, 0, 0)]))
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(), scale=Vector3(0, 0, 0.03),
text='Step' + str(self.step_number),
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
ActionStepMarker._im_server.insert(int_marker,
self.marker_feedback_cb)
def talker():
rospy.init_node('talker', anonymous=True)
# desired_pose_publisher = rospy.Publisher('/equilibrium_pose', Pose, queue_size=10)
interactive_marker_init_publisher = rospy.Publisher(
'/equilibrium_pose_marker/update_full',
InteractiveMarkerInit,
queue_size=10)
rate = rospy.Rate(30) # Hz
seq_num = 1693
# interactive_marker.type = 1 # MAY NEED TO UNCOMMENT IF IT DOESNT WORK
# Topic to be published
interactive_marker_init = InteractiveMarkerInit()
interactive_marker_init.server_id = "/interactive_marker"
interactive_marker = InteractiveMarker()
interactive_marker.name = "equilibrium_pose"
interactive_marker.header.frame_id = "panda_link0"
interactive_marker.scale = 0.3
interactive_marker.description = "Equilibrium Pose\nBE CAREFUL! If you move the \nequilibrium pose the robot will follow\
\ it\nso be aware of potential collisions"
# Controls - X direction
imc = InteractiveMarkerControl()
imc.name = "rotate_x"
imc.orientation.x = 1.0
imc.orientation.y = 0
imc.orientation.z = 0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 5
interactive_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "move_x"
imc.orientation.x = 1.0
imc.orientation.y = 0
imc.orientation.z = 0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 3
interactive_marker.controls.append(imc)
# Controls - Y direction
imc = InteractiveMarkerControl()
imc.name = "rotate_y"
imc.orientation.x = 0
imc.orientation.y = 1.0
imc.orientation.z = 0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 5
interactive_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "move_y"
imc.orientation.x = 0
imc.orientation.y = 1.0
imc.orientation.z = 0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 3
interactive_marker.controls.append(imc)
# Controls - Z direction
imc = InteractiveMarkerControl()
imc.name = "rotate_z"
imc.orientation.x = 0
imc.orientation.y = 0
imc.orientation.z = 1.0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 5
interactive_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "move_z"
imc.orientation.x = 0
imc.orientation.y = 0
imc.orientation.z = 1.0
imc.orientation.w = 1.0
imc.orientation_mode = 0
imc.interaction_mode = 3
interactive_marker.controls.append(imc)
desired_pose = Pose()
desired_pose.position.x = 0.441047421421
desired_pose.position.y = 0.00039597222295
desired_pose.position.z = 0.428872718226
desired_pose.orientation.x = 0.998920857906
desired_pose.orientation.y = -0.0855322959381
desired_pose.orientation.z = 0.0364367915829
desired_pose.orientation.w = -0.00293869199231
interactive_marker.pose = desired_pose
interactive_marker_init.markers.append(InteractiveMarker())
positive_y_thresh = 0.20
negative_y_thresh = -0.10
rospy.loginfo("Publishing!!")
while desired_pose.position.y < positive_y_thresh:
desired_pose.position.y += 0.001
rospy.loginfo("Pose(%f, %f, %f || %f, %f, %f, %f)",
desired_pose.position.x, desired_pose.position.y,
desired_pose.position.z, desired_pose.orientation.x,
desired_pose.orientation.y, desired_pose.orientation.z,
desired_pose.orientation.w)
interactive_marker_init.seq_num = seq_num
interactive_marker.pose = desired_pose
interactive_marker_init.markers[0] = interactive_marker
interactive_marker_init_publisher.publish(interactive_marker_init)
seq_num = seq_num + 1
rate.sleep()
while desired_pose.position.y > negative_y_thresh:
desired_pose.position.y -= 0.001
rospy.loginfo("Pose(%f, %f, %f || %f, %f, %f, %f)",
desired_pose.position.x, desired_pose.position.y,
desired_pose.position.z, desired_pose.orientation.x,
desired_pose.orientation.y, desired_pose.orientation.z,
desired_pose.orientation.w)
interactive_marker_init.seq_num = seq_num
interactive_marker.pose = desired_pose
interactive_marker_init.markers[0] = interactive_marker
interactive_marker_init_publisher.publish(interactive_marker_init)
seq_num = seq_num + 1
rate.sleep()
def create_interactive_6dof(name,
color=(1, 0, 0, 1),
frame_id='base_link',
transform=None,
scale=0.05):
if transform is None:
transform = np.eye(4)
int_marker = InteractiveMarker()
int_marker.header.stamp = get_safe_stamp()
int_marker.header.frame_id = frame_id
int_marker.name = name
int_marker.scale = scale
int_marker.pose = criros.conversions.to_pose(transform)
# Move X
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
# Move Y
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
# Move Z
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
# Rotate X
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# Rotate Y
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# Rotate Z
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
return int_marker
def publish_gripper(server, pose_stamped, name):
"""Publishes a marker representing a gripper.
Code taken from action_step_marker.py in PR2/PbD.
Args:
server: An InteractiveMarkerServer
pose_stamped: A PoseStamped giving the wrist_roll_link pose.
name: string, a unique name for this gripper.
"""
# Set angle of meshes based on gripper open vs closed.
angle = 28 * math.pi / 180.0 # Fully open.
STR_MESH_GRIPPER_FOLDER = 'package://pr2_description/meshes/gripper_v0/'
STR_GRIPPER_PALM_FILE = STR_MESH_GRIPPER_FOLDER + 'gripper_palm.dae'
STR_GRIPPER_FINGER_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger.dae'
STR_GRIPPER_FINGERTIP_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger_tip.dae'
# Make transforms in preparation for meshes 1, 2, and 3.
# NOTE(mbforbes): There are some magic numbers in here that are
# used a couple times. Seems like a good candidate for
# refactoring to constants, but I think they're more clear if
# left in here as (a) they likely won't be changed, and (b) it's
# easier to understand the computations with them here.
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
# Create mesh 1 (palm).
mesh1 = Marker()
mesh1.header.frame_id = pose_stamped.header.frame_id
mesh1.mesh_use_embedded_materials = True
mesh1.type = Marker.MESH_RESOURCE
mesh1.scale.x = 1.0
mesh1.scale.y = 1.0
mesh1.scale.z = 1.0
mesh1.mesh_resource = STR_GRIPPER_PALM_FILE
mesh1.pose = pose_stamped.pose
# Create mesh 2 (finger).
mesh2 = Marker()
mesh2.mesh_use_embedded_materials = True
mesh2.type = Marker.MESH_RESOURCE
mesh2.scale.x = 1.0
mesh2.scale.y = 1.0
mesh2.scale.z = 1.0
mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh2.pose = _get_pose_from_transform(t_proximal)
# Create mesh 3 (fingertip).
mesh3 = Marker()
mesh3.mesh_use_embedded_materials = True
mesh3.type = Marker.MESH_RESOURCE
mesh3.scale.x = 1.0
mesh3.scale.y = 1.0
mesh3.scale.z = 1.0
mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh3.pose = _get_pose_from_transform(t_distal)
# Make transforms in preparation for meshes 4 and 5.
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(math.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
# Create mesh 4 (other finger).
mesh4 = Marker()
mesh4.mesh_use_embedded_materials = True
mesh4.type = Marker.MESH_RESOURCE
mesh4.scale.x = 1.0
mesh4.scale.y = 1.0
mesh4.scale.z = 1.0
mesh4.mesh_resource = STR_GRIPPER_FINGER_FILE
mesh4.pose = _get_pose_from_transform(t_proximal)
# Create mesh 5 (other fingertip).
mesh5 = Marker()
mesh5.mesh_use_embedded_materials = True
mesh5.type = Marker.MESH_RESOURCE
mesh5.scale.x = 1.0
mesh5.scale.y = 1.0
mesh5.scale.z = 1.0
mesh5.mesh_resource = STR_GRIPPER_FINGERTIP_FILE
mesh5.pose = _get_pose_from_transform(t_distal)
# Append all meshes we made.
control = InteractiveMarkerControl()
control.markers = [mesh1, mesh2, mesh3, mesh4, mesh5]
control.interaction_mode = InteractiveMarkerControl.NONE
interactive_marker = InteractiveMarker()
interactive_marker.controls = [control]
interactive_marker.header.frame_id = pose_stamped.header.frame_id
interactive_marker.pose = pose_stamped.pose
interactive_marker.name = name
server.insert(interactive_marker)
server.applyChanges()
def make_marker_flexible(fixed, ps, scale, color, mtype,
ignore_rotation, ignore_x=False,
ignore_y=False, ignore_z=False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = ps.header.frame_id
int_marker.pose = ps.pose
int_marker.scale = scale
int_marker.name = 'simple_6dof'
int_marker.description = ''
# insert a marker
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(make_marker(scale, color, mtype))
int_marker.controls.append(control)
if fixed:
int_marker.name += '_fixed'
int_marker.description += '\n(fixed orientation)'
if not ignore_x:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1,0,0,1)
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_y:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,0,1,1)
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_z:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,1,0,1)
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_rotation:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1,0,0,1)
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,0,1,1)
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,1,0,1)
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
return int_marker
while not initial_pose_found:
rospy.sleep(1)
state_sub.unregister()
pose_pub = rospy.Publisher(
"equilibrium_pose", PoseStamped, queue_size=10)
server = InteractiveMarkerServer("equilibrium_pose_marker")
int_marker = InteractiveMarker()
int_marker.header.frame_id = link_name
int_marker.scale = 0.3
int_marker.name = "equilibrium_pose"
int_marker.description = ("Equilibrium Pose\nBE CAREFUL! "
"If you move the \nequilibrium "
"pose the robot will follow it\n"
"so be aware of potential collisions")
int_marker.pose = marker_pose.pose
# run pose publisher
rospy.Timer(rospy.Duration(0.005),
lambda msg: publisherCallback(msg, link_name))
# insert a box
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
def _update_viz_core(self):
'''Updates visualization after a change.'''
# Create a new IM control.
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
# Multiplex marker types added based on action step type.
if self.action_step.type == ActionStep.ARM_TARGET:
# Handle "normal" steps (saved poses).
menu_control = self._make_gripper_marker(
menu_control, self._is_hand_open())
elif self.action_step.type == ActionStep.ARM_TRAJECTORY:
# Handle trajectories.
# First, get all trajectory positions.
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
# Add a main maker for all points in the trajectory (sphere
# list).
menu_control.markers.append(
Marker(
type=Marker.SPHERE_LIST,
id=self.get_uid(),
lifetime=TRAJ_MARKER_LIFETIME,
scale=SCALE_TRAJ_STEP_SPHERES,
header=Header(frame_id=frame_id),
color=COLOR_TRAJ_STEP_SPHERES,
points=point_list
)
)
# Add a marker for the first step in the trajectory.
menu_control.markers.append(
ActionStepMarker._make_sphere_marker(
self.get_uid() + ID_OFFSET_TRAJ_FIRST,
self._get_traj_pose(0),
frame_id,
TRAJ_ENDPOINT_SCALE
)
)
# Add a marker for the last step in the trajectory.
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(
ActionStepMarker._make_sphere_marker(
self.get_uid() + ID_OFFSET_TRAJ_LAST,
self._get_traj_pose(last_index),
frame_id,
TRAJ_ENDPOINT_SCALE
)
)
else:
# Neither "normal" pose nor trajectory; error...
rospy.logerr(
'Non-handled action step type ' + str(self.action_step.type))
# Add an arrow to the relative object, if there is one.
ref_frame = World.get_ref_from_name(frame_id)
if ref_frame == ArmState.OBJECT:
menu_control.markers.append(
Marker(
type=Marker.ARROW,
id=(ID_OFFSET_REF_ARROW + self.get_uid()),
lifetime=TRAJ_MARKER_LIFETIME,
scale=SCALE_OBJ_REF_ARROW,
header=Header(frame_id=frame_id),
color=COLOR_OBJ_REF_ARROW,
points=[pose.position, Point(0, 0, 0)]
)
)
# Make and add the text for this step ('Step X').
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + TEXT_Z_OFFSET
menu_control.markers.append(
Marker(
type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(),
scale=SCALE_STEP_TEXT,
text='Step ' + str(self.step_number),
color=COLOR_STEP_TEXT,
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))
)
)
# Make and add interactive marker.
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = INT_MARKER_SCALE
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
ActionStepMarker._im_server.insert(
int_marker, self.marker_feedback_cb)
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
from visualization_msgs.msg import InteractiveMarker, InteractiveMarkerControl
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
from interactive_markers.interactive_marker_server import *
from geometry_msgs.msg import Pose, PoseStamped
if __name__=="__main__":
rospy.init_node('marker_teleop')
pub = rospy.Publisher('robot_interaction_interactive_marker_topic', InteractiveMarker)
robot = MoveGroupCommander("sia5d");
server = InteractiveMarkerServer("simple_marker")
# create interactive marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = "world"
int_marker.name = "my_marker"
int_marker.description = "Teleop Control"
p = robot.get_current_pose()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
p.pose.position.x = p.pose.position.x+0.05
int_marker.pose = p.pose
pub.publish(int_marker)
print 'heh'
rate.sleep()
def _update_viz_core(self):
'''Updates visualization after a change'''
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
if (self.action_step.type == ActionStep.ARM_TARGET):
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
elif (self.action_step.type == ActionStep.ARM_TRAJECTORY):
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
main_marker = Marker(type=Marker.SPHERE_LIST,
id=self.get_uid(),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.02, 0.02),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.4, 0.0, 0.8),
points=point_list)
menu_control.markers.append(main_marker)
menu_control.markers.append(
ActionStepMarker.make_sphere_marker(self.get_uid() + 2000,
self._get_traj_pose(0),
frame_id, 0.05))
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(
ActionStepMarker.make_sphere_marker(
self.get_uid() + 3000, self._get_traj_pose(last_index),
frame_id, 0.05))
else:
rospy.logerr('Non-handled action step type ' +
str(self.action_step.type))
ref_frame = World.get_ref_from_name(frame_id)
if (ref_frame == ArmState.OBJECT):
menu_control.markers.append(
Marker(type=Marker.ARROW,
id=(1000 + self.get_uid()),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.03, 0.04),
header=Header(frame_id=frame_id),
color=ColorRGBA(1.0, 0.8, 0.2, 0.5),
points=[pose.position, Point(0, 0, 0)]))
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
menu_control.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(),
scale=Vector3(0, 0, 0.03),
text='Step' + str(self.step_number),
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb)
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.frame_id
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "" # "EEF 6DOF control"
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
int_marker.controls.append(control)
# self.menu_handler.insert("Do stuff", callback=self.processFeedback)
# This makes the floating text appear
# make one control using default visuals
# control = InteractiveMarkerControl()
# control.interaction_mode = InteractiveMarkerControl.MENU
# control.description="Menu"
# control.name = "menu_only_control"
# int_marker.controls.append(copy.deepcopy(control))
# marker = self.makeArrow( int_marker )
# control.markers.append( marker )
# control.always_visible = False
# int_marker.controls.append(control)
self.server.insert(int_marker, self.processFeedback)
def make_marker_flexible(fixed,
ps,
scale,
color,
mtype,
ignore_rotation,
ignore_x=False,
ignore_y=False,
ignore_z=False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = ps.header.frame_id
int_marker.pose = ps.pose
int_marker.scale = scale
int_marker.name = 'simple_6dof'
int_marker.description = ''
# insert a marker
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(make_marker(scale, color, mtype))
int_marker.controls.append(control)
if fixed:
int_marker.name += '_fixed'
int_marker.description += '\n(fixed orientation)'
if not ignore_x:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1, 0, 0, 1)
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_y:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_z:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_rotation:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1, 0, 0, 1)
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
return int_marker
def _update_viz_core(self):
'''Updates visualization after a change'''
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = self._get_ref_name()
pose = self.get_pose()
if (self.action_step.type == ArmStepType.ARM_TARGET):
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
elif (self.action_step.type == ArmStepType.ARM_TRAJECTORY):
point_list = []
for j in range(len(self.action_step.armTrajectory.timing)):
point_list.append(self._get_traj_pose(j).position)
main_marker = Marker(type=Marker.SPHERE_LIST, id=self.get_uid(),
lifetime=rospy.Duration(2),
scale=Vector3(0.02, 0.02, 0.02),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.4, 0.0, 0.8),
points=point_list)
menu_control.markers.append(main_marker)
menu_control.markers.append(ArmStepMarker.make_sphere_marker(
self.get_uid() + 2000,
self._get_traj_pose(0), frame_id, 0.05))
last_index = len(self.action_step.armTrajectory.timing) - 1
menu_control.markers.append(ArmStepMarker.make_sphere_marker(
self.get_uid() + 3000, self._get_traj_pose(last_index),
frame_id, 0.05))
else:
rospy.logerr('Non-handled action step type '
+ str(self.action_step.type))
ref_frame = World.get_world().get_ref_from_name(frame_id)
if (ref_frame == ArmState.OBJECT):
# The following is needed to properly display the arrtow in browser, due to the fact that ros3djs
# displays all nested markers in the reference frame of the interactive marker.
# Thus, we need to calculate the position of the object in the reference frame of the interactive marker.
quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
inv_quat_matrix = quaternion_matrix(quaternion_inverse(quat))
pose_vec = numpy.array((pose.position.x, pose.position.y, pose.position.z, 0))
new_pose = numpy.dot(inv_quat_matrix, pose_vec)
menu_control.markers.append(Marker(type=Marker.ARROW,
id=(1000 + self.get_uid()),
lifetime=rospy.Duration(2),
scale=Vector3(0.01, 0.01, 0.0001),
header=Header(frame_id=frame_id),
color=ColorRGBA(0.8, 0.8, 0.0, 0.6),
points=[Point(0, 0, 0), Point(-new_pose[0], -new_pose[1], -new_pose[2])]))
# Calculate text position so that they "orbit" around the marker;
# this is done so that poses in identical or similar positions
# have non-overlapping text. Note that to do this without moving
# the text around as the camera is moved, we assume that the viewer
# is always looking directly at the robot, so we assume the x dimension
# is constant and "orbin" in the y-z plane.
n_orbitals = 8 # this should be a constant
offset = 0.15 # this should be a constant
orbital = (self.step_number - 1) % n_orbitals # - 1 to make 0-based
angle_rad = (float(orbital) / n_orbitals) * (-2 * numpy.pi) + \
(numpy.pi / 2.0) # start above, at pi/2 (90 degrees)
text_pos = Point()
text_pos.x = 0
text_pos.y = numpy.cos(angle_rad) * offset
text_pos.z = numpy.sin(angle_rad) * offset
r,g,b = self.get_marker_color()
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=self.get_uid(), scale=Vector3(0.06, 0.06, 0.06),
lifetime=rospy.Duration(1.5),
text='Step ' + str(self.step_number),
color=ColorRGBA(r, g, b, 1.0),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(menu_control)
self.parent_step_sequence.im_server.insert(int_marker,
self.marker_feedback_cb)
def create_interactive_6dof(name, color=(1,0,0,1), frame_id='base_link',
transform=None, scale=0.05):
"""
Create a 6-DoF interactive marker
Parameters
----------
name: string
Marker name
color: array_like
Marker color is a 4 elements iterable to be used as RGBA color
frame_id: str
Reference frame of the marker frame. This a frame that must exist in TF
transform: array_like
Homogenous transformation (4x4) of the marker with respect to the
reference frame ``frame_id``
scale: float
Scale of the marker applied before the position/orientation.
Returns
----------
int_marker: visualization_msgs.InteractiveMarker
The 6-DoF interactive marker
"""
if transform is None:
transform = np.eye(4)
int_marker = InteractiveMarker()
int_marker.header.stamp = get_safe_stamp()
int_marker.header.frame_id = frame_id
int_marker.name = name
int_marker.scale = scale
int_marker.pose = criconv.to_pose(transform)
# Move X
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
# Move Y
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
# Move Z
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
# Rotate X
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# Rotate Y
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# Rotate Z
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
return int_marker
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
def make_6dof_gripper(fixed, ps, scale, color, robot_type = "pr2",
ignore_rotation = False, ignore_x=False,
ignore_y=False, ignore_z=False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = ps.header.frame_id
int_marker.pose = ps.pose
int_marker.scale = scale
int_marker.name = 'gripper_6dof'
control = InteractiveMarkerControl()
control.always_visible = True
control.name = 'pr2_gripper_control'
if robot_type == "pr2":
control = make_pr2_gripper_marker(ps, [0.3, 0.3, 0.3, 0.7], control=control)
int_marker.description = 'pr2_gripper_control'
elif robot_type == "cody":
control = make_cody_ee_marker(ps, [1, 1, 1, 0.4], control=control)
int_marker.description = 'cody_ee_control'
elif robot_type == "darci":
control = make_darci_ee_marker(ps, [1, 1, 1, 0.4], control=control)
int_marker.description = 'darci_ee_control'
int_marker.controls.append( control )
if not ignore_x:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1,0,0,1)
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_y:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,0,1,1)
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_z:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,1,0,1)
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_rotation:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1,0,0,1)
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,0,1,1)
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0,1,0,1)
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
return int_marker