def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map"):
marker = Marker()
marker.mesh_resource = resource;
marker.header.frame_id = frame_id
marker.type = marker.MESH_RESOURCE
marker.scale.x = scale*scales[0]
marker.scale.y = scale*scales[1]
marker.scale.z = scale*scales[2]
marker.color.a = rgba[3]
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
obj_control = InteractiveMarkerControl()
obj_control.always_visible = True
obj_control.markers.append( marker )
return obj_control
def _make_revolute_marker(self, revolute_joint: RevoluteJoint_):
int_marker = InteractiveMarker()
int_marker.header.frame_id = revolute_joint.child_body().body_frame().name()
int_marker.name = revolute_joint.name()
int_marker.scale = 0.3
int_marker.pose.position.x = 0.
int_marker.pose.position.y = 0.
int_marker.pose.position.z = 0.
int_marker.pose.orientation.w = 1.
int_marker.pose.orientation.x = 0.
int_marker.pose.orientation.y = 0.
int_marker.pose.orientation.z = 0.
# Drake revolute axis is in frame F on parent
axis_hat = revolute_joint.revolute_axis()
self._joint_axis_in_child[revolute_joint.name()] = axis_hat
# What rotation would get the parent X axis to align with the joint axis?
rotation_matrix = ComputeBasisFromAxis(0, axis_hat)
pydrake_quat = RotationMatrix(rotation_matrix).ToQuaternion()
joint_control = InteractiveMarkerControl()
joint_control.orientation.w = pydrake_quat.w()
joint_control.orientation.x = pydrake_quat.x()
joint_control.orientation.y = pydrake_quat.y()
joint_control.orientation.z = pydrake_quat.z()
joint_control.always_visible = True
joint_control.name = f'rotate_axis_{revolute_joint.name()}'
joint_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(joint_control)
return int_marker
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 CreateVisualControlFromMarker(marker, always_visible=True, interaction_mode=InteractiveMarkerControl.MENU):
control = InteractiveMarkerControl()
control.name = "visual"
control.always_visible = always_visible
control.interaction_mode = interaction_mode
control.markers.append(marker)
return control
def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map"):
marker = Marker()
marker.mesh_resource = resource;
marker.header.frame_id = frame_id
marker.type = marker.MESH_RESOURCE
marker.scale.x = scale*scales[0]
marker.scale.y = scale*scales[1]
marker.scale.z = scale*scales[2]
marker.color.a = rgba[3]
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
obj_control = InteractiveMarkerControl()
obj_control.always_visible = True
obj_control.markers.append( marker )
return obj_control
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 make_marker(self):
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = "map"
self.int_marker.pose = self.pose
self.int_marker.scale = 1
self.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
self.int_marker.controls.append(copy.deepcopy(control))
# make a box which also moves in the plane
markers = self.make_individual_markers(self.int_marker)
for marker in markers:
control.markers.append(marker)
control.always_visible = True
self.int_marker.controls.append(control)
# we want to use our special callback function
self.server.insert(self.int_marker, self.feedback)
def MakeMuneObject(self, MenuName, MenuPose):
MenuInteractiveMarker = InteractiveMarker()
MenuInteractiveMarker.name = MenuName
MenuInteractiveMarker.header.frame_id = self.frame_id
MenuInteractiveMarker.pose.position.z += self.MenuHight
MenuInteractiveMarker.scale = self.MenuScale
MenuControl = InteractiveMarkerControl()
MenuControl.interaction_mode = InteractiveMarkerControl.MENU
MenuControl.always_visible = False
MenuMarker = Marker()
MenuMarker.type = Marker.ARROW
MenuMarker.scale.x = MenuInteractiveMarker.scale * 2
MenuMarker.scale.y = MenuInteractiveMarker.scale * 0.45
MenuMarker.scale.z = MenuInteractiveMarker.scale * 0.45
MenuMarker.color.r = 0.5
MenuMarker.color.g = 0.5
MenuMarker.color.b = 0.5
MenuMarker.color.a = 1.0
MenuMarker.pose = MenuPose
MenuControl.markers.append(MenuMarker)
MenuInteractiveMarker.controls.append(MenuControl)
#print '###################MenuInteractiveMarker info:\n', MenuInteractiveMarker
self.server.insert(MenuInteractiveMarker)
rospy.loginfo('insert Menu Marker Object')
def main():
rospy.init_node('interactive_marker_node')
server = InteractiveMarkerServer("simple_marker")
# create an interative marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.name = "my_marker"
int_marker.description = "Simple Click Control"
int_marker.pose.position.x = 1
int_marker.pose.orientation.w = 1
# create a cube for the interactive marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.pose.orientation.w = 1
box_marker.scale.x = 0.45
box_marker.scale.y = 0.45
box_marker.scale.z = 0.45
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
# create a control for the interactive marker
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
button_control.markers.append(box_marker)
int_marker.controls.append(button_control)
server.insert(int_marker, handle_viz_input)
server.applyChanges()
rospy.spin()
def create_marker(path_msg, color_msg, description, path_id, width=0.1, delta_z=0.1):
int_marker = InteractiveMarker()
int_marker.header.frame_id = path_msg.header.frame_id
int_marker.name = str(path_id)
int_marker.description = "Path {0}".format(path_id)
# line_marker = Marker()
# line_marker.type = Marker.LINE_STRIP
# line_marker.scale.x = width
# line_marker.color = color_msg
# line_marker.points = [p.pose.position for p in path_msg.poses]
# for point in line_marker.points:
# point.z += delta_z
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MENU
# control.markers.append(line_marker)
points = [node(pose, delta_z) for pose in path_msg.poses]
for p1, p2 in zip(points[:-1], points[1:]):
control.markers.append(cylinder_between(p1, p2, color_msg, width))
for p in points:
control.markers.append(sphere_at(p, color_msg, width))
int_marker.controls.append(copy.deepcopy(control))
menu_handler = MenuHandler()
# put all the information in the main menu
#d = menu_handler.insert("Description")
for line in description:
menu_handler.insert(line)#, parent=d)
return menu_handler, int_marker
def create_interactive_mesh(name,
resource,
color=(1, 0, 0, 1),
frame_id='base_link',
transform=None,
scale=1):
if transform is None:
transform = np.eye(4)
# Start with the interactive control
int_marker = create_interactive_6dof(name, color, frame_id, transform)
# Mesh control
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.NONE
# Mesh marker
marker = Marker()
marker.type = Marker.MESH_RESOURCE
marker.scale = Vector3(*[scale for _ in range(3)])
marker.color = ColorRGBA(*color)
marker.action = Marker.ADD
marker.mesh_resource = resource
# Add mesh control
control.markers.append(marker)
int_marker.controls.append(control)
return int_marker
def createCubeMarker(offset=(0,0,0), marker_id = 0, rgba=(1,0,0,1), orientation=(0,0,0,1), scale=(0.1,0.1,0.1), frame_id="/map"):
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.CUBE
marker.id = marker_id
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.color.a = rgba[3]
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
obj_control = InteractiveMarkerControl()
obj_control.always_visible = True
obj_control.markers.append( marker )
return obj_control
def make(self):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.name = self._name
int_marker.description = self._name
int_marker.pose.position.x = self._x
int_marker.pose.position.y = self._y
int_marker.pose.orientation.w = 1
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.pose.orientation.w = 1
box_marker.scale.x = 0.45
box_marker.scale.y = 0.45
box_marker.scale.z = 0.45
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
button_control.markers.append(box_marker)
int_marker.controls.append(button_control)
self._server.insert(int_marker, self.handle_viz_input)
self._server.applyChanges()
def MessControl(self, unit, mode): control=InteractiveMarkerControl() control.orientation.w = 1 control.orientation.y = 1 control.interaction_mode= mode control.always_visible=True control.markers.append(copy.deepcopy(unit))
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 createCubeMarker(offset=(0,0,0), marker_id = 0, rgba=(1,0,0,1), orientation=(0,0,0,1), scale=(0.1,0.1,0.1), frame_id="/map", ns = ''):
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.CUBE
marker.id = marker_id
marker.ns = ns
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.color.a = rgba[3]
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
obj_control = InteractiveMarkerControl()
obj_control.always_visible = True
obj_control.markers.append( marker )
return obj_control
def _CreateMarkerControl(name, orientation, marker_type):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.interaction_mode = marker_type
control.always_visible = False
return control
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 make_box_control(msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.orientation.w = 1
control.markers.append(make_box(msg))
msg.controls.append(control)
return control
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 MessControl(self, unit, mode):
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.y = 1
control.interaction_mode = mode
control.always_visible = True
control.markers.append(copy.deepcopy(unit))
def makeHumanTagMarker(self, position, name):
"""
Make coordinates and control for tag
:param: position of tag
:param: name for tag
:returns:
"""
int_marker = InteractiveMarker()
int_marker.pose.position = position
int_marker.scale = 1
int_marker.name = name
int_marker.description = name
self.makeBoxControlHumanTag(int_marker)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
server.insert(int_marker, self.processFeedback)
def start(self):
# create blue cube marker
obj_marker = Marker()
obj_marker.type = Marker.CUBE
obj_marker.pose.orientation.w = 1
obj_marker.scale.x = 0.06
obj_marker.scale.y = 0.06
obj_marker.scale.z = 0.06
obj_marker.color.r = 0.09
obj_marker.color.g = 0.97
obj_marker.color.b = 2.39
obj_marker.color.a = 1.0
obj_control = InteractiveMarkerControl()
obj_control.interaction_mode = InteractiveMarkerControl.MENU
obj_control.always_visible = True
obj_control.markers.append(obj_marker)
obj_im = InteractiveMarker()
obj_im.name = 'object'
obj_im.header.frame_id = 'base_link'
obj_im.pose.position.x = 0.7
obj_im.pose.position.y = 0
obj_im.pose.position.z = 0.5
obj_im.pose.orientation.w = 1
obj_im.scale = 0.3
obj_im.menu_entries.append(menu_entry(1, 'Pick from front'))
obj_im.menu_entries.append(menu_entry(2, 'Open gripper'))
obj_im.controls.append(obj_control)
obj_im.controls.extend(six_dof_controls())
self._im_server.insert(obj_im, feedback_cb=self.handle_feedback)
self._im_server.applyChanges()
self.update_gripper()
def make_interactive_marker(pose, model):
int_marker = InteractiveMarker()
int_marker.name = model
int_marker.description = model
int_marker.header.frame_id = "map"
int_marker.pose = pose
int_marker.pose.position.z = max(int_marker.pose.position.z,
0.01) # ensure marker is above ground
int_marker.pose.orientation = quaternion_msg_from_yaw(
yaw(pose)) # discard all but yaw to ensure marker is flat
int_marker.scale = 1
make_model_control(int_marker, model)
control = InteractiveMarkerControl()
control.name = "drag"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
normalize_quaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
server.insert(int_marker, process_feedback)
menu_handler.apply(server, int_marker.name)
return int_marker
def add_menu_handler(int_marker, menu_handler, server):
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MENU
control.description="Options"
control.name = "menu_only_control"
int_marker.controls.append(control)
menu_handler.apply(server, int_marker.name)
server.applyChanges()
def add_menu_handler(int_marker, menu_handler, server):
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MENU
control.description = "Options"
control.name = "menu_only_control"
int_marker.controls.append(control)
menu_handler.apply(server, int_marker.name)
server.applyChanges()
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 CreatePrimitiveControl(name, scaleFactor, marker_type, id=randint(0,10000)):
marker = CreatePrimitiveMarker(name, scaleFactor, marker_type, id)
control = InteractiveMarkerControl()
control.name = name
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MENU
control.markers.append(marker)
return control
def addControl(self, direction, control_type):
control = InteractiveMarkerControl()
quat = exo.KDLFrame([0, 0, 0] + direction + [1]).get_quaternion()
control.orientation.x = quat[0]
control.orientation.y = quat[1]
control.orientation.z = quat[2]
control.orientation.w = quat[3]
control.interaction_mode = control_type
self.int_marker.controls.append(control)
def add_new_marker(self,
name,
body,
callback,
selected=True,
delay_update=False,
frame=None,
pose=None):
"""Adds a new interactive marker to the node.
:param name: Name of the marker
:type name: str
:param body: Body controlled by the marker
:type body: RigidBody, MultiBody
:param callback: Callback function for the marker
:type callback: function
:param selected: Should the marker be selected upon creation
:type selected: bool
:param delay_update: Delay the update to the server
:type delay_update: bool
:param frame: Name of reference frame
:type frame: NoneType, str
:param pose: Initial pose of the marker
:type pose: NoneType, Frame
"""
intMarker = InteractiveMarker()
intMarker.name = name
intMarker.header.frame_id = frame if frame is not None else name
intMarker.header.stamp = rospy.Time(0)
intMarker.scale = 1.0
if pose is not None:
intMarker.pose.position.x = pose.position[0]
intMarker.pose.position.y = pose.position[1]
intMarker.pose.position.z = pose.position[2]
intMarker.pose.orientation.x = pose.quaternion[0]
intMarker.pose.orientation.y = pose.quaternion[1]
intMarker.pose.orientation.z = pose.quaternion[2]
intMarker.pose.orientation.w = pose.quaternion[3]
control = InteractiveMarkerControlMsg()
control.interaction_mode = InteractiveMarkerControlMsg.MOVE_3D
control.always_visible = True
control.orientation.w = 1.0
control.name = 'visual'
control.description = name
intMarker.controls.append(control)
make6DOFGimbal(intMarker)
self.create_visual_marker(name, body, intMarker, control)
activateMarker(intMarker, selected)
self.marker_server.insert(intMarker, callback)
self.markers[name] = (intMarker, callback)
body.register_deletion_cb(self.on_obj_deleted)
if not delay_update:
self.marker_server.applyChanges()
def axis_control(name, q):
cr = InteractiveMarkerControl()
cr.orientation.x = q[0]
cr.orientation.y = q[1]
cr.orientation.z = q[2]
cr.orientation.w = q[3]
cr.name = name
return cr
def initialize_object_marker(self, object_id, pose):
print "Adding interactive marker for object: ", object_id
color = self.object_id_color(object_id)
marker = InteractiveMarker()
marker.header.frame_id = "map"
marker.name = "object_marker_" + str(object_id)
marker.description = "Object " + str(object_id)
click_marker = InteractiveMarker()
click_marker.header.frame_id = "map"
click_marker.name = "button_object_marker_" + str(object_id)
click_marker.description = ""
# the marker in the middle
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.25
box_marker.scale.y = 0.25
box_marker.scale.z = 0.25
box_marker.color.r = color[0]
box_marker.color.g = color[1]
box_marker.color.b = color[2]
box_marker.color.a = 1.
box_marker.id = 1000
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
#box_control.always_visible = False
box_control.markers.append(box_marker)
box_control.name = "button"
box_control.interaction_mode = InteractiveMarkerControl.BUTTON
marker.controls.append(box_control)
# move x
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.always_visible = True
# control.name = "move_x"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.name = "move_plane"
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
marker.controls.append(control)
self.marker_poses[object_id] = pose
self.previous_poses[object_id] = pose
self.marker_server.insert(marker, self.marker_feedback)
self.marker_server.applyChanges()
pose.position.z = 0.15
self.marker_server.setPose( marker.name, pose )
self.marker_server.applyChanges()
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 __init__(self,
frame_id,
name,
description,
is_manager=False,
speed=0.2):
InteractiveMarker.__init__(self)
marker_scale_x = rospy.get_param('~marker_scale_x', 1.0)
marker_scale_y = rospy.get_param('~marker_scale_y', 0.2)
marker_scale_z = rospy.get_param('~marker_scale_z', 0.2)
print "Marker scales " + str(marker_scale_x) + ", " + str(
marker_scale_y) + ", " + str(marker_scale_z)
self.header.frame_id = frame_id
self.name = name
self.description = description
self.speed = speed
self.marker = Marker()
self.marker.type = Marker.ARROW
self.marker.scale.x = marker_scale_x
self.marker.scale.y = marker_scale_y
self.marker.scale.z = marker_scale_z
#self.marker.pose.position.z = 0.20
if is_manager:
self.marker.color.r = 0.8
self.marker.color.g = 0.0
self.marker.color.b = 0.0
self.marker.color.a = 0.5
else:
self.marker.color.r = 0.0
self.marker.color.g = 0.8
self.marker.color.b = 0.0
self.marker.color.a = 0.5
self.marker_control = InteractiveMarkerControl()
self.marker_control.always_visible = True
self.marker_control.orientation.w = 1
self.marker_control.orientation.x = 0
self.marker_control.orientation.y = 1
self.marker_control.orientation.z = 0
self.marker_control.name = "move_plane"
self.marker_control.markers.append(self.marker)
self.marker_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
self.controls.append(self.marker_control)
self.marker_control2 = InteractiveMarkerControl()
self.marker_control2.orientation.w = 1
self.marker_control2.orientation.x = 0
self.marker_control2.orientation.y = 1
self.marker_control2.orientation.z = 0
self.marker_control2.name = "rotate_z"
self.marker_control2.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.controls.append(self.marker_control2)
def CreateVisualControlFromMarker(
marker,
always_visible=True,
interaction_mode=InteractiveMarkerControl.MENU):
control = InteractiveMarkerControl()
control.name = "visual"
control.always_visible = always_visible
control.interaction_mode = interaction_mode
control.markers.append(marker)
return control
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
'''Creates and returns one component of the 6dof controller.
Args:
name (str): Name for hte control
orientation (Quaternion): How the control should be
oriented.
is_move (bool): Looks like whether the marker moves the
object (?). Currently passed as True for moving markers,
False for rotating ones.
is_fixed (bool): Looks like whether position is fixed (?).
Currently always passed as True.
Returns:
InteractiveMarkerControl
'''
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if self.is_control_visible:
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def CreatePrimitiveControl(name,
scaleFactor,
marker_type,
id=randint(0, 10000)):
marker = CreatePrimitiveMarker(name, scaleFactor, marker_type, id)
control = InteractiveMarkerControl()
control.name = name
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MENU
control.markers.append(marker)
return control
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 initialize_room_markers(self):
for room_id in range(0, len(self.regions)):
pose = Pose()
pose.position.x = self.centers[room_id, 0]
pose.position.y = self.centers[room_id, 1]
pose.position.z = 0.2
pose.orientation.x = 0.
pose.orientation.y = 0.
pose.orientation.z = 0.
pose.orientation.w = 1.
marker = InteractiveMarker()
marker.header.frame_id = "map"
marker.name = "room_marker_" + str(room_id)
marker.description = "Room " + str(room_id)
# the marker in the middle
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.35
box_marker.scale.y = 0.35
box_marker.scale.z = 0.35
box_marker.color.r = 0.
box_marker.color.g = 0.
box_marker.color.b = 1.
box_marker.color.a = 1.
box_marker.id = 1000
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
#box_control.always_visible = False
box_control.markers.append(box_marker)
box_control.name = "button"
box_control.interaction_mode = InteractiveMarkerControl.BUTTON
marker.controls.append(box_control)
#marker.controls.append(box_control)
# move x
#control = InteractiveMarkerControl()
#control.orientation.w = 1
#control.orientation.x = 0
#control.orientation.y = 1
#control.orientation.z = 0
#control.always_visible = True
# control.name = "move_x"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.marker_server.insert(marker, self.room_feedback)
self.marker_server.applyChanges()
self.marker_server.setPose( marker.name, pose )
self.marker_server.applyChanges()
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 create_object_marker(self, soma_obj, roi, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = soma_type + ' (' + roi + ')'
int_marker.pose = pose
int_marker.pose.position.z = 0.01
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
int_marker.pose.position.z = (marker.scale.z / 2)
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
#marker.pose = pose
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
int_marker.controls.append(control);
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append( marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
def create_object_marker(self, soma_obj, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = soma_obj
int_marker.description = "id" + soma_obj
int_marker.pose = pose
mesh_marker = Marker()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.scale.x = 1
mesh_marker.scale.y = 1
mesh_marker.scale.z = 1
random.seed(soma_type)
val = random.random()
mesh_marker.color.r = r_func(val)
mesh_marker.color.g = g_func(val)
mesh_marker.color.b = b_func(val)
mesh_marker.color.a = 1.0
#mesh_marker.pose = pose
mesh_marker.mesh_resource = self.mesh[soma_type]
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
int_marker.controls.append(control);
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append( mesh_marker) #makeBox(int_marker) )
int_marker.controls.append(menu_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 __init__(self):
rospy.sleep(1.0)
self.items = [
'pinger1', 'pinger2', 'dice', 'start_gate1', 'start_gate2'
]
self.guess_service = rospy.Service('guess_location', GuessRequest,
self.request_location)
self.markers_subscribers = []
self.markers_locations = dict.fromkeys(self.items)
self.markers_servers = []
self.markers = []
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.45
box_marker.scale.y = 0.45
box_marker.scale.z = 0.45
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append(box_marker)
rotate_control = InteractiveMarkerControl()
rotate_control.name = "move_x"
rotate_control.orientation.w = 0.707
rotate_control.orientation.x = 0
rotate_control.orientation.y = 0.707
rotate_control.orientation.z = 0
rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
spacer = 0
for i in self.items:
self.markers.append(InteractiveMarker())
self.markers[spacer].header.frame_id = "map"
self.markers[spacer].name = i
self.markers[spacer].description = i
self.markers[spacer].controls.append(box_control)
self.markers[spacer].controls.append(rotate_control)
self.markers[spacer].pose.position.x = spacer
self.markers[spacer].pose.position.y = 0
self.markers[spacer].pose.position.z = 0
spacer = spacer + 1
def talker():
pub = rospy.Publisher("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update", InteractiveMarkerUpdate)
rospy.init_node('talker', anonymous=True)
r = rospy.Rate(1)
int_marker = InteractiveMarker()
int_marker.name = "EE:goal_link_t"
controller = InteractiveMarkerControl()
controller.name = '_u1'
int_marker.controls = controller
p = InteractiveMarkerPose()
updater = InteractiveMarkerUpdate()
while not rospy.is_shutdown():
p.pose.position.x += 0.5
updater.markers = int_marker
updater.poses = p
print updater
pub.publish(updater)
r.sleep()
def create_interactive_marker(holder, id, pos, size, color, func):
# Make interactive marker for mouse selection
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base"
int_marker.name = "object"+str(id)
int_marker.pose.position.x = pos[0]
int_marker.pose.position.y = pos[1]
int_marker.pose.position.z = pos[2]
#color = [1, 0, 0]
# Add click control
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.interaction_mode = InteractiveMarkerControl.BUTTON
create_shape(box_control, "object", id, pos, size=size, color=color)
# add the control to the interactive marker
int_marker.controls.append( box_control )
holder.insert(int_marker, lambda x: func(x, color))
def create_roi_marker(self, roi, soma_type, pose, points):
#print "POINTS: " + str(points)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = "ROI-" + roi
int_marker.description = roi
int_marker.pose = pose
marker = Marker()
marker.type = Marker.LINE_STRIP
marker.scale.x = 0.1
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append( marker )
int_marker.controls.append(control )
marker.points = []
for point in points:
p = Point()
pose = self._soma_obj_pose[point]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
p = Point()
pose = self._soma_obj_pose[points[0]]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
return int_marker
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def createPointMarker(points, colors, offset=(0,0,0), orientation=(0,0,0,1)):
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.scale.x = 0.003
marker.scale.y = 0.003
marker.scale.z = 0.003
n = len(points)//3
for i in range(n):
p = Point()
p.x = points[i*3+0]
p.y = points[i*3+1]
p.z = points[i*3+2]
marker.points.append(p)
p = ColorRGBA()
p.r = colors[i*3+0]
p.g = colors[i*3+1]
p.b = colors[i*3+2]
p.a = 1
marker.colors.append(p)
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
obj_control = InteractiveMarkerControl()
obj_control.always_visible = True
obj_control.markers.append( marker )
return obj_control
def __init__(self):
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = "/world"
self.int_marker.name = "my_marker"
self.int_marker.description = "Simple 2-DOF Control"
self.int_marker.scale = 0.5
self.mesh_marker = MeshMarker()
self.mesh_marker.set_color(color=(1.0,1.0,1.0,0.5))
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = False
box_control.markers.append(self.mesh_marker.marker)
# add the control to the interactive marker
self.int_marker.controls.append(box_control)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.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
self.int_marker.controls.append(control)
self.set_position()
self.set_orientation()
def init_marker(self):
self.server = InteractiveMarkerServer("control_markers")
control_marker = InteractiveMarker()
control_marker.header.frame_id = "/base"
control_marker.name = "move_arm_marker"
move_control = InteractiveMarkerControl()
move_control.name = "move_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_y"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "move_z"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_x"
move_control.orientation.w = 1
move_control.orientation.x = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_y"
move_control.orientation.w = 1
move_control.orientation.z = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
move_control = InteractiveMarkerControl()
move_control.name = "rotate_z"
move_control.orientation.w = 1
move_control.orientation.y = 1
move_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control_marker.controls.append(move_control)
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
box = Marker()
box.type = Marker.CUBE
box.scale.x = 0.15
box.scale.y = 0.03
box.scale.z = 0.03
box.color.r = 0.5
box.color.g = 0.5
box.color.b = 0.5
box.color.a = 1.0
menu_control.markers.append(box)
box2 = deepcopy(box)
box2.scale.x = 0.03
box2.scale.z = 0.1
box2.pose.position.z=0.05
menu_control.markers.append(box2)
control_marker.controls.append(menu_control)
control_marker.scale = 0.25
self.server.insert(control_marker, self.control_marker_feedback)
self.menu_handler = MenuHandler()
self.menu_handler.insert("Move Arm", callback=self.move_arm_cb)
obs_entry = self.menu_handler.insert("Obstacles")
self.menu_handler.insert("No Obstacle", callback=self.no_obs_cb, parent=obs_entry)
self.menu_handler.insert("Simple Obstacle", callback=self.simple_obs_cb, parent=obs_entry)
self.menu_handler.insert("Hard Obstacle", callback=self.complex_obs_cb, parent=obs_entry)
self.menu_handler.insert("Super-hard Obstacle", callback=self.super_obs_cb, parent=obs_entry)
options_entry = self.menu_handler.insert("Options")
self.plot_entry = self.menu_handler.insert("Plot trajectory", parent=options_entry,
callback = self.plot_cb)
self.menu_handler.setCheckState(self.plot_entry, MenuHandler.UNCHECKED)
self.menu_handler.apply(self.server, "move_arm_marker",)
self.server.applyChanges()
Ttrans = tf.transformations.translation_matrix((0.6,0.2,0.2))
Rtrans = tf.transformations.rotation_matrix(3.14159,(1,0,0))
self.server.setPose("move_arm_marker", convert_to_message(numpy.dot(Ttrans,Rtrans)))
self.server.applyChanges()
int_marker.name = "my_marker"
int_marker.description = "Simple 1-DOF Control"
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.45
box_marker.scale.y = 0.45
box_marker.scale.z = 0.45
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append( box_marker )
# add the control to the interactive marker
int_marker.controls.append( box_control )
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
rotate_control = InteractiveMarkerControl()
rotate_control.name = "move_x"
rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
# add the control to the interactive marker
int_marker.controls.append(rotate_control);
left_offset = translation_matrix([0.01, 0.02 + foot_margin / 2.0, 0])
right_offset = translation_matrix([0.01, -0.02 - foot_margin / 2.0, 0])
foot_depth = 0.240
foot_width = 0.140
lleg_end_coords = "lleg_end_coords"
rleg_end_coords = "rleg_end_coords"
# create a grey box marker
lleg_marker = makeFootCube(foot_depth, foot_width, left_offset)
rleg_marker = makeFootCube(foot_depth, foot_width, right_offset)
lleg_marker.color.g = 1.0
rleg_marker.color.r = 1.0
# rleg_marker.pose.position.y = - foot_margin / 2.0
# lleg_marker.pose.position.y = foot_margin / 2.0
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.extend([lleg_marker, rleg_marker])
# add the control to the interactive marker
int_marker.controls.append( box_control )
int_marker.controls.extend(make6DOFControls())
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
server.insert(int_marker, processFeedback)
menu_handler.apply(server, int_marker.name)
def make6DOFControls():
translation_x_control = InteractiveMarkerControl()
translation_x_control.name = "move_x"
translation_x_control.orientation.w = 1
translation_x_control.orientation.x = 1
translation_x_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
translation_y_control = InteractiveMarkerControl()
translation_y_control.name = "move_y"
translation_y_control.orientation.w = 1
translation_y_control.orientation.y = 1
translation_y_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
translation_z_control = InteractiveMarkerControl()
translation_z_control.name = "move_z"
translation_z_control.orientation.w = 1
translation_z_control.orientation.z = 1
translation_z_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
rotation_x_control = InteractiveMarkerControl()
rotation_x_control.name = "rotate_x"
rotation_x_control.orientation.w = 1
rotation_x_control.orientation.x = 1
rotation_x_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
rotation_y_control = InteractiveMarkerControl()
rotation_y_control.name = "rotate_y"
rotation_y_control.orientation.w = 1
rotation_y_control.orientation.y = 1
rotation_y_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
rotation_z_control = InteractiveMarkerControl()
rotation_z_control.name = "rotate_z"
rotation_z_control.orientation.w = 1
rotation_z_control.orientation.z = 1
rotation_z_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
return [translation_x_control, translation_y_control,
translation_z_control,
rotation_x_control, rotation_y_control,
rotation_z_control]
def make6DofMarker( fixed = False ):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.scale = 0.05
int_marker.pose.position.x = -0.100988589227
int_marker.pose.position.y = 0.035845387727
int_marker.pose.position.z = 0.266128748655
int_marker.name = "paper_sheet"
int_marker.description = "Place the sheet of paper"
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.21
box_marker.scale.y = 0.297
box_marker.scale.z = 0.001
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = 1.0
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append( a4_sheet() )
# add the control to the interactive marker
int_marker.controls.append( box_control )
if fixed:
int_marker.name += "_fixed"
int_marker.description += "\n(fixed orientation)"
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
if fixed:
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
if fixed:
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
if fixed:
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
if fixed:
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
if fixed:
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
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
server.insert(int_marker, processFeedback)
def __init__(self):
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
#get this from the frame_tracker parameters
if rospy.has_param('cartesian_controller/chain_tip_link'):
self.active_frame = rospy.get_param("cartesian_controller/chain_tip_link")
else:
rospy.logerr("No chain_tip_link specified. Aborting!")
sys.exit()
if rospy.has_param('cartesian_controller/tracking_frame'):
self.tracking_frame = rospy.get_param("cartesian_controller/tracking_frame")
else:
rospy.logerr("No tracking_frame specified. Aborting!")
sys.exit()
if rospy.has_param('cartesian_controller/root_frame'):
self.root_frame = rospy.get_param("cartesian_controller/root_frame")
else:
rospy.logerr("No root_frame specified. Setting to 'base_link'!")
self.root_frame = "base_link"
if rospy.has_param('cartesian_controller/movable_trans'):
self.movable_trans = rospy.get_param("cartesian_controller/movable_trans")
else:
rospy.logerr("No movable_trans specified. Setting True!")
self.movable_trans = True
if rospy.has_param('cartesian_controller/movable_rot'):
self.movable_rot = rospy.get_param("cartesian_controller/movable_rot")
else:
rospy.logerr("No movable_rot specified. Setting True!")
self.movable_rot = True
self.tracking = False
print "Waiting for StartTrackingServer..."
rospy.wait_for_service('frame_tracker/start_tracking')
print "...done!"
self.start_tracking_client = rospy.ServiceProxy('frame_tracker/start_tracking', SetString)
print "Waiting for StopTrackingServer..."
rospy.wait_for_service('frame_tracker/stop_tracking')
print "...done!"
self.stop_tracking_client = rospy.ServiceProxy('frame_tracker/stop_tracking', Empty)
self.target_pose = PoseStamped()
self.target_pose.header.stamp = rospy.Time.now()
self.target_pose.header.frame_id = self.root_frame
self.target_pose.pose.orientation.w = 1.0
##give tf_listener some time to fill cache
#try:
#rospy.sleep(1.0)
#except rospy.ROSInterruptException as e:
##print "ROSInterruptException"
#pass
transform_available = False
while not transform_available:
try:
(trans,rot) = self.listener.lookupTransform(self.root_frame, self.active_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
#rospy.logwarn("Waiting for transform...")
try:
rospy.sleep(0.1)
except rospy.ROSInterruptException as e:
#print "ROSInterruptException"
pass
continue
transform_available = True
self.target_pose.pose.position.x = trans[0]
self.target_pose.pose.position.y = trans[1]
self.target_pose.pose.position.z = trans[2]
self.target_pose.pose.orientation.x = rot[0]
self.target_pose.pose.orientation.y = rot[1]
self.target_pose.pose.orientation.z = rot[2]
self.target_pose.pose.orientation.w = rot[3]
self.ia_server = InteractiveMarkerServer("marker_server")
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = self.root_frame
self.int_marker.pose = self.target_pose.pose
self.int_marker.name = "interactive_target"
self.int_marker.description = self.tracking_frame
self.int_marker.scale = 0.8
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 0.1
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
control_3d = InteractiveMarkerControl()
control_3d.always_visible = True
control_3d.name = "move_rotate_3D"
control_3d.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control_3d.markers.append( box_marker )
self.int_marker.controls.append(control_3d)
control = InteractiveMarkerControl()
control.always_visible = True
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
if(self.movable_trans):
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "move_y"
control.orientation.x = 0
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "move_z"
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
if(self.movable_rot):
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
self.int_marker.controls.append(deepcopy(control))
control.name = "rotate_y"
control.orientation.x = 0
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "rotate_z"
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(deepcopy(control))
self.ia_server.insert(self.int_marker, self.marker_fb)
#create menu
self.menu_handler = MenuHandler()
self.menu_handler.insert( "StartTracking", callback=self.start_tracking )
self.menu_handler.insert( "StopTracking", callback=self.stop_tracking )
self.menu_handler.insert( "ResetTracking", callback=self.reset_tracking )
self.int_marker_menu = InteractiveMarker()
self.int_marker_menu.header.frame_id = self.root_frame
self.int_marker_menu.name = "marker_menu"
self.int_marker_menu.description = rospy.get_namespace()
self.int_marker_menu.scale = 1.0
self.int_marker_menu.pose.position.z = 1.2
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MENU
control.name = "menu_control"
control.description= "InteractiveTargetMenu"
self.int_marker_menu.controls.append(copy.deepcopy(control))
self.ia_server.insert(self.int_marker_menu, self.menu_fb)
self.menu_handler.apply( self.ia_server, self.int_marker_menu.name )
self.ia_server.applyChanges()
def createMoveControls(fixed=False):
controls = []
## rotate control 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
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
controls.append(control)
## rotate control y
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
controls.append(control)
## rotate control z
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
controls.append(control)
## move control 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
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
controls.append(control)
## move control y
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
controls.append(control)
## move control z
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
controls.append(control)
return controls
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)
