def add_menu_handler(int_marker, menu_handler, server):
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MENU
control.description="Options"
control.name = "menu_only_control"
int_marker.controls.append(control)
menu_handler.apply(server, int_marker.name)
server.applyChanges()
def add_menu_handler(int_marker, menu_handler, server):
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MENU
control.description = "Options"
control.name = "menu_only_control"
int_marker.controls.append(control)
menu_handler.apply(server, int_marker.name)
server.applyChanges()
def add_new_marker(self,
name,
body,
callback,
selected=True,
delay_update=False,
frame=None,
pose=None):
"""Adds a new interactive marker to the node.
:param name: Name of the marker
:type name: str
:param body: Body controlled by the marker
:type body: RigidBody, MultiBody
:param callback: Callback function for the marker
:type callback: function
:param selected: Should the marker be selected upon creation
:type selected: bool
:param delay_update: Delay the update to the server
:type delay_update: bool
:param frame: Name of reference frame
:type frame: NoneType, str
:param pose: Initial pose of the marker
:type pose: NoneType, Frame
"""
intMarker = InteractiveMarker()
intMarker.name = name
intMarker.header.frame_id = frame if frame is not None else name
intMarker.header.stamp = rospy.Time(0)
intMarker.scale = 1.0
if pose is not None:
intMarker.pose.position.x = pose.position[0]
intMarker.pose.position.y = pose.position[1]
intMarker.pose.position.z = pose.position[2]
intMarker.pose.orientation.x = pose.quaternion[0]
intMarker.pose.orientation.y = pose.quaternion[1]
intMarker.pose.orientation.z = pose.quaternion[2]
intMarker.pose.orientation.w = pose.quaternion[3]
control = InteractiveMarkerControlMsg()
control.interaction_mode = InteractiveMarkerControlMsg.MOVE_3D
control.always_visible = True
control.orientation.w = 1.0
control.name = 'visual'
control.description = name
intMarker.controls.append(control)
make6DOFGimbal(intMarker)
self.create_visual_marker(name, body, intMarker, control)
activateMarker(intMarker, selected)
self.marker_server.insert(intMarker, callback)
self.markers[name] = (intMarker, callback)
body.register_deletion_cb(self.on_obj_deleted)
if not delay_update:
self.marker_server.applyChanges()
def __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 __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 __init__(self):
#self.client = actionlib.SimpleActionClient('lookat_action', cob_lookat_action.msg.LookAtAction)
#print "Waiting for Server..."
##self.client.wait_for_server()
#print "...done!"
print "Waiting for StartTrackingServer..."
rospy.wait_for_service('/lookat_controller/start_tracking')
print "...done!"
self.start_tracking_client = rospy.ServiceProxy('/lookat_controller/start_tracking', SetString)
print "Waiting for StopTrackingServer..."
rospy.wait_for_service('/lookat_controller/stop_tracking')
print "...done!"
self.stop_tracking_client = rospy.ServiceProxy('/lookat_controller/stop_tracking', Empty)
self.target_pose = PoseStamped()
self.target_pose.header.stamp = rospy.Time.now()
self.target_pose.header.frame_id = "base_link"
self.target_pose.pose.orientation.w = 1.0
#self.viz_pub = rospy.Publisher('lookat_target', PoseStamped)
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
transform_available = False
while not transform_available:
try:
(trans,rot) = self.listener.lookupTransform('/base_link', '/odometry_monitor_target', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn("Waiting for transform...")
rospy.sleep(0.5)
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.ia_server = InteractiveMarkerServer("marker_server")
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = "base_link"
self.int_marker.pose = self.target_pose.pose
self.int_marker.name = "lookat_target"
self.int_marker.description = "virtual lookat target"
self.int_marker.scale = 0.5
# 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
#box_control = InteractiveMarkerControl()
#box_control.always_visible = True
#box_control.markers.append( box_marker )
#self.int_marker.controls.append(box_control)
#control = InteractiveMarkerControl()
#control.always_visible = True
#control.orientation.w = 1
#control.orientation.x = 1
#control.orientation.y = 0
#control.orientation.z = 0
#control.name = "move_3D"
#control.interaction_mode = InteractiveMarkerControl.MOVE_3D
#self.int_marker.controls.append(deepcopy(control))
#control.name = "move_x"
#control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
#self.int_marker.controls.append(deepcopy(control))
#control.name = "rotate_x"
#control.interaction_mode = InteractiveMarkerControl.ROTATE_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 = "rotate_y"
#control.interaction_mode = InteractiveMarkerControl.ROTATE_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))
#control.name = "rotate_z"
#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( "Lookat", callback=self.lookat )
self.menu_handler.insert( "StartTracking", callback=self.start_tracking )
self.menu_handler.insert( "StopTracking", callback=self.stop_tracking )
self.int_marker_menu = InteractiveMarker()
self.int_marker_menu.header.frame_id = "base_link"
self.int_marker_menu.name = "lookat_menu"
self.int_marker_menu.description = "Menu"
self.int_marker_menu.scale = 1.0
self.int_marker_menu.pose.position.z = 1.2
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MENU
control.description="Lookat"
control.name = "menu_only_control"
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()
