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):
self.goto_pub = rospy.Publisher('/move_base_simple/goal', geometry_msgs.msg.PoseStamped)
self.pose_pub = rospy.Publisher('/pose_names', map_annotator.msg.PoseNames, latch=True)
rospy.Subscriber('/amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped, self._handle_pose_callback)
self.names = {}
filename = "/home/team3/catkin_ws/src/cse481c/map_annotator/simstations.pickle"
#filename = rospy.get_param('~pose_file')
try:
with open(filename, "r") as f:
self.names = pickle.load(f)
except EOFError:
self.names = {}
except IOError:
self.names = {}
self.pose = None
self.server = InteractiveMarkerServer('simple_marker')
self.name_markers = {}
self.name_controls = {}
pose_message = map_annotator.msg.PoseNames()
pose_message.poses = list(self.names.keys())
self.pose_pub.publish(pose_message)
print("initiated actuator nodes with", self.names, "from", filename)
def __init__(self, network_file):
self.network_file = network_file
with open(network_file, 'r') as file_obj:
network = yaml.safe_load(file_obj)
self.nodes = {
node['id']: Node.from_dict(node)
for node in network['nodes']
}
self.connections = network['connections']
self.node_id_counter = max(self.nodes.keys())
# ros params
self.frame = rospy.get_param('frame', 'map')
self._topology_pub = rospy.Publisher('/topology',
MarkerArray,
queue_size=1)
self.interactive_marker_server = InteractiveMarkerServer(
"two_dof_marker")
rospy.Subscriber('/clicked_point', PointStamped, self.clicked_point_cb)
rospy.Subscriber('~command', String, self.command_cb)
rospy.sleep(1.0)
self._topology_pub.publish(self.get_marker_array())
rospy.sleep(1.0)
for node in self.nodes.values():
self._add_interactive_marker(node.x, node.y, node.id)
rospy.loginfo('Initialised')
def __init__(self):
# Public attributes
if World.tf_listener is None:
World.tf_listener = TransformListener()
self.surface = None
# Private attributes
self._lock = threading.Lock()
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer(TOPIC_IM_SERVER)
rospy.wait_for_service(SERVICE_BB)
self._bb_service = rospy.ServiceProxy(
SERVICE_BB, FindClusterBoundingBox)
self._object_action_client = actionlib.SimpleActionClient(
ACTION_OBJ_DETECTION, UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo(
'Interactive object detection action server has responded.')
# Setup other ROS machinery
rospy.Subscriber(
TOPIC_OBJ_RECOGNITION, GraspableObjectList,
self.receive_object_info)
rospy.Subscriber(TOPIC_TABLE_SEG, Marker, self.receive_table_marker)
# Init
self.clear_all_objects()
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.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, rate=30.0, filename=None):
# Marker server
self.server = InteractiveMarkerServer('camera_marker')
# TF broadcaster
self.tf = TransformBroadcaster()
# Marker pose
self.pose_mutex = Lock()
self.marker_position = (0.0, 0.0, 0.0)
self.marker_orientation = (0.0, 0.0, 0.0, 1.0)
# Load init position
self.filename = filename
if self.filename:
with open(self.filename, 'r') as stream:
init_data = yaml.load(stream)['init_position']
self.marker_position = (init_data['x'], init_data['y'],
init_data['z'])
self.marker_orientation = (0.0, 0.0, 0.0, 1.0)
# Register shutdown callback
rospy.on_shutdown(self.shutdown)
# Add marker
self.add_6DOF()
# Timer for TF broadcaster
rospy.Timer(rospy.Duration(1.0 / rate), self.publish_transform)
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__(self, name="interactive_markers"):
self.server = InteractiveMarkerServer(name)
self.mg = MarkerGenerator()
self.mg.type = 1
self.mg.scale = [.25] * 3
self.c = 0
self.markers = {}
def main(args):
server = InteractiveMarkerServer("guess_markers")
guess = Guess()
for i in range(len(guess.items)):
server.insert(guess.markers[i], guess.process_feedback)
server.applyChanges()
rospy.spin()
def __init__(self):
rospy.init_node('look_at_marker_node')
self._server = InteractiveMarkerServer('look_at_marker_server')
self.makeQuadrocopterMarker(Point(3, 0, 1))
self.makeStaticMarker(Point(0, 0, 1))
self._tf_listener = tf.TransformListener()
self._head = robot_api.FullBodyLookAt(tf_listener=self._tf_listener)
self._look_at_target = None
def __init__(self, marker_name):
host = rospy.get_param("mongodb_host")
port = rospy.get_param("mongodb_port")
self._client = pymongo.MongoClient(host, port)
self._traj = dict()
self._retrieve_logs(marker_name)
self._server = InteractiveMarkerServer(marker_name)
def main():
rospy.init_node('mmmm')
# Initializing interactive markers
marker_server = InteractiveMarkerServer("simple_server")
monitor = InteractiveMarkerMonitor(marker_server)
marker_server.applyChanges()
rospy.spin()
def __init__(self, setup, world_frame):
super(IntCollisionEnv, self).__init__(setup, world_frame)
self.im_server = InteractiveMarkerServer(self.NS + "markers")
self.check_attached_timer = rospy.Timer(rospy.Duration(0.1),
self.check_attached_timer_cb)
self.create_menus()
def main():
rospy.init_node('gripper_teleop')
arm = Arm()
gripper = Gripper()
im_server = InteractiveMarkerServer('gripper_im_server')
auto_pick_im_server = InteractiveMarkerServer('auto_pick_im_server')
teleop = GripperTeleop(arm, gripper, im_server)
auto_pick = AutoPickTeleop(arm, gripper, auto_pick_im_server)
teleop.start()
auto_pick.start()
rospy.spin()
def __init__(self, server_ns='interactive_markers'):
self.server = InteractiveMarkerServer(server_ns)
self._menu_handlers = {}
self._menu_cmds = {}
self.markers_created_cnt = 0
self.marker_cnt = 0
self._int_marker_name_list = []
self.name_to_marker_dict = {}
self.name_to_position_only_flag = {}
def __init__(self):
self.transform = None
super(InteractivePose2DTF, self).__init__()
self.pub_tf = tf2_ros.TransformBroadcaster()
self.interact = InteractiveMarkerServer("interactive_markers")
self.marker_pose = Pose()
self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5))
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, robot, *robotargs):
super(PickAndPlaceNode, self).__init__('pp_node')
rospy.loginfo("PickAndPlaceNode")
_post_perceive_trans = defaultdict(lambda: self._pick)
_post_perceive_trans.update({'c': self._calibrate})
_preplace = defaultdict(lambda: self._preplace)
self.transition_table = {
# If calibration has already happened, allow skipping it
'initial': {'c': self._calibrate, 'q': self._perceive,
's': self._preplace},
'calibrate': {'q': self._perceive, 'c': self._calibrate},
'perceive': {'p': self._post_perceive, 'q': self._perceive, 's': self._stop_perceive, 'c': self._calibrate},
'post_perceive': _post_perceive_trans,
'postpick': {'1': self._level, '2': self._level, '9': self._level},
'level': _preplace,
'preplace': {'s': self._place},
'place': {'q': self._perceive, 'c': self._calibrate}
}
rospy.loginfo("PickAndPlaceNode1")
if callable(robot):
self.robot = robot(*robotargs)
else:
self.robot = robot
self.robot.level = 1
rospy.loginfo("PickAndPlaceNode2")
# Hardcoded place for now
self.place_pose = PoseStamped(
Header(0, rospy.Time(0), self.robot.base),
Pose(Point(0.526025806, 0.4780144, -0.161326153),
Quaternion(1, 0, 0, 0)))
self.tl = tf.TransformListener()
self.num_objects = 0
# Would this work too? Both tf and tf2 have (c) 2008...
# self.tf2 = tf2_ros.TransformListener()
self.n_objects_sub = rospy.Subscriber("/num_objects", Int64,
self.update_num_objects,
queue_size=1)
self.perception_pub = rospy.Publisher("/perception/enabled",
Bool,
queue_size=1)
self.alignment_pub = rospy.Publisher("/alignment/doit",
Bool,
queue_size=1)
self.br = tf.TransformBroadcaster()
rospy.loginfo("PickAndPlaceNode3")
self.int_marker_server = InteractiveMarkerServer('int_markers')
# Dict to map imarker names and their updated poses
self.int_markers = {}
# Ideally this call would be in a Factory/Metaclass/Parent
self.show_options()
self.perceive = False
class AbstractInteractiveMarker(object):
def __init__(self, name, X_init, box_scale=0.1):
self.name = name
self.X = X_init
self.marker = createBoxMarker(scale=box_scale)
self.interactive_marker = create6DofInteractive('/map', name, name,
X_init, self.marker)
self.marker_server = InteractiveMarkerServer(name + 'server')
self.marker_server.insert(self.interactive_marker, self.markerMoved)
self.marker_server.applyChanges()
def markerMoved(self, feedback):
self.X = transform.fromPose(feedback.pose)
def __init__(self):
super(RvizPathServer, self).__init__()
rospy.init_node("traversability_rviz_paths_node")
self.server = InteractiveMarkerServer("paths")
self.paths = {}
self.delta_z = rospy.get_param('~offset', 0.15)
self.width = rospy.get_param('~width', 0.15)
self.pub = rospy.Publisher("selected_path", NavPath, queue_size=1)
rospy.Subscriber("paths", Paths, self.updatePaths, queue_size=1)
# self.add_marker(test_msg(), 0)
# self.server.applyChanges()
rospy.spin()
def __init__(self):
'''
Create a POIServer
'''
# TF bootstrap
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# Radius of interactive marker for POIs
self.marker_scale = rospy.get_param("~marker_scale", 0.5)
# Create intial empty POI array
self.pois = POIArray()
# Get the global frame to be used
self.global_frame = rospy.get_param("~global_frame", "map")
self.pois.header.frame_id = self.global_frame
# Create publisher to notify clients of updates and interactive marker server
self.pub = rospy.Publisher("points_of_interest",
POIArray,
queue_size=1,
latch=True)
self.interactive_marker_server = InteractiveMarkerServer(
"points_of_interest")
# Load initial POIs from params
if rospy.has_param('~initial_pois'):
pois = rospy.get_param('~initial_pois')
assert isinstance(pois, dict)
for key, value in pois.iteritems():
assert type(key) == str
assert type(value) == list
assert len(value) == 3
name = key
position = numpy_to_point(value)
self._add_poi(name, position)
# Update clients / markers of changes from param
self.update()
# Create services to add / delete / move a POI
self.add_poi_server = rospy.Service('~add', AddPOI, self.add_poi_cb)
self.delete_poi_server = rospy.Service('~delete', DeletePOI,
self.delete_poi_cb)
self.move_poi_service = rospy.Service('~move', MovePOI,
self.move_poi_cb)
self.save_to_param = rospy.Service("~save_to_param", Trigger,
self.save_to_param_cb)
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, whicharm):
self.whicharm = whicharm
self.robot_state = pr2_control_utilities.RobotState()
self.joint_controller = pr2_control_utilities.PR2JointMover(
self.robot_state)
self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller)
self.server = InteractiveMarkerServer("~trajectory_markers")
self.tf_listener = self.planner.tf_listener
self.visualizer_pub = rospy.Publisher("~trajectory_markers_path",
MarkerArray)
self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray)
rospy.Subscriber("~overwrite_trajectory", PoseArray,
self.overwrite_trajectory)
rospy.Service("~execute_trajectory", Empty,
self.execute_trajectory_srv)
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link"
int_marker.pose.position.x = 0.5
int_marker.pose.position.z = 1.0
int_marker.name = "move_" + whicharm + "_arm"
int_marker.description = "Move the " + whicharm + " arm"
int_marker.scale = 0.2
self.server.insert(int_marker, self.main_callback)
# create the main marker shape
#color = (1,0,0,1)
color = None
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker)
#add the controls
utils.make_6DOF_marker(int_marker)
self.int_marker = int_marker
self.create_menu()
self.server.applyChanges()
self.trajectory = PoseArray()
self.trajectory.header.frame_id = "/base_link"
self.last_gripper_pose = None
if whicharm == "right":
self.ik_utils = self.planner.right_ik
else:
self.ik_utils = self.planner.left_ik
rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
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 main():
rospy.init_node('gripper_teleop')
wait_for_time()
arm = fetch_api.Arm()
gripper = fetch_api.Gripper()
im_server = InteractiveMarkerServer('gripper_im_server', q_size=10)
teleop = GripperTeleop(arm, gripper, im_server)
teleop.start()
pp_im_server = InteractiveMarkerServer('pick_place_im_server', q_size=10)
pick_place = PickPlaceTeleop(arm, gripper, pp_im_server)
pick_place.start()
rospy.spin()
def __init__(self):
if World.tf_listener == None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer('world_objects')
bb_service_name = 'find_cluster_bounding_box'
rospy.wait_for_service(bb_service_name)
self._bb_service = rospy.ServiceProxy(bb_service_name,
FindClusterBoundingBox)
rospy.Subscriber('interactive_object_recognition_result',
GraspableObjectList, self.receive_object_info)
self._object_action_client = actionlib.SimpleActionClient(
'object_detection_user_command', UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo('Interactive object detection action ' +
'server has responded.')
self.clear_all_objects()
# The following is to get the table information
rospy.Subscriber('tabletop_segmentation_markers',
Marker, self.receive_table_marker)
rospy.Subscriber("ar_pose_marker",
AlvarMarkers, self.receive_ar_markers)
self.marker_dims = Vector3(0.04, 0.04, 0.01)
def main():
global pose_marker_server
global nav_server
global target_pose_pub
global pose_names_pub
rospy.init_node('web_teleop_navigation')
wait_for_time()
pose_marker_server = InteractiveMarkerServer('simple_marker')
pose_names_pub = rospy.Publisher('/pose_names',
PoseNames,
queue_size=10,
latch=True)
nav_server = NavigationServer()
# Create
nav_server.loadMarkers()
for name in nav_server.pose_names_list:
marker = PoseMarker(pose_marker_server, name,
nav_server.pose_names_list[name])
message = PoseNames()
message.names = nav_server.pose_names_list
pose_names_pub.publish(message)
user_actions_sub = rospy.Subscriber('/user_actions', UserAction,
handle_user_actions)
rospy.spin()
def main():
rospy.init_node("rviz_stuff")
server = InteractiveMarkerServer("rviz_stuff")
cube = assign(
Marker(), type=Marker.CUBE,
scale=assign(x=0.1, y=0.1, z=0.1),
color=assign(r=1, g=0, b=0, a=1),
pose=assign(Pose(), position=assign(x=0.1, y=0, z=0)),
)
sphere = assign(
Marker(), type=Marker.SPHERE,
scale=assign(x=0.1, y=0.1, z=0.1),
color=assign(r=1, g=0, b=0, a=1),
pose=assign(Pose(), position=assign(x=-0.1, y=0.1, z=0)),
)
X_WA = assign(Pose(), position=assign(x=1, y=0, z=1), orientation=assign(w=1, x=0, y=0, z=0))
a = SimpleInteractiveMarker(server, "a", X_WA, [cube])
X_WB = assign(Pose(), position=assign(x=1, y=1, z=2), orientation=assign(w=1, x=0, y=0, z=0))
b = SimpleInteractiveMarker(server, "b", X_WB, [cube, sphere])
b.set_pose(assign(Pose(), position=assign(x=-1)))
X_WC = Pose()
c = SimpleInteractiveMarker(server, "c", X_WC, [sphere])
c.delete()
c.delete()
rospy.spin()
def main():
rospy.init_node('map_annotator_server')
myargv = rospy.myargv()
if len(myargv) < 2:
print 'Usage: rosrun map_annotator server.py path/to/db'
return
db_path = myargv[1]
wait_for_time()
database = PoseDatabase(db_path)
list_pub = rospy.Publisher('map_annotator/pose_names',
PoseNames,
latch=True,
queue_size=1)
im_server = InteractiveMarkerServer('map_annotator/map_poses')
marker_server = PoseMarkers(database, im_server)
move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
server = Server(database, list_pub, marker_server, move_base)
user_action_sub = rospy.Subscriber('map_annotator/user_actions',
UserAction, server.handle_user_action)
rospy.sleep(0.5)
marker_server.start()
server.start()
def handle_shutdown():
pn = PoseNames()
list_pub.publish(pn)
database.save()
rospy.on_shutdown(handle_shutdown)
rospy.spin()
def main():
rospy.init_node('map_annotator')
server = InteractiveMarkerServer("simple_server")
pose_ctrl = PoseController()
marker_ctrl = MarkerController(server, pose_ctrl)
monitor = MapAnnotatorMonitor(pose_ctrl, marker_ctrl)
rospy.spin()
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 init_int_marker(self):
self.ims = InteractiveMarkerServer("simple_marker")
self.im = InteractiveMarker()
self.im.header.frame_id = "/ned"
self.im.name = "my_marker"
self.im.description = "Simple 1-DOF control"
bm = Marker()
bm.type = Marker.CUBE
bm.scale.x = bm.scale.y = bm.scale.z = 0.45
bm.color.r = 0.0
bm.color.g = 0.5
bm.color.b = 0.5
bm.color.a = 1.0
bc = InteractiveMarkerControl()
bc.always_visible = True
bc.markers.append(bm)
self.im.controls.append(bc)
rc = InteractiveMarkerControl()
rc.name = "move_x"
rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.im.controls.append(rc)
self.ims.insert(self.im, self.process_marker_feedback)
self.ims.applyChanges()
def __init__(self, rate = 30.0, filename = None):
# Marker server
self.server = InteractiveMarkerServer('camera_marker')
# TF broadcaster
self.tf = TransformBroadcaster()
# Marker pose
self.pose_mutex = Lock()
self.marker_position = (0.0, 0.0, 0.0)
self.marker_orientation = (0.0, 0.0, 0.0, 1.0)
# Load init position
self.filename = filename
if self.filename:
with open(self.filename, 'r') as stream:
init_data = yaml.load(stream)['init_position']
self.marker_position = (init_data['x'], init_data['y'], init_data['z'])
self.marker_orientation = (0.0, 0.0, 0.0, 1.0)
# Register shutdown callback
rospy.on_shutdown(self.shutdown)
# Add marker
self.add_6DOF()
# Timer for TF broadcaster
rospy.Timer(rospy.Duration(1.0/rate), self.publish_transform)
def __init__(self, step_number, arm_index, action_step, marker_click_cb):
'''
Args:
step_number (int): The 1-based index of the step.
arm_index (int): Side.RIGHT or Side.LEFT
action_step (ActionStep): The action step this marker marks.
marker_click_cb (function(int,bool)): The function to call
when a marker is clicked. Pass the uid of the marker
(as calculated by get_uid(...) as well as whether it's
selected.
'''
if ActionStepMarker._im_server is None:
im_server = InteractiveMarkerServer(TOPIC_IM_SERVER)
ActionStepMarker._im_server = im_server
self.action_step = action_step
self.arm_index = arm_index
self.arm_name = ARM_NAMES[arm_index]
self.step_number = step_number
self.is_requested = False
self.is_deleted = False
self.is_control_visible = False
self.is_edited = False
self.has_object = False
self._sub_entries = None
self._menu_handler = None
self._prev_is_reachable = None
ActionStepMarker._marker_click_cb = marker_click_cb
def __init__(self, name, X_init, box_scale=0.1):
self.name = name
self.X = X_init
self.marker = createBoxMarker(scale=box_scale)
self.interactive_marker = create6DofInteractive('/map', name, name,
X_init, self.marker)
self.marker_server = InteractiveMarkerServer(name + 'server')
self.marker_server.insert(self.interactive_marker, self.markerMoved)
self.marker_server.applyChanges()
def __init__(self, marker_name):
host = rospy.get_param("mongodb_host")
port = rospy.get_param("mongodb_port")
self._client = pymongo.MongoClient(host, port)
self._traj = dict()
self._retrieve_logs()
self._server = InteractiveMarkerServer(marker_name)
class PlanarInteractiveMarker(object):
def __init__(self, name, frame_id, X_offset, text_offset, scale, axes_list=[0,1]):
self.name = name
self.scale = scale
self.force_list = [0.] * len(axes_list)
self.axes_list = axes_list
# interactive marker
self.marker = createBoxMarker(scale=0.001)
intMarker = createVisInteractive(frame_id, name, name+' 2dcontrol', X_offset, self.marker)
frame = toNumpy(Matrix3d.Identity())
for i in axes_list:
control = createTranslationControlMarker('control'+str(i), frame[i,:].tolist()[0])
intMarker.controls.append(control)
self.marker_server = InteractiveMarkerServer(name + 'server')
self.marker_server.insert(intMarker, self.markerMoved)
self.marker_server.applyChanges()
# text display of forces
self.text_marker = TextMarker(name, frame_id, text_offset)
self.updateText()
def updateText(self):
self.text_marker.update(str(self.force_list[0]) + ' N \n' \
+ str(self.force_list[1]) + ' N')
def markerMoved(self, feedback):
#TODO: better coding, check the ordering of x,y,z
for i in self.axes_list:
if i == 0:
self.force_list[i] = feedback.pose.position.x * self.scale
elif i == 1:
self.force_list[i] = feedback.pose.position.z * self.scale
elif i == 2:
self.force_list[i] = feedback.pose.position.y * self.scale
else:
print 'error, expected 0-2'
self.updateText()
self.text_marker.publish()
def __init__(self, whicharm):
self.whicharm = whicharm
self.robot_state = pr2_control_utilities.RobotState()
self.joint_controller = pr2_control_utilities.PR2JointMover(self.robot_state)
self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller)
self.server = InteractiveMarkerServer("~trajectory_markers")
self.tf_listener = self.planner.tf_listener
self.visualizer_pub = rospy.Publisher("~trajectory_markers_path",
MarkerArray)
self.trajectory_pub = rospy.Publisher("~trajectory_poses",
PoseArray)
self.gripper_pose_pub = rospy.Publisher("~gripper_pose",
PoseStamped)
rospy.Subscriber("~overwrite_trajectory",
PoseArray,
self.overwrite_trajectory)
rospy.Service("~execute_trajectory", Empty,
self.execute_trajectory_srv)
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link"
int_marker.pose.position.x = 0.5
int_marker.pose.position.z = 1.0
int_marker.name = "move_" + whicharm + "_arm"
int_marker.description = "Move the " + whicharm + " arm"
int_marker.scale = 0.2
self.server.insert(int_marker, self.main_callback)
# create the main marker shape
#color = (1,0,0,1)
color = None
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker);
#add the controls
utils.make_6DOF_marker(int_marker)
self.int_marker = int_marker
self.create_menu()
self.server.applyChanges()
self.trajectory = PoseArray()
self.trajectory.header.frame_id = "/base_link"
self.last_gripper_pose = None
if whicharm == "right":
self.ik_utils = self.planner.right_ik
else:
self.ik_utils = self.planner.left_ik
self.planning_waiting_time = 10.0
rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
class MassInteractiveMarker(object):
def __init__(self, name, frame_id, X_offset, text_offset, mass, scale):
self.name = name
self.mass = mass
self.scale = scale
# interactive marker
self.marker = createBoxMarker(scale=0.001)
self.interactive_marker = create1DofTransInteractive(frame_id, name, name+' control',
X_offset, self.marker, direction=-Vector3d.UnitY())
self.marker_server = InteractiveMarkerServer(name + 'server')
self.marker_server.insert(self.interactive_marker, self.markerMoved)
self.marker_server.applyChanges()
self.text_marker = TextMarker(name, frame_id, text_offset)
def markerMoved(self, feedback):
self.mass = abs(feedback.pose.position.z) * self.scale
self.text_marker.update(str(self.mass) + ' kg')
self.text_marker.publish()
def __init__(self):
self.transform = None
super(InteractivePose2DTF, self).__init__()
self.pub_tf = tf2_ros.TransformBroadcaster()
self.interact = InteractiveMarkerServer("interactive_markers")
self.marker_pose = Pose()
self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5))
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()
class WrenchInteractiveMarker(object):
def __init__(self, name, frame_id, X_init, text_offset, scale):
self.name = name
self.marker = createBoxMarker()
self.interactive_marker = create6DofInteractive(frame_id, name, name,
X_init, self.marker)
self.marker_server = InteractiveMarkerServer(name + 'server')
self.marker_server.insert(self.interactive_marker, self.markerMoved)
self.marker_server.applyChanges()
self.text_marker = TextMarker(name, frame_id, text_offset)
self.X_init_inv = X_init.inv()
self.scale = scale
self.forces = Vector3d.Zero()
self.torques = Vector3d.Zero()
def markerMoved(self, feedback):
X = transform.fromPose(feedback.pose) * self.X_init_inv
self.forces = self.scale[0] * X.translation()
self.torques = self.scale[1] * sva.rotationError(Matrix3d.Identity(), X.rotation())
self.text_marker.update(str(self.forces) + '\n' + str(self.torques))
self.text_marker.publish()
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):
if World.tf_listener is None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer('world_objects')
self.clear_all_objects()
self.relative_frame_threshold = 0.4
# rospy.Subscriber("ar_pose_marker",
# AlvarMarkers, self.receive_ar_markers)
self.is_looking_for_markers = False
self.marker_dims = Vector3(0.04, 0.04, 0.01)
World.world = self
def __init__(self):
# create a simple TF listener
self.tf_listener = tf.TransformListener()
self.grasp_check = rospy.ServiceProxy('/interactive_world_hackathon/grasp_check', GraspCheck)
self.speak = rospy.ServiceProxy('/tts/speak', Speak)
self.play = rospy.ServiceProxy('/tts/play', Speak)
# create the nav client
self.nav = actionlib.SimpleActionClient('navigate_action', navigateAction)
self.nav.wait_for_server()
# create the backup client
self.backup = actionlib.SimpleActionClient('backup_action', BackupAction)
self.backup.wait_for_server()
# create the place action client
self.place = actionlib.SimpleActionClient('object_manipulator/object_manipulator_place', PlaceAction)
self.place.wait_for_server()
# Segmentation client
self.segclient = actionlib.SimpleActionClient('/object_detection_user_command', UserCommandAction)
self.segclient.wait_for_server()
self.recognition=None
# create the IMGUI action client
self.imgui = actionlib.SimpleActionClient('imgui_action', IMGUIAction)
self.imgui.wait_for_server()
# listen for graspable objects
rospy.Subscriber('/interactive_object_recognition_result', GraspableObjectList, self.proc_grasp_list)
# create the save service
rospy.Service('~save_template', SaveTemplate, self.save)
self.load_server = actionlib.SimpleActionServer('load_template', LoadAction, execute_cb=self.load, auto_start=False)
self.load_server.start()
# create the IM server
self.server = InteractiveMarkerServer('~fake_marker_server')
# create return list of templates
rospy.Service('~print_templates', PrintTemplates, self.get_templates)
# used to get model meshes
self.get_mesh = rospy.ServiceProxy('/objects_database_node/get_model_mesh', GetModelMesh)
# hack to get the grasp
rospy.Subscriber('/object_manipulator/object_manipulator_pickup/result', PickupActionResult, self.store_grasp)
self.last_grasp = None
self.objects = []
self.objects.append(OBJECT1)
self.objects.append(OBJECT2)
self.objects.append(OBJECT3)
self.reset_objects()
# check for saved templates
try:
self.templates = pickle.load(open(SAVE_FILE, 'rb'))
rospy.loginfo('Loaded ' + str(len(self.templates.keys())) + ' template(s).')
except:
self.templates = dict()
rospy.loginfo('New template file started.')
self.play('/home/rctoris/wav/GLaDOS_generic_security_camera_destroyed-2.wav')
def __init__(self, name, frame_id, X_init, text_offset, scale):
self.name = name
self.marker = createBoxMarker()
self.interactive_marker = create6DofInteractive(frame_id, name, name,
X_init, self.marker)
self.marker_server = InteractiveMarkerServer(name + 'server')
self.marker_server.insert(self.interactive_marker, self.markerMoved)
self.marker_server.applyChanges()
self.text_marker = TextMarker(name, frame_id, text_offset)
self.X_init_inv = X_init.inv()
self.scale = scale
self.forces = Vector3d.Zero()
self.torques = Vector3d.Zero()
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, name, frame_id, X_offset, text_offset, mass, scale):
self.name = name
self.mass = mass
self.scale = scale
# interactive marker
self.marker = createBoxMarker(scale=0.001)
self.interactive_marker = create1DofTransInteractive(frame_id, name, name+' control',
X_offset, self.marker, direction=-Vector3d.UnitY())
self.marker_server = InteractiveMarkerServer(name + 'server')
self.marker_server.insert(self.interactive_marker, self.markerMoved)
self.marker_server.applyChanges()
self.text_marker = TextMarker(name, frame_id, text_offset)
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()
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
def __init__(self):
if World.tf_listener == None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer("world_objects")
bb_service_name = "find_cluster_bounding_box"
rospy.wait_for_service(bb_service_name)
self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox)
rospy.Subscriber("interactive_object_recognition_result", GraspableObjectList, self.receieve_object_info)
self._object_action_client = actionlib.SimpleActionClient("object_detection_user_command", UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo("Interactive object detection action " + "server has responded.")
self.clear_all_objects()
# The following is to get the table information
rospy.Subscriber("tabletop_segmentation_markers", Marker, self.receieve_table_marker)
def __init__(self, bag_file, md_path, calib_file, tracklets):
self.timestamp_map = extract_bag_timestamps(bag_file)
self.calib_file = calib_file
self.frame_map = generate_frame_map(tracklets)
md = None
metadata = load_metadata(md_path)
for obs in metadata:
if obs['obstacle_name'] == 'obs1':
md = obs
assert md, 'obs1 metadata not found'
self.metadata = md
self.server = InteractiveMarkerServer("obstacle_marker")
self.br = tf.TransformBroadcaster()
self.offset = [0,0,0]
self.rotation_offset = [0,0,0,1]
self.orient = (0,0,0,1)
self.marker = Marker()
self.marker.type = Marker.CUBE
self.marker.header.frame_id = "velodyne"
md = self.metadata
self.marker.scale.x = md['l']
self.marker.scale.y = md['w']
self.marker.scale.z = md['h']
self.marker.color.r = 0.2
self.marker.color.g = 0.5
self.marker.color.b = 0.2
self.marker.color.a = 0.7
outputName = '/image_bbox'
self.imgOutput = rospy.Publisher(outputName, Image, queue_size=1)
#self.markOutput = rospy.Publisher("bbox", Marker, queue_size=1)
self.velodyne_marker = self.setup_marker(frame = "velodyne",
name = "capture vehicle", translation=True)
self.obs_marker = self.setup_marker(frame = "velodyne",
name = "obstacle vehicle", translation=False)
def __init__(self, side_prefix):
self._ik_service = IK(side_prefix)
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()
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']
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix))
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert("Move {} arm here".format(side_prefix), 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()
def __init__(self, md_path, calib_file):
self.reset()
self.current_time = rospy.Time()
self.calib_file = calib_file
md = None
metadata = load_metadata(md_path)
for obs in metadata:
if obs['obstacle_name'] == 'obs1':
md = obs
assert md, 'obs1 metadata not found'
self.metadata = md
self.server = InteractiveMarkerServer("obstacle_marker")
self.br = tf.TransformBroadcaster()
self.offset = [0,0,0]
self.rotation_offset = [0,0,0,1]
self.orient = (0,0,0,1)
self.marker = Marker()
self.marker.type = Marker.CUBE
self.marker.header.frame_id = "obs_centroid"
md = self.metadata
self.marker.scale.x = md['l']
self.marker.scale.y = md['w']
self.marker.scale.z = md['h']
self.marker.color.r = 0.2
self.marker.color.g = 0.5
self.marker.color.b = 0.2
self.marker.color.a = 0.7
self.velodyne_marker = self.setup_marker(frame = "velodyne",
name = "capture vehicle", translation=True)
self.obs_marker = self.setup_marker(frame = "obs_centroid",
name = "obstacle vehicle", translation=False)
def __init__(self, name, frame_id, X_offset, text_offset, scale, axes_list=[0,1]):
self.name = name
self.scale = scale
self.force_list = [0.] * len(axes_list)
self.axes_list = axes_list
# interactive marker
self.marker = createBoxMarker(scale=0.001)
intMarker = createVisInteractive(frame_id, name, name+' 2dcontrol', X_offset, self.marker)
frame = toNumpy(Matrix3d.Identity())
for i in axes_list:
control = createTranslationControlMarker('control'+str(i), frame[i,:].tolist()[0])
intMarker.controls.append(control)
self.marker_server = InteractiveMarkerServer(name + 'server')
self.marker_server.insert(intMarker, self.markerMoved)
self.marker_server.applyChanges()
# text display of forces
self.text_marker = TextMarker(name, frame_id, text_offset)
self.updateText()
class AutosequencesVizModule(VizModuleBase):
"""
Visualization module for control_mode_autosequence.
Provides:
- polygon representing autosequence path
- markers for autosequence points
- labels
- heading indicators at each point
"""
got_autosequence = False
current_autosequence = None
latest_autoseq_info = None
latest_hover_info = None
loaded_as_name = ""
def __init__(self):
super(AutosequencesVizModule, self).__init__(id="autoseq")
def init_viz(self):
self.as_poly = PolygonMarker('autosequence', (),
color=Colors.BLUE+Alpha(0.5), width=0.02, closed=False)
self.as_hover_point_sphere = SphereMarker('hover_point', (0,0,0), radius=0.02, color=Colors.BLUE+Alpha(0.5))
self.mg = MarkerGroup(VizModuleBase.mapub)
self.mg.add(self.as_poly, self.as_hover_point_sphere)
self.init_int_marker()
def init_vars(self):
pass
def init_subscribers(self):
prefix = self.subscriber_topic_prefix
#self.llstatus_sub = rospy.Subscriber(prefix+'autopilot/LL_STATUS', LLStatus, self.llstatus_callback) # AscTec specific.. how to make more generic?
self.autoseq_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/autosequence_info',
control_mode_autosequence_info, self.autoseq_info_callback)
self.hover_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/info',
control_mode_hover_info, self.hover_info_callback)
self.controller_status_sub = rospy.Subscriber(prefix+'controller/status', controller_status, self.controller_status_callback)
def init_int_marker(self):
self.ims = InteractiveMarkerServer("simple_marker")
self.im = InteractiveMarker()
self.im.header.frame_id = "/ned"
self.im.name = "my_marker"
self.im.description = "Simple 1-DOF control"
bm = Marker()
bm.type = Marker.CUBE
bm.scale.x = bm.scale.y = bm.scale.z = 0.45
bm.color.r = 0.0
bm.color.g = 0.5
bm.color.b = 0.5
bm.color.a = 1.0
bc = InteractiveMarkerControl()
bc.always_visible = True
bc.markers.append(bm)
self.im.controls.append(bc)
rc = InteractiveMarkerControl()
rc.name = "move_x"
rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.im.controls.append(rc)
self.ims.insert(self.im, self.process_marker_feedback)
self.ims.applyChanges()
def process_marker_feedback(self, feedback):
p = feedback.pose.position
print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z)
def publish_timer_callback(self, event):
if self.got_autosequence:
as_points = [(p.hover_point.x, p.hover_point.y, 0) for p in self.current_autosequence.points]
self.as_poly.set(points=as_points)
if self.latest_hover_info is not None:
slhi = self.latest_hover_info
self.as_hover_point_sphere.set(origin=(slhi.north_cmd, slhi.east_cmd, 0))
now = rospy.Time.now()
VizModuleBase.publish(self,now)
def autoseq_info_callback(self, msg):
#rospy.loginfo('got autoseq info: %s' % str(msg))
self.latest_autoseq_info = msg
def hover_info_callback(self, msg):
self.latest_hover_info = msg
def _new_autosequence(self):
return (self.latest_autoseq_info is not None
and len(self.latest_autoseq_info.defined_autosequences) > 0
and (self.latest_autoseq_info.current_autosequence != self.loaded_as_name))
def controller_status_callback(self, msg):
self.latest_controller_status = msg
#rospy.loginfo('got controller status: %s' % str(msg))
if msg.active_mode == 'autosequence':
if self._new_autosequence():
self.got_autosequence = False
if (not self.got_autosequence
and self.latest_autoseq_info is not None
and len(self.latest_autoseq_info.defined_autosequences) > 0
and len(self.latest_autoseq_info.current_autosequence) > 0):
as_name = self.latest_autoseq_info.current_autosequence
self.get_autosequence(as_name)
def get_autosequence(self, as_name):
get_auto_proxy = rospy.ServiceProxy('control_mode_autosequence/get_autosequence', GetAutosequence)
rospy.loginfo("Asking for autosequence '%s'..." % as_name)
resp = get_auto_proxy(autosequence_name=as_name)
if resp.found:
self.current_autosequence = resp.autosequence
rospy.loginfo('Got autosequence: %s' % str(self.current_autosequence))
self.got_autosequence = True
self.loaded_as_name = as_name
else:
rospy.logerr("Service call failed: autosequence '%s' not found" % as_name)
class TFMarkerServer(object):
"""TFMarkerServer"""
def __init__(self, rate = 30.0, filename = None):
# Marker server
self.server = InteractiveMarkerServer('camera_marker')
# TF broadcaster
self.tf = TransformBroadcaster()
# Marker pose
self.pose_mutex = Lock()
self.marker_position = (0.0, 0.0, 0.0)
self.marker_orientation = (0.0, 0.0, 0.0, 1.0)
# Load init position
self.filename = filename
if self.filename:
with open(self.filename, 'r') as stream:
init_data = yaml.load(stream)['init_position']
self.marker_position = (init_data['x'], init_data['y'], init_data['z'])
self.marker_orientation = (0.0, 0.0, 0.0, 1.0)
# Register shutdown callback
rospy.on_shutdown(self.shutdown)
# Add marker
self.add_6DOF()
# Timer for TF broadcaster
rospy.Timer(rospy.Duration(1.0/rate), self.publish_transform)
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 publish_transform(self, timer_event):
time = rospy.Time.now()
self.pose_mutex.acquire()
self.tf.sendTransform(self.marker_position, self.marker_orientation,
time, 'sensor_base', 'map')
self.pose_mutex.release()
def marker_feedback(self, feedback):
rospy.loginfo('Feedback from ' + feedback.marker_name)
# Check event
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.loginfo( 'Pose changed')
# Update marker position
self.pose_mutex.acquire()
self.marker_position = (feedback.pose.position.x,
feedback.pose.position.y, feedback.pose.position.z)
# Update marker orientation
self.marker_orientation = (feedback.pose.orientation.x,
feedback.pose.orientation.y, feedback.pose.orientation.z,
feedback.pose.orientation.w)
self.pose_mutex.release()
def shutdown(self):
data = {
'init_position' :
{
'x': 0.0,
'y': 0.0,
'z': 0.0,
'roll': 0.0,
'pitch': 0.0,
'yaw': 0.0,
}
}
rospy.loginfo('Writing current position')
with open(self.filename, 'w') as outfile:
outfile.write(yaml.dump(data, default_flow_style=True))
class GripperMarkers:
_offset = 0.09
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()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logwarn('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_solution = self.ik.get_ik_for_ee(target_pose)
#TODO: Send the arms to ik_solution
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
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 _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 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)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -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)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def _offset_pose(pose):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [GripperMarkers._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform)
return GripperMarkers.get_pose_from_transform(hand_transform)
@staticmethod
def get_matrix_from_pose(pose):
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
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
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self._ik_service = IK(side_prefix)
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()
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']
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix))
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert("Move {} arm here".format(side_prefix), 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()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except Exception as e:
rospy.logerr('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
import traceback
traceback.print_exc()
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
self.move_to_joints(self.side_prefix, [ik_sol], 1.0)
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def marker_cb(self, pose_markers):
rospy.loginfo('AR Marker Pose updating')
transform = GripperMarkers.get_matrix_from_pose(pose_markers.markers[0].pose.pose)
offset_array = [0, 0, .03]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
self.ee_pose = GripperMarkers.get_pose_from_transform(hand_transform)
self.update_viz()
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=%.3f y=%.3f z=%.3f" % (pose.position.x, pose.position.y, 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))))
label_pos = Point()
label_pos.x = pose.position.x
label_pos.y = pose.position.y
label_pos.z = pose.position.z
label = "{} arm".format(self.side_prefix)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=label,
color=ColorRGBA(0.0, 0.0, 0.0, 0.9),
header=Header(frame_id=frame_id),
pose=Pose(label_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 _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 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)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -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)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def _offset_pose(pose):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [-GripperMarkers._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
return GripperMarkers.get_pose_from_transform(hand_transform)
@staticmethod
def get_matrix_from_pose(pose):
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
if ik_sol == None:
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
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 move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
time_move = time_to_joint
print "using following positions %s" % positions
for pose in positions:
velocities = [0] * len(pose)
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=pose,
velocities=velocities, time_from_start=rospy.Duration(time_move)))
time_move += time_to_joint
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
