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
int_marker.pose.position.z = 0.01
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
if self.marker[soma_type] == '3D':
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
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 _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 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 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 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 _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 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 _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 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 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 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 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 __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 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 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 init_marker(self):
control_marker = InteractiveMarker()
control_marker.header.frame_id = "/world_link"
control_marker.name = "cg_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)
control_marker.scale = 0.25
self.server.insert(control_marker, self.control_marker_feedback)
self.server.applyChanges()
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 __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 create_menus(self):
self.global_menu_handler = MenuHandler()
g_art = self.global_menu_handler.insert("Artificial")
self.global_menu_handler.insert("Add primitive",
parent=g_art,
callback=self.global_menu_add_prim_cb)
self.global_menu_handler.insert("Clear all",
parent=g_art,
callback=self.global_menu_clear_all_cb)
self.global_menu_handler.insert(
"Clear all (permanent)",
parent=g_art,
callback=self.global_menu_clear_all_perm_cb)
self.global_menu_handler.insert("Save all",
parent=g_art,
callback=self.global_menu_save_all_cb)
self.global_menu_handler.insert("Reload",
parent=g_art,
callback=self.global_menu_reload_cb)
g_det = self.global_menu_handler.insert("Detected")
self.global_menu_handler.insert("Pause",
parent=g_det,
callback=self.global_menu_pause_cb)
self.global_menu_handler.insert(
"Clear all",
parent=g_det,
callback=self.global_menu_det_clear_all_cb)
self.a_obj_menu_handler = MenuHandler()
mov = self.a_obj_menu_handler.insert("Moveable",
callback=self.menu_moveable_cb)
self.a_obj_menu_handler.setCheckState(mov, MenuHandler.UNCHECKED)
sc = self.a_obj_menu_handler.insert("Scaleable",
callback=self.menu_scaleable_cb)
self.a_obj_menu_handler.setCheckState(sc, MenuHandler.UNCHECKED)
self.a_obj_menu_handler.insert("Save", callback=self.menu_save_cb)
self.a_obj_menu_handler.insert("Clear", callback=self.menu_remove_cb)
self.d_obj_menu_handler = MenuHandler()
self.d_obj_menu_handler.insert("Clear", callback=self.d_menu_clear_cb)
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.world_frame
int_marker.pose.position.z = 1.2
int_marker.scale = 0.5
int_marker.name = "global_menu"
int_marker.description = "Global Menu"
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.always_visible = True
marker = Marker()
marker.type = Marker.CUBE
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
control.markers.append(marker)
int_marker.controls.append(control)
self.im_server.insert(int_marker, self.ignored_cb)
self.global_menu_handler.apply(self.im_server, int_marker.name)
self.im_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 init_marker(self):
self.server = InteractiveMarkerServer("control_markers")
control_marker = InteractiveMarker()
control_marker.header.frame_id = self.robot.get_root()
control_marker.name = "cc_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)
control_marker.scale = 0.25
self.server.insert(control_marker, self.control_marker_feedback)
global cc_entry, ik_entry, ik_go_entry
self.menu_handler = MenuHandler()
cc_entry = self.menu_handler.insert("Cartesian Control Mode", callback=self.set_cc_callback)
ik_entry = self.menu_handler.insert("Inverse Kinematics Mode", callback=self.set_ik_callback)
ik_go_entry = self.menu_handler.insert("IK to Here", callback=self.set_ik_go_callback)
self.menu_handler.setCheckState( cc_entry, MenuHandler.CHECKED )
self.menu_handler.setCheckState( ik_entry, MenuHandler.UNCHECKED )
self.menu_handler.setVisible( ik_go_entry, False )
self.menu_handler.apply(self.server, "cc_marker",)
(joint_name, first_link) = self.robot.child_map[self.robot.get_root()][0]
redundancy_marker = InteractiveMarker()
redundancy_marker.header.frame_id = first_link
redundancy_marker.name = "red_marker"
rotate_control = InteractiveMarkerControl()
rotate_control.name = "rotate_z"
rotate_control.orientation.w = 1
rotate_control.orientation.y = 1
rotate_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
redundancy_marker.controls.append(rotate_control)
redundancy_marker.scale = 0.25
self.server.insert(redundancy_marker, self.redundancy_marker_feedback)
# 'commit' changes and send to all clients
self.server.applyChanges()
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 add_6DOF(self, init_position = Point( 0.0, 0.0, 0.0), frame_id = 'map'):
marker = InteractiveMarker()
marker.header.frame_id = frame_id
marker.pose.position = init_position
marker.scale = 0.3
marker.name = 'camera_marker'
marker.description = 'Camera 6-DOF pose control'
# X axis rotation
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
marker.controls.append(control)
# X axis traslation
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
marker.controls.append(control)
# Y axis rotation
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
marker.controls.append(control)
# Y axis traslation
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
marker.controls.append(control)
# Z axis rotation
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
marker.controls.append(control)
# Z axis traslation
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
marker.controls.append(control)
# Add marker to server
self.server.insert(marker, self.marker_feedback)
self.server.applyChanges()
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_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()
def make_6dof_marker(self, pose, size, frame_id, base_frame_id, interaction_mode):
int_marker = InteractiveMarker()
int_marker.header.frame_id = base_frame_id
int_marker.pose.position.x = pose[0][0]
int_marker.pose.position.y = pose[0][1]
int_marker.pose.position.z = pose[0][2]
int_marker.pose.orientation.x = pose[1][0]
int_marker.pose.orientation.y = pose[1][1]
int_marker.pose.orientation.z = pose[1][2]
int_marker.pose.orientation.w = pose[1][3]
int_marker.scale = 1
int_marker.name = frame_id
int_marker.description = frame_id
# insert a box
self._make_box_control(int_marker, size)
int_marker.controls[0].interaction_mode = interaction_mode
n = norm([1, 1])
control = InteractiveMarkerControl()
control.orientation.w = 1 / n
control.orientation.x = 1 / n
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 / n
control.orientation.x = 1 / n
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 / n
control.orientation.x = 0
control.orientation.y = 1 / n
control.orientation.z = 0
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1 / n
control.orientation.x = 0
control.orientation.y = 1 / n
control.orientation.z = 0
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1 / n
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1 / n
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1 / n
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1 / n
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self._server.insert(int_marker, self.on_marker_feedback)
self._server.applyChanges()
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)
"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, arm_id))
# 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()
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
def setup_marker(self,
frame="velodyne",
name="capture vehicle",
translation=True):
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.name = name
int_marker.description = name
int_marker.scale = 3
marker_control = InteractiveMarkerControl()
marker_control.always_visible = True
marker_control.markers.append(self.marker)
int_marker.controls.append(marker_control)
control = InteractiveMarkerControl()
control.name = "rotate_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "rotate_z"
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(control)
control = InteractiveMarkerControl()
control.name = "rotate_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
if not translation:
#int_marker.pose.position = Point(0,0,0)
return int_marker
control = InteractiveMarkerControl()
control.name = "move_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "move_z"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "move_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
def setup_marker(self, frame="velodyne", name = "capture vehicle", translation=True):
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.name = name
int_marker.description = name
int_marker.scale = 3
marker_control = InteractiveMarkerControl()
marker_control.always_visible = True
marker_control.markers.append(self.marker)
int_marker.controls.append(marker_control)
control = InteractiveMarkerControl()
control.name = "rotate_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "rotate_z"
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(control)
control = InteractiveMarkerControl()
control.name = "rotate_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
if not translation :
#int_marker.pose.position = Point(0,0,0)
return int_marker
control = InteractiveMarkerControl()
control.name = "move_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "move_z"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "move_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
def add_6DOF(self, init_position=Point(0.0, 0.0, 0.0), frame_id='map'):
marker = InteractiveMarker()
marker.header.frame_id = frame_id
marker.pose.position = init_position
marker.scale = 0.3
marker.name = 'camera_marker'
marker.description = 'Camera 6-DOF pose control'
# X axis rotation
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
marker.controls.append(control)
# X axis traslation
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
marker.controls.append(control)
# Y axis rotation
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
marker.controls.append(control)
# Y axis traslation
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
marker.controls.append(control)
# Z axis rotation
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
marker.controls.append(control)
# Z axis traslation
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
marker.controls.append(control)
# Add marker to server
self.server.insert(marker, self.marker_feedback)
self.server.applyChanges()
def __init__(self):
rospy.init_node('revisualizer')
self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2)
self.kill_server = InteractiveMarkerServer("interactive_kill")
# text marker
# TODO: Clean this up, there should be a way to set all of this inline
self.surface_marker = visualization_msgs.Marker()
self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
self.surface_marker.scale.z = 0.1
self.depth_marker = visualization_msgs.Marker()
self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.depth_marker.scale.z = 0.1
# create marker for displaying current battery voltage
self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0)
self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5)
self.voltage_marker = visualization_msgs.Marker()
self.voltage_marker.header.frame_id = "base_link"
self.voltage_marker.lifetime = rospy.Duration(5)
self.voltage_marker.ns = 'sub'
self.voltage_marker.id = 22
self.voltage_marker.pose.position.x = -2.0
self.voltage_marker.scale.z = 0.2
self.voltage_marker.color.a = 1
self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING
# create an interactive marker to display kill status and change it
self.need_kill_update = True
self.kill_marker = InteractiveMarker()
self.kill_marker.header.frame_id = "base_link"
self.kill_marker.pose.position.x = -2.3
self.kill_marker.name = "kill button"
kill_status_marker = Marker()
kill_status_marker.type = Marker.TEXT_VIEW_FACING
kill_status_marker.text = "UNKILLED"
kill_status_marker.id = 55
kill_status_marker.scale.z = 0.2
kill_status_marker.color.a = 1.0
kill_button_control = InteractiveMarkerControl()
kill_button_control.name = "kill button"
kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
kill_button_control.markers.append(kill_status_marker)
self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
self.kill_marker.controls.append(kill_button_control)
self.killed = False
# connect kill marker to kill alarm
self.kill_listener = AlarmListener("kill")
self.kill_listener.add_callback(self.kill_alarm_callback)
self.kill_alarm = AlarmBroadcaster("kill")
# distance to bottom
self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
# distance to surface
self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
# battery voltage
self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
def QuadMain(isjoy):
def make_state_cost(dsys, base, x):
weight = base * np.ones((dsys.nX, ))
weight[system.get_config('ballx').index] = 50
weight[system.get_config('bally').index] = 50
weight[system.get_config('ballz').index] = 25
weight[system.get_config('quad1bry').index] = 500
weight[system.get_config('quad1brx').index] = 100
weight[system.get_config('quad1ry').index] = 10
weight[system.get_config('quad1rx').index] = 10
weight[system.get_config('quad1rz').index] = 1
weight[system.get_config('quad2bry').index] = 500
weight[system.get_config('quad2brx').index] = 15
weight[system.get_config('quad2ry').index] = 10
weight[system.get_config('quad2rx').index] = 10
weight[system.get_config('quad2rz').index] = 1
return np.diag(weight)
def make_input_cost(dsys, base, x):
weight = base * np.ones((dsys.nU, ))
# weight[system.get_input('x-force').index] = x
return np.diag(weight)
quad1cm_m = 1.0
quadmotor1_m = 1.0
quad2cm_m = 1.0
quadmotor2_m = 1.0
ball_m = 0.5
dt = 0.01 # timestep set to 10ms
# Create a new trep system - 6 generalized coordinates for quadrotor: x,y,z,roll,pitch,yaw
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
# define ball frame
ballz = trep.Frame(system.world_frame, trep.TZ, "ballz")
bally = trep.Frame(ballz, trep.TY, "bally")
ballx = trep.Frame(bally, trep.TX, "ballx")
ballx.set_mass(ball_m)
# define quadrotor 1 frame
quad1bry = trep.Frame(ballx, trep.RY, "quad1bry")
quad1brx = trep.Frame(quad1bry, trep.RX, "quad1brx")
quad1cm = trep.Frame(quad1brx, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 0, 2)),
"quad1cm")
quad1ry = trep.Frame(quad1cm, trep.RY, "quad1ry")
quad1rx = trep.Frame(quad1ry, trep.RX, "quad1rx")
quad1rz = trep.Frame(quad1rx, trep.RZ, "quad1rz")
quad1cm.set_mass(quad1cm_m) # central point mass
# define quadrotor 1 motor positions and mass
quad1m1 = trep.Frame(quad1rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (1, 0, 0)),
"quad1m1")
quad1m1.set_mass(quadmotor1_m)
quad1m2 = trep.Frame(quad1rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (-1, 0, 0)),
"quad1m2")
quad1m2.set_mass(quadmotor1_m)
quad1m3 = trep.Frame(quad1rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 1, 0)),
"quad1m3")
quad1m3.set_mass(quadmotor1_m)
quad1m4 = trep.Frame(quad1rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, -1, 0)),
"quad1m4")
quad1m4.set_mass(quadmotor1_m)
# set four thrust vectors with inputs u1,u2,u3,u4
trep.forces.BodyWrench(system,
quad1m1, (0, 0, 'u1q1', 0, 0, 0),
name='quad1w1')
trep.forces.BodyWrench(system,
quad1m2, (0, 0, 'u2q1', 0, 0, 0),
name='quad1w2')
trep.forces.BodyWrench(system,
quad1m3, (0, 0, 'u3q1', 0, 0, 0),
name='quad1w3')
trep.forces.BodyWrench(system,
quad1m4, (0, 0, 'u4q1', 0, 0, 0),
name='quad1w4')
# define quadrotor 2 frame
quad2bry = trep.Frame(ballx, trep.RY, "quad2bry")
quad2brx = trep.Frame(quad2bry, trep.RX, "quad2brx")
quad2cm = trep.Frame(quad2brx, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 0, 2)),
"quad2cm")
quad2ry = trep.Frame(quad2cm, trep.RY, "quad2ry")
quad2rx = trep.Frame(quad2ry, trep.RX, "quad2rx")
quad2rz = trep.Frame(quad2rx, trep.RZ, "quad2rz")
quad2cm.set_mass(quad2cm_m) # central point mass
# define quadrotor 2 motor positions and mass
quad2m1 = trep.Frame(quad2rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (1, 0, 0)),
"quad2m1")
quad2m1.set_mass(quadmotor2_m)
quad2m2 = trep.Frame(quad2rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (-1, 0, 0)),
"quad2m2")
quad2m2.set_mass(quadmotor2_m)
quad2m3 = trep.Frame(quad2rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 1, 0)),
"quad2m3")
quad2m3.set_mass(quadmotor2_m)
quad2m4 = trep.Frame(quad2rz, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, -1, 0)),
"quad2m4")
quad2m4.set_mass(quadmotor2_m)
# set four thrust vectors with inputs u1,u2,u3,u4
trep.forces.BodyWrench(system,
quad2m1, (0, 0, 'u1q2', 0, 0, 0),
name='quad2w1')
trep.forces.BodyWrench(system,
quad2m2, (0, 0, 'u2q2', 0, 0, 0),
name='quad2w2')
trep.forces.BodyWrench(system,
quad2m3, (0, 0, 'u3q2', 0, 0, 0),
name='quad2w3')
trep.forces.BodyWrench(system,
quad2m4, (0, 0, 'u4q2', 0, 0, 0),
name='quad2w4')
# set quadrotor initial altitude at 5m
system.get_config("ballz").q = 0.0
system.get_config("quad1bry").q = (math.pi / 2 - math.acos(1.5 / 2.0))
system.get_config("quad2bry").q = -(math.pi / 2 - math.acos(1.5 / 2.0))
horzf = 0.5 * ball_m * 9.8 * math.tan((math.pi / 2 - math.acos(1.5 / 2.0)))
vertf = (0.5 * ball_m + quad1cm_m + 4 * quadmotor1_m) * 9.8
quad1ang = math.atan(horzf / ((quad1cm_m + 4 * quadmotor1_m) * 9.8))
system.get_config(
"quad1ry").q = -(math.pi / 2 - math.acos(1.5 / 2.0)) + quad1ang
system.get_config("quad2ry").q = (math.pi / 2 -
math.acos(1.5 / 2.0)) - quad1ang
# Now we'll extract the current configuration into a tuple to use as
# initial conditions for a variational integrator.
q0 = system.q
# Create the discrete system
mvi = trep.MidpointVI(system)
t = np.arange(0.0, 10.0, 0.01)
dsys = discopt.DSystem(mvi, t)
# Generate cost function
Qcost = make_state_cost(dsys, 1, 0.00)
Rcost = make_input_cost(dsys, 0.01, 0.01)
totuf = math.sqrt(math.pow(horzf, 2) + math.pow(vertf, 2))
u0 = np.array([
totuf / 4, totuf / 4, totuf / 4, totuf / 4, totuf / 4, totuf / 4,
totuf / 4, totuf / 4
])
u = tuple(u0)
mvi.initialize_from_configs(0.0, q0, dt, q0)
pref = mvi.p2
Uint = np.zeros((len(t) - 1, system.nu))
qd = np.zeros((len(t), system.nQ))
pd = np.zeros((len(t), system.nQ))
for i, t in enumerate(t[:-1]):
Uint[i, :] = u0
qd[i, :] = q0
pd[i, :] = pref
qd[len(qd) - 1, :] = q0
pd[len(pd) - 1, :] = pref
Qk = lambda k: Qcost
(X, U) = dsys.build_trajectory(Q=qd, p=pd, u=Uint)
Kstab = dsys.calc_feedback_controller(X, U, Qk)
#print Kstab[0]
# Create and initialize the variational integrator)
u = tuple(u0)
#mvi.initialize_from_configs(0.0, q0, dt, q0)
# These are our simulation variables.
q = mvi.q2
p = mvi.p2
pref = p
t = mvi.t2
# start ROS node to broadcast transforms to rviz
rospy.init_node('quad_simulator')
broadcaster = tf.TransformBroadcaster()
pub = rospy.Publisher('config', Float32MultiArray, queue_size=2)
statepub = rospy.Publisher('joint_states', JointState, queue_size=2)
jointstates = JointState()
configs = Float32MultiArray()
if isjoy == True:
markerpub = rospy.Publisher('refmark', Marker, queue_size=2)
refmark = Marker()
if isjoy == False:
############### NEW ################################33
# create an interactive marker server on the topic namespace simple_marker
server = InteractiveMarkerServer("simple_marker")
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/world"
int_marker.name = "my_marker"
int_marker.description = "Simple 1-DOF Control"
int_marker.pose.position.z = 2.0
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.SPHERE
box_marker.scale.x = 0.15
box_marker.scale.y = 0.15
box_marker.scale.z = 0.15
box_marker.color.r = 0.0
box_marker.color.g = 1.0
box_marker.color.b = 0.0
box_marker.color.a = 1.0
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.orientation.w = 1
box_control.orientation.x = 0
box_control.orientation.y = 1
box_control.orientation.z = 0
box_control.name = "rotate_x"
box_control.name = "move_x"
box_control.always_visible = True
box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
int_marker.controls.append(box_control)
box_control = InteractiveMarkerControl()
box_control.orientation.w = 1
box_control.orientation.x = 0
box_control.orientation.y = 1
box_control.orientation.z = 0
box_control.name = "rotate_x"
box_control.name = "move_x"
box_control.always_visible = True
box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
box_control.markers.append(box_marker)
int_marker.controls.append(box_control)
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
server.insert(int_marker, processFeedback)
# 'commit' changes and send to all clients
server.applyChanges()
############### END NEW ###############################
if isjoy == True:
# subscribe to joystick topic from joy_node
rospy.Subscriber("joy", Joy, joycall)
r = rospy.Rate(100) # simulation rate set to 100Hz
# Simulator Loop
while not rospy.is_shutdown():
# reset simulator if trigger pressed
if buttons[0] == 1:
mvi.initialize_from_configs(0.0, q0, dt, q0)
qref = [0, axis[1], axis[0], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
u = tuple(u0 - np.dot(Kstab[0], np.append(q - q0 - qref, p - pref)))
# advance simluation one timestep using trep VI
try:
mvi.step(mvi.t2 + dt, u)
except trep.ConvergenceError:
print 'Trep simulation error - system resetting...'
rospy.sleep(2.)
mvi.initialize_from_configs(0.0, q0, dt, q0)
q = mvi.q2
p = mvi.p2
t = mvi.t2
configs.data = tuple(q) + u
if isjoy == True:
refmark.header.frame_id = "/world"
refmark.header.stamp = rospy.Time.now()
refmark.type = 2
refmark.scale.x = 0.15
refmark.scale.y = 0.15
refmark.scale.z = 0.15
refmark.color.r = 0.0
refmark.color.g = 1.0
refmark.color.b = 0.0
refmark.color.a = 1.0
refmark.pose.position.y = axis[1]
refmark.pose.position.x = axis[0]
refmark.pose.position.z = 2.0
jointstates.header.stamp = rospy.Time.now()
jointstates.name = [
"quad1bry", "quad1brx", "quad1ry", "quad1rx", "quad1rz",
"quad2bry", "quad2brx", "quad2ry", "quad2rx", "quad2rz"
]
jointstates.position = [
q[3], q[4], q[5], q[6], q[7], q[8], q[9], q[10], q[11], q[12]
]
jointstates.velocity = [
system.configs[3].dq, system.configs[4].dq, system.configs[5].dq,
system.configs[6].dq, system.configs[7].dq, system.configs[8].dq,
system.configs[9].dq, system.configs[10].dq, system.configs[11].dq,
system.configs[12].dq
]
# send transform data to rviz
broadcaster.sendTransform(
(q[2], q[1], q[0] + 2),
tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0),
rospy.Time.now(), "ball", "world")
statepub.publish(jointstates)
pub.publish(configs)
if isjoy == True:
markerpub.publish(refmark)
r.sleep()
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);
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
server.insert(int_marker, processFeedback)
# 'commit' changes and send to all clients
server.applyChanges()
rospy.spin()
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 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 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
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)
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
server.insert(int_marker, processFeedback)
# 'commit' changes and send to all clients
server.applyChanges()
rospy.spin()
def make6DofMarker(frame_id, fixed=False, description="Simple 6-DOF Control"):
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame_id
int_marker.scale = 1
int_marker.name = "simple_6dof"
int_marker.description = description
# insert a box
makeBoxControl(int_marker)
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)
return int_marker
def add_grasp_marker(self, frame_id, radius, height, init_position):
# Descripcion basica
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame_id
int_marker.pose.position = init_position
int_marker.scale = max(radius * 2, height) + 0.02
int_marker.name = "grasp_marker"
int_marker.description = "Graspable object"
# Geometria
marker = Marker()
marker.type = Marker.CYLINDER
marker.scale.x = radius * 2
marker.scale.y = radius * 2
marker.scale.z = height
marker.color.r = random()
marker.color.g = random()
marker.color.b = random()
marker.color.a = 1.0
# Control 6DOF
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(marker)
int_marker.controls.append(control)
int_marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
# Control roll
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "roll"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# Movimiento en 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)
# Control yaw
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "yaw"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# Movimiento en 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)
# Control pitch
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "pitch"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# Movimiento en 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)
self.server.insert(int_marker, self.process_feedback)
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
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 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 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()
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
# 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).
# NOTE(jstn): This will not work anymore, trajectories are
# unsupported for now.
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.
base_pose = world.get_absolute_pose(self.get_target())
# base_pose refers to the wrist link, which looks weird in
# visualizations, so offset_pose is shifted forward to be in the
# "middle" of the gripper
offset_pose = ActionStepMarker._offset_pose(base_pose, 1)
ref_frame = world.get_ref_from_name(self._get_ref_name())
if ref_frame == ArmState.OBJECT:
menu_control.markers.append(
Marker(type=Marker.ARROW,
id=(ID_OFFSET_REF_ARROW + self.get_uid()),
scale=SCALE_OBJ_REF_ARROW,
header=Header(frame_id=BASE_LINK),
color=COLOR_OBJ_REF_ARROW,
points=[
offset_pose.position,
self.get_target().refFrameLandmark.pose.position
]))
# Make and add the text for this step ('Step X').
text_pos = Point()
text_pos.x = offset_pose.position.x
text_pos.y = offset_pose.position.y
text_pos.z = offset_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=BASE_LINK),
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 = BASE_LINK
int_marker.pose = offset_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)
