def __init__(self, parent_instance_handle, server_name, ik_solver):
self.parent_instance_handle = parent_instance_handle
self.server = InteractiveMarkerServer(server_name)
self.ik_solver = ik_solver
self.ik_position_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_position_rpy_bounds = [31416.0, 31416.0, 31416.0] # NOTE: This implements position-only IK
self._menu_handlers = {}
self._menu_cmds = {}
self.menu_handler = MenuHandler()
del_sub_menu_handle = self.menu_handler.insert("Delete Waypoint")
self._menu_cmds['del_wp'] = self.menu_handler.insert("Yes", parent=del_sub_menu_handle, callback=self._process_feedback)
self.menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback)
ins_sub_menu_handle = self.menu_handler.insert("Insert Waypoint")
self._menu_cmds['ins_wp_before'] = self.menu_handler.insert("Before", parent=ins_sub_menu_handle, callback=self._process_feedback)
self._menu_cmds['ins_wp_after'] = self.menu_handler.insert("After", parent=ins_sub_menu_handle, callback=self._process_feedback)
self.menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback)
self._int_marker_name_list = []
self.markers_created_cnt = 0
self.marker_cnt = 0
self.name_to_marker_dict = {}
self.waypoint_to_marker_dict = {}
self.name_to_waypoint_dict = {}
def __init__(self, name, pose, dimensions, db_id):
"""
Constructs a WorldLandmark object.
Use bounding_box() or cloud_box() instead.
Args:
name (str): Name of this landmark
pose (Pose): Pose of the landmark, in base frame
dimensions (Vector3): Size of bounding box
db_id (str): The MongoDB id if this is a cloud box
"""
self._name = name
self._pose = pose
self._dimensions = dimensions
self._db_id = db_id
if self.is_bounding_box():
self.object = Landmark(type=Landmark.TABLE_TOP,
name=name,
pose=pose,
dimensions=dimensions,
db_id='')
elif self.is_cloud_box():
self.object = Landmark(type=Landmark.CLOUD_BOX,
name=name,
pose=pose,
dimensions=dimensions,
db_id=db_id)
self.int_marker = None
self.is_removed = False
# TODO(jstn): Move this outside.
self.menu_handler = MenuHandler()
self.menu_handler.insert('Remove from scene', callback=self.remove)
def __init__(self, root_tips, suffix=u''):
"""
:param root_tips: list containing root->tip tuple for each interactive marker.
:type root_tips: list
:param suffix: the marker will be called 'eef_control{}'.format(suffix)
:type suffix: str
"""
self.enable_self_collision = rospy.get_param(u'~enable_self_collision',
True)
self.giskard = GiskardWrapper()
if len(root_tips) > 0:
self.roots, self.tips = zip(*root_tips)
else:
self.roots = []
self.tips = []
self.suffix = suffix
self.markers = {}
# marker server
self.server = InteractiveMarkerServer(u'eef_control{}'.format(
self.suffix))
self.menu_handler = MenuHandler()
for root, tip in zip(self.roots, self.tips):
int_marker = self.make_6dof_marker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip)
self.server.insert(
int_marker,
self.process_feedback(self.server, int_marker.name, root, tip,
self.giskard,
self.enable_self_collision))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def init_menu(self):
self.menu_handler = MenuHandler()
# Remove Goal entry
self.menu_remove_goal_title = "Remove Goal"
self.menu_remove_goal_handle = self.menu_handler.insert(
self.menu_remove_goal_title, callback=self.process_feedback)
def __init__(self, from_frame, to_frame, position, orientation):
self.server = InteractiveMarkerServer("posestamped_im")
o = orientation
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("Publishing transform and PoseStamped from: " +
from_frame + " to " + to_frame + " at " +
str(position.x) + " " + str(position.y) + " " +
str(position.z) + " and orientation " + str(o.x) + " " +
str(o.y) + " " + str(o.z) + " " + str(o.w) + " or rpy " +
str(r) + " " + str(p) + " " + str(y))
self.menu_handler = MenuHandler()
self.from_frame = from_frame
self.to_frame = to_frame
# Transform broadcaster
self.tf_br = tf.TransformBroadcaster()
self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " +
str(self.pub.name))
pose = Pose()
pose.position = position
pose.orientation = orientation
self.last_pose = pose
self.print_commands(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def create_menu(self):
"""
Create and populates all the menu entries
"""
menu_handler = MenuHandler()
menu_handler.insert("Point the head", callback=self.move_head)
menu_handler.insert("Add position to trajectory",
callback=self.add_point)
menu_handler.insert("Place marker over gripper",
callback=self.place_gripper)
menu_handler.insert("Execute trjectory",
callback=self.execute_trajectory)
menu_handler.insert("Clear trajectory", callback=self.clear_trajectory)
menu_handler.insert("Publish trajectory",
callback=self.publish_trajectory)
menu_handler.insert("Move the arm (planning)", callback=self.plan_arm)
menu_handler.insert("Move the arm (collision-free)",
callback=self.collision_free_arm)
menu_handler.insert(
"Move the arm to trajectory start (collision-free)",
callback=self.arm_trajectory_start)
menu_handler.insert("Update planning scene",
callback=self.update_planning)
menu_handler.apply(self.server, self.int_marker.name)
def create_marker(path_msg, color_msg, description, path_id, width=0.1, delta_z=0.1):
int_marker = InteractiveMarker()
int_marker.header.frame_id = path_msg.header.frame_id
int_marker.name = str(path_id)
int_marker.description = "Path {0}".format(path_id)
# line_marker = Marker()
# line_marker.type = Marker.LINE_STRIP
# line_marker.scale.x = width
# line_marker.color = color_msg
# line_marker.points = [p.pose.position for p in path_msg.poses]
# for point in line_marker.points:
# point.z += delta_z
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MENU
# control.markers.append(line_marker)
points = [node(pose, delta_z) for pose in path_msg.poses]
for p1, p2 in zip(points[:-1], points[1:]):
control.markers.append(cylinder_between(p1, p2, color_msg, width))
for p in points:
control.markers.append(sphere_at(p, color_msg, width))
int_marker.controls.append(copy.deepcopy(control))
menu_handler = MenuHandler()
# put all the information in the main menu
#d = menu_handler.insert("Description")
for line in description:
menu_handler.insert(line)#, parent=d)
return menu_handler, int_marker
def __init__(self, root_link, tip_links):
# tf
self.tfBuffer = Buffer(rospy.Duration(1))
self.tf_listener = TransformListener(self.tfBuffer)
# giskard goal client
# self.client = SimpleActionClient('move', ControllerListAction)
self.client = SimpleActionClient('qp_controller/command',
ControllerListAction)
self.client.wait_for_server()
# marker server
self.server = InteractiveMarkerServer("eef_control")
self.menu_handler = MenuHandler()
for tip_link in tip_links:
int_marker = self.make6DofMarker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link)
self.server.insert(
int_marker,
self.process_feedback(self.server, self.client, root_link,
tip_link))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def _update_menu(self):
'''Recreates the menu when something has changed'''
self._menu_handler = MenuHandler()
frame_entry = self._menu_handler.insert('Reference frame')
self._sub_entries = [None] * len(ActionStepMarker._ref_names)
for i in range(len(ActionStepMarker._ref_names)):
self._sub_entries[i] = self._menu_handler.insert(
ActionStepMarker._ref_names[i],
parent=frame_entry,
callback=self.change_ref_cb)
self._menu_handler.insert('Move arm here', callback=self.move_to_cb)
self._menu_handler.insert('Move to current arm pose',
callback=self.move_pose_to_cb)
self._menu_handler.insert('Delete', callback=self.delete_step_cb)
for i in range(len(ActionStepMarker._ref_names)):
self._menu_handler.setCheckState(self._sub_entries[i],
MenuHandler.UNCHECKED)
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id == None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED)
self._update_viz_core()
self._menu_handler.apply(ActionStepMarker._im_server, self._get_name())
ActionStepMarker._im_server.applyChanges()
def __init__(self):
self.server = InteractiveMarkerServer("posestamped_im")
self.menu_handler = MenuHandler()
# self.pub = rospy.Publisher('/whole_body_kinematic_controler/wrist_right_ft_link_goal', PoseStamped, queue_size=1)
# self.pub = rospy.Publisher('/whole_body_kinematic_controler/arm_right_7_link_goal', PoseStamped, queue_size=1)
self.pub = rospy.Publisher('/pose_to_follow',
PoseStamped,
queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " +
str(self.pub.name))
#self.global_frame_id = 'world'
self.global_frame_id = 'base_footprint'
pose = Pose()
pose.position.x = 0.2
# pose.position.y = 0.35
pose.position.y = -0.35
#pose.position.z = 0.1
pose.position.z = 1.1
pose.orientation.w = 1.0
# pose.orientation.x = 0.35762
# pose.orientation.y = 0.50398
# pose.orientation.z = 0.45213
# pose.orientation.w = 0.64319
#self.makeMenuMarker(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def __init__(self, server):
self.server = server
self.pose = Pose()
self.publish = True
self.make_marker()
self.menu_handler = MenuHandler()
item = self.menu_handler.insert("publish", callback=self.menu_callback)
self.menu_handler.setCheckState(item, MenuHandler.CHECKED)
self.menu_handler.apply(self.server, self.marker_name)
def __init__(self, server, id_, name, initial_pose=None):
self._menu_handler = MenuHandler()
self._menu_handler.insert("Delete", callback=self.deleteCallback)
self._server = server
self._key = name + str(id_)
self._marker_map = {}
self._callback_map = {}
# not all templates will have a dynamic reconfigure server
self._dserver = None
self._initial_pose = initial_pose
def __init__(self, pose, index, dimensions, is_recognized):
''' Initialization of objects'''
self.index = index
self.assigned_name = None
self.is_recognized = is_recognized
self.object = Object(Object.TABLE_TOP, self.get_name(), pose,
dimensions)
self.menu_handler = MenuHandler()
self.int_marker = None
self.is_removed = False
self.menu_handler.insert('Remove from scene', callback=self.remove)
def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here',
callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
# Code for moving the robots joints
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
def __init__(self, server):
self.server = server
self.pose = Pose()
self.active = True
self.covariance = 0.1
self.int_marker = None
self.make_marker()
self.menu_handler = MenuHandler()
item = self.menu_handler.insert("active", callback=self.menu_callback)
self.menu_handler.setCheckState(item, MenuHandler.CHECKED)
high_conf = self.menu_handler.insert("high confidence", callback=self.conf_callback)
self.menu_handler.setCheckState(high_conf, MenuHandler.CHECKED)
self.menu_handler.apply(self.server, self.marker_name)
def _update_menu(self):
"""Recreates the menu when something has changed."""
self._menu_handler = MenuHandler()
# Insert sub entries.
self._sub_entries = []
self._menu_options = []
frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref'])
refs = ActionStepMarker._ref_names
for ref in refs:
if ref != BASE_LINK and "Obj" not in str(ref):
subent = self._menu_handler.insert(ref + ' center',
parent=frame_entry,
callback=self.subent_cb)
self._sub_entries += [subent]
self._menu_options += [subent]
elif ref == BASE_LINK:
subent = self._menu_handler.insert(ref,
parent=frame_entry,
callback=self.subent_cb)
self._sub_entries += [subent]
self._menu_options += [subent]
select_one = self._menu_handler.insert('Select an object side',
parent=frame_entry,
callback=self.select_one_cb)
self._menu_options += [select_one]
# Inset main menu entries.
self._menu_handler.insert(MENU_OPTIONS['move_here'],
callback=self.move_to_cb)
self._menu_handler.insert(MENU_OPTIONS['move_current'],
callback=self.move_pose_to_cb)
self._menu_handler.insert(MENU_OPTIONS['del'],
callback=self.delete_step_cb)
# Make all unchecked to start.
for subent in self._sub_entries:
self._menu_handler.setCheckState(subent, MenuHandler.UNCHECKED)
self._menu_handler.setCheckState(select_one, MenuHandler.UNCHECKED)
# Check if necessary. Set base_link to checked
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id is None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED)
# Update.
self._update_viz_core()
self._menu_handler.apply(ActionStepMarker._im_server, self._get_name())
ActionStepMarker._im_server.applyChanges()
def __init__(self, side_prefix):
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers_' +
self.side_prefix)
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here',
callback=self.move_to_pose_cb)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
# Create a trajectory action client
r_traj_contoller_name = None
l_traj_contoller_name = None
if self.side_prefix == 'r':
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(
r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory ' +
'action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
else:
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(
l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory ' +
'action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.ik = IK(side_prefix)
self.update_viz()
self._menu_handler.apply(self._im_server,
'ik_target_marker_' + self.side_prefix)
self._im_server.applyChanges()
print self.ik
def init(self):
# The parameter /robot_description must exist
if not rospy.has_param('/robot_description'):
rospy.logerr(
"Parameter '/robot_description' must exist to continue")
sys.exit(1)
self.config = loadJSONConfig(self.args['calibration_file'])
if self.config is None:
sys.exit(1) # loadJSON should tell you why.
# self.config = CalibConfig()
# ok = self.config.loadJSON(self.args['calibration_file'])
# # Exit if it fails to open a valid calibration file
# if not ok:
# sys.exit(1)
# Load the urdf from /robot_description
self.urdf = URDF.from_parameter_server()
# ok = validateLinks(self.config.world_link, self.config.sensors, self.urdf)
# if not ok:
# sys.exit(1)
print('Number of sensors: ' + str(len(self.config['sensors'])))
# Init interaction
self.server = InteractiveMarkerServer("interactive_first_guess")
self.menu = MenuHandler()
self.menu.insert("Save sensors configuration",
callback=self.onSaveFirstGuess)
self.menu.insert("Reset to initial configuration",
callback=self.onReset)
# For each node generate an interactive marker.
for name, sensor in self.config['sensors'].items():
print(Fore.BLUE + '\nSensor name is ' + name + Style.RESET_ALL)
params = {
"frame_world": self.config['world_link'],
"frame_opt_parent": sensor['parent_link'],
"frame_opt_child": sensor['child_link'],
"frame_sensor": sensor['link'],
"marker_scale": self.args['marker_scale']
}
# Append to the list of sensors
self.sensors.append(Sensor(name, self.server, self.menu, **params))
self.server.applyChanges()
def __init__(self, side_prefix):
if GripperMarkers._tf_listener is None:
GripperMarkers._tf_listener = TransformListener()
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer(side_prefix + '_ik_request_markers')
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.marker_pose = self._offset_pose(self.get_ee_pose(), -GripperMarkers._offset)
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def __init__(self, server, cam_info):
self.cam_info = cam_info
self.marker_name = "goal"
self.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
self.absolute_publisher = rospy.Publisher("goal_absolute",
PoseWithCertaintyArray,
queue_size=1)
self.relative_publisher = rospy.Publisher("goal_relative",
PoseWithCertaintyArray,
queue_size=1)
self.relative_posts_publisher = rospy.Publisher("goal_posts_relative",
PoseWithCertaintyArray,
queue_size=1)
super(GoalMarker, self).__init__(server)
self.menu_handler = MenuHandler()
self.pose.position.x = 3.0
def __init__(self, pose, index, dimensions, is_recognized):
'''
Args:
pose (Pose): Position of bounding box
index (int): For naming object in world (e.g. "thing 0")
dimensions (Vector3): Size of bounding box
is_recognized (bool): Result of object recognition.
'''
self.index = index
self.assigned_name = None
self.is_recognized = is_recognized
self.object = Landmark(
Landmark.TABLE_TOP, self.get_name(), pose, dimensions)
self.menu_handler = MenuHandler()
self.int_marker = None
self.is_removed = False
self.menu_handler.insert('Remove from scene', callback=self.remove)
def update(self):
'''Updates marker/arm loop'''
self._menu_handler = MenuHandler()
# Inset main menu entries.
self._menu_handler.insert('Move gripper here',
callback=self._move_to_cb)
self._menu_handler.insert('Move marker to current gripper pose',
callback=self._move_pose_to_cb)
if self._is_hand_open():
self._menu_handler.insert('Close gripper',
callback=self._close_gripper_cb)
else:
self._menu_handler.insert('Open gripper',
callback=self._open_gripper_cb)
frame_id = REF_FRAME
pose = self.get_pose()
# if self._marker_moved() or self._menu_control is None:
rospy.loginfo("Marker moved")
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control = self._make_gripper_marker(menu_control,
self._is_hand_open())
self._menu_control = menu_control
# Make and add interactive marker.
int_marker = InteractiveMarker()
int_marker.name = self._get_name()
int_marker.header.frame_id = frame_id
int_marker.pose = pose.pose
int_marker.scale = INT_MARKER_SCALE
self._add_6dof_marker(int_marker, True)
int_marker.controls.append(self._menu_control)
ArmControlMarker._im_server.insert(int_marker,
self._marker_feedback_cb)
self._menu_handler.apply(ArmControlMarker._im_server, self._get_name())
ArmControlMarker._im_server.applyChanges()
def __init__(self, topic, initial_x, initial_y, initial_z, initial_roll,
initial_pitch, initial_yaw, frame_id):
self.server = InteractiveMarkerServer("posestamped_im")
self.menu_handler = MenuHandler()
self.pub = rospy.Publisher(topic, PoseStamped, queue_size=1)
rospy.loginfo("Publishing posestampeds at topic: " +
str(self.pub.resolved_name))
pose = Pose()
pose.position.x = initial_x
pose.position.y = initial_y
pose.position.z = initial_z
quat = quaternion_from_euler(initial_roll, initial_pitch, initial_yaw)
pose.orientation = Quaternion(*quat)
self.initial_pose = pose
self.frame_id = frame_id
# self.makeMenuMarker(pose)
self.makeGraspIM(pose)
self.server.applyChanges()
def start_once(self):
# giskard goal client
self.client = SimpleActionClient(u'qp_controller/command', MoveAction)
self.client.wait_for_server()
# marker server
self.server = InteractiveMarkerServer(u'eef_control{}'.format(
self.suffix))
self.menu_handler = MenuHandler()
all_goals = {}
for root, tip in zip(self.roots, self.tips):
int_marker = self.make_6dof_marker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip)
self.server.insert(
int_marker,
self.process_feedback(self.server, int_marker.name,
self.client, root, tip, all_goals))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def __init__(self, server, name, X_WM, markers):
self._server = server
self.name = name
self._X_WM = X_WM
self._menu = MenuHandler()
self._edit_id = self._menu.insert("Edit", callback=self._edit_callback)
self._delete_id = self._menu.insert("Delete", callback=self._delete_callback)
self._menu.insert(self.name, callback=noop) # For annotation.
self._t_click_prev = -float('inf')
# Make template marker.
template = InteractiveMarker()
template.name = self.name
template.description = name
template.header.frame_id = "world"
viz = InteractiveMarkerControl()
viz.interaction_mode = InteractiveMarkerControl.BUTTON
viz.always_visible = True
viz.markers.extend(markers)
template.controls.append(viz)
self._template = template
# Initialize.
self._edit = False
self._update()
def _update_menu(self):
'''Recreates the menu when something has changed.'''
self._menu_handler = MenuHandler()
# Insert sub entries.
self._sub_entries = []
frame_entry = self._menu_handler.insert(MENU_OPTIONS['ref'])
refs = ActionStepMarker._ref_names
for ref in refs:
subent = self._menu_handler.insert(ref,
parent=frame_entry,
callback=self.change_ref_cb)
self._sub_entries += [subent]
# Inset main menu entries.
self._menu_handler.insert(MENU_OPTIONS['move_here'],
callback=self.move_to_cb)
self._menu_handler.insert(MENU_OPTIONS['move_current'],
callback=self.move_pose_to_cb)
self._menu_handler.insert(MENU_OPTIONS['del'],
callback=self.delete_step_cb)
# Make all unchecked to start.
for subent in self._sub_entries:
self._menu_handler.setCheckState(subent, MenuHandler.UNCHECKED)
# Check if necessary.
menu_id = self._get_menu_id(self._get_ref_name())
if menu_id is None:
self.has_object = False
else:
self._menu_handler.setCheckState(menu_id, MenuHandler.CHECKED)
# Update.
self._update_viz_core()
self._menu_handler.apply(ActionStepMarker._im_server, self._get_name())
ActionStepMarker._im_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()
# ------------------------
# IMPORT MODULES #
# ------------------------
import argparse
import rospkg
import rospy
from visualization_msgs.msg import InteractiveMarkerControl, Marker, InteractiveMarker
from interactive_markers.interactive_marker_server import InteractiveMarkerServer
from interactive_markers.menu_handler import MenuHandler
from interactive_calibration.data_collector_and_labeler import DataCollectorAndLabeler
server = None
menu_handler = MenuHandler()
def menuFeedback(feedback):
handle = feedback.menu_entry_id
if handle == 1: # collect snapshot
print('Save collection selected')
data_collector.saveCollection()
def initMenu():
menu_handler.insert("Save collection", callback=menuFeedback)
def createInteractiveMarker(world_link):
marker = InteractiveMarker()
def add_marker(self, initial_pose, frame="base_link", description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.pose.position = initial_pose.position
int_marker.scale = 0.1
int_marker.name = rospy.get_name() + str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.name_to_position_only_flag[int_marker.name] = True
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
pos_opt_handle = menu_handler.insert("Position-Only Ctrl", callback=self._process_feedback)
self._menu_cmds['position_only'] = pos_opt_handle
menu_handler.setCheckState(pos_opt_handle, MenuHandler.CHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
self._int_marker_name_list.append(int_marker.name)
return int_marker.name
def add_waypoint_marker(self, waypoint, description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.pose.position = waypoint.end_effector_pose.position
int_marker.scale = 0.1
int_marker.name = str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.waypoint_to_marker_dict[waypoint] = int_marker
self.name_to_waypoint_dict[int_marker.name] = waypoint
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
del_sub_menu_handle = menu_handler.insert("Delete Waypoint")
self._menu_cmds['del_wp'] = menu_handler.insert("Yes", parent=del_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("No", parent=del_sub_menu_handle, callback=self._process_feedback)
ins_sub_menu_handle = menu_handler.insert("Insert Waypoint")
self._menu_cmds['ins_wp_before'] = menu_handler.insert("Before", parent=ins_sub_menu_handle,
callback=self._process_feedback)
self._menu_cmds['ins_wp_after'] = menu_handler.insert("After", parent=ins_sub_menu_handle,
callback=self._process_feedback)
menu_handler.insert("Cancel", parent=ins_sub_menu_handle, callback=self._process_feedback)
# Set time
time_sub_menu_handle = menu_handler.insert("Set Waypoint Time")
menu_time_cmds = {}
time = self.name_to_waypoint_dict[int_marker.name].time
time_list = [float(num) for num in range((int(m.ceil(time))))]
time_list.append(time)
time_list.extend([m.floor(time) + cnt + 1 for cnt in range(6)])
self._menu_cmds[int_marker.name] = {"menu_time_cmds": menu_time_cmds}
for t in time_list:
time_opt_handle = menu_handler.insert(str(t), parent=time_sub_menu_handle, callback=self._process_feedback)
menu_time_cmds[time_opt_handle] = t
if t == time:
menu_handler.setCheckState(time_opt_handle, MenuHandler.CHECKED)
else:
menu_handler.setCheckState(time_opt_handle, MenuHandler.UNCHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
if int(int_marker.description)-1 < len(self._int_marker_name_list):
self._int_marker_name_list.insert(int(int_marker.description) - 1, int_marker.name)
else:
self._int_marker_name_list.append(int_marker.name)
