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 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 main():
rospy.init_node('interactive_marker_node')
server = InteractiveMarkerServer("simple_marker")
# create an interative marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.name = "my_marker"
int_marker.description = "Simple Click Control"
int_marker.pose.position.x = 1
int_marker.pose.orientation.w = 1
# create a cube for the interactive marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.pose.orientation.w = 1
box_marker.scale.x = 0.45
box_marker.scale.y = 0.45
box_marker.scale.z = 0.45
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
# create a control for the interactive marker
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
button_control.markers.append(box_marker)
int_marker.controls.append(button_control)
server.insert(int_marker, handle_viz_input)
server.applyChanges()
rospy.spin()
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)
class RvizPathServer(object):
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 add_marker(self, msg, path_id):
menu, marker = create_marker(path_msg=msg.path, color_msg=msg.color,
description=msg.description, path_id=path_id,
width=self.width,
delta_z=self.delta_z)
self.server.insert(marker, ignore)
menu.insert("FOLLOW", callback=self.goto(path_id))
menu.apply(self.server, marker.name)
self.paths[path_id] = msg.path
def goto(self, path_id):
def f(msg):
rospy.loginfo("Follow path %d", path_id)
self.pub.publish(self.paths[path_id])
return f
def updatePaths(self, msg):
path_msg = NavPath()
path_msg.header.frame_id = 'map'
self.pub.publish(path_msg)
self.server.clear()
for i, m in enumerate(msg.paths):
self.add_marker(m, i)
self.server.applyChanges()
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()
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()
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 main():
rospy.init_node('interactive_marker_demo')
wait_for_time()
server = InteractiveMarkerServer("simple_marker")
marker = InteractiveMarker()
marker.header.frame_id = "base_link"
marker.name = "adamson_marker"
marker.description = "Simple Click Control"
marker.pose.position.x = 1
marker.pose.orientation.w = 1
print("This is my interactive marker:")
pprint(marker)
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.pose.orientation.w = 1
box_marker.scale.x = 0.45
box_marker.scale.y = 0.45
box_marker.scale.z = 0.45
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
button_control.markers.append(box_marker)
marker.controls.append(button_control)
print("\nAnd this is my normal marker")
pprint(box_marker)
server.insert(marker, handle_vix_input)
server.applyChanges()
rospy.spin()
translate_y.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
translate_y.orientation.w = 1
translate_y.orientation.x = 0
translate_y.orientation.y = 0
translate_y.orientation.z = 1
translate_y.name = "translate_y"
interactive_marker.controls.append(translate_y)
translate_z = InteractiveMarkerControl()
translate_z.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
translate_z.orientation.w = 1
translate_z.orientation.x = 0
translate_z.orientation.y = 1
translate_z.orientation.z = 0
translate_z.name = "translate_z"
interactive_marker.controls.append(translate_z)
def update_object(feedback):
p = feedback.pose.position
sim_object.set_position(p.x, p.y, p.z)
print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z)
# add the interactive marker to our collection &
# tell the server to call update_object() when feedback arrives for it
server.insert(interactive_marker, update_object)
# 'commit' changes and send to all clients
server.applyChanges()
rospy.spin()
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)
class PickAndPlaceNode(Manager):
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
# self.robot.home()
# self.move_calib_position()
def move_calib_position(self):
# move arm to the calibration position
self.calib_pose = PoseStamped(
Header(0, rospy.Time(0), self.robot.base),
Pose(Point(0.338520675898,-0.175860479474,0.0356775075197),
Quaternion(-0.0183493755758,0.708424150944, 0.704712092876, 0.0343413949013)))
self.robot.move_ik(self.calib_pose)
def update_num_objects(self, msg):
self.num_objects = msg.data
def _calibrate(self):
self.state = "calibrate"
self.alignment_pub.publish(Bool(True))
def _perceive(self):
self.state = "perceive"
rospy.loginfo("Asking for perception...")
self.perception_pub.publish(Bool(True))
def _stop_perceive(self):
self.state = "perceive"
self.perception_pub.publish(Bool(False))
def _post_perceive(self):
self.state = "post_perceive"
rospy.loginfo("Asking to stop perception...")
self.perception_pub.publish(Bool(False))
def _preplace(self):
# State not modified until placing succeeds
try:
obj_to_get = int(self.character)
except ValueError:
rospy.logerr("Please provide a number in placing mode")
return
self.state = "preplace"
frame_name = "object_pose_{}".format(obj_to_get)
rospy.loginfo(
"Placing object on top of object {}...".format(obj_to_get))
if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name):
t = self.tl.getLatestCommonTime(self.robot.base, frame_name)
position, quaternion = self.tl.lookupTransform(self.robot.base,
frame_name,
t)
position = list(position)
# Height of cubelet
position[2] += self.robot.level * 0.042
# YCB
# position[2] += self.robot.level * 0.026
# Update pose position from perception
self.place_pose.pose.position = Point(*position)
rospy.loginfo("Adjusting place position...")
imarker_name = 'place_pose'
self.int_markers[imarker_name] = self.place_pose
imarker = make_interactive_marker(imarker_name,
self.place_pose.pose)
# TODO delete imarker at post place
self.int_marker_server.insert(imarker, self.imarker_callback)
self.int_marker_server.setCallback(imarker_name, self.imarker_callback)
self.int_marker_server.applyChanges()
rospy.loginfo("imarker stuff done")
def imarker_callback(self, msg):
# http://docs.ros.org/jade/api/visualization_msgs/html/msg/InteractiveMarkerFeedback.html # noqa
rospy.loginfo('imarker_callback called')
name = msg.marker_name
new_pose = msg.pose
self.int_markers[name] = PoseStamped(msg.header, new_pose)
def _place(self):
self.state = "place"
rospy.loginfo("Placing...")
place_pose = self.int_markers['place_pose']
# It seems positions and orientations are randomly required to
# be actual Point and Quaternion objects or lists/tuples. The
# least coherent API ever seen.
self.br.sendTransform(Point2list(place_pose.pose.position),
Quaternion2list(place_pose.pose.orientation),
rospy.Time.now(),
"place_pose",
self.robot.base)
self.robot.place(place_pose)
# For cubelets:
place_pose.pose.position.z += 0.042
# For YCB:
# place_pose.pose.position.z += 0.026
self.place_pose = place_pose
def _pick(self):
# State not modified until picking succeeds
try:
obj_to_get = int(self.character)
except ValueError:
rospy.logerr("Please provide a number in picking mode")
return
frame_name = "object_pose_{}".format(obj_to_get)
rospy.loginfo("Picking object {}...".format(obj_to_get))
if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name):
t = self.tl.getLatestCommonTime(self.robot.base, frame_name)
position, quaternion = self.tl.lookupTransform(self.robot.base,
frame_name,
t)
print("position", position)
print("quaternion", quaternion)
position = list(position)
# Vertical orientation
self.br.sendTransform(position,
[1, 0, 0, 0],
rospy.Time.now(),
"pick_pose",
self.robot.base)
# Ignore orientation from perception
pose = Pose(Point(*position),
Quaternion(1, 0, 0, 0))
h = Header()
h.stamp = t
h.frame_id = self.robot.base
stamped_pose = PoseStamped(h, pose)
self.robot.pick(stamped_pose)
self.state = 'postpick'
def _level(self):
self.robot.level = int(self.character)
self.state = 'level'
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 MarkerTele(object):
DISTANCE_MAGNITUDE = 0.5
ANGULAR_MAGNITUDE = math.pi / 6
def __init__(self):
self._server = InteractiveMarkerServer("simple_marker")
self._base = robot_api.Base()
def click_forward(self, input):
if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK):
self._base.go_forward(self.DISTANCE_MAGNITUDE)
def click_backward(self, input):
if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK):
self._base.go_forward(self.DISTANCE_MAGNITUDE * -1)
def click_left(self, input):
if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK):
self._base.turn(self.ANGULAR_MAGNITUDE)
def click_right(self, input):
if (input.event_type == InteractiveMarkerFeedback.BUTTON_CLICK):
self._base.turn(self.ANGULAR_MAGNITUDE * -1)
def callback(self, msg):
forward_int_marker = InteractiveMarker()
forward_int_marker.header.frame_id = "base_link"
forward_int_marker.name = "forward_marker"
forward_int_marker.description = "Forward"
forward_int_marker.pose.position.x = 0.5
forward_int_marker.pose.position.z = 0.75
forward_int_marker.pose.orientation.w = 0
forward_marker = Marker()
forward_marker.ns = "front_marker"
forward_marker.type = Marker.CUBE
forward_marker.pose.orientation.w = 0
forward_marker.scale.x = 0.45
forward_marker.scale.y = 0.45
forward_marker.scale.z = 0.45
forward_marker.color.r = 0.0
forward_marker.color.g = 0.5
forward_marker.color.b = 0.5
forward_marker.color.a = 1.0
backward_int_marker = InteractiveMarker()
backward_int_marker.header.frame_id = "base_link"
backward_int_marker.name = "backward_marker"
backward_int_marker.description = "Backward"
backward_int_marker.pose.position.x = -0.5
backward_int_marker.pose.position.z = 0.75
backward_int_marker.pose.orientation.w = 0
backward_marker = Marker()
backward_marker.ns = "back_marker"
backward_marker.type = Marker.CUBE
backward_marker.pose.orientation.w = 0
backward_marker.scale.x = 0.45
backward_marker.scale.y = 0.45
backward_marker.scale.z = 0.45
backward_marker.color.r = 0.0
backward_marker.color.g = 0.5
backward_marker.color.b = 0.5
backward_marker.color.a = 1.0
left_int_marker = InteractiveMarker()
left_int_marker.header.frame_id = "base_link"
left_int_marker.name = "left_marker"
left_int_marker.description = "left"
left_int_marker.pose.position.y = 0.5
left_int_marker.pose.position.z = 0.75
left_int_marker.pose.orientation.w = 0
left_marker = Marker()
left_marker.ns = "left_marker"
left_marker.type = Marker.CUBE
left_marker.pose.orientation.w = 0
left_marker.scale.x = 0.45
left_marker.scale.y = 0.45
left_marker.scale.z = 0.45
left_marker.color.r = 0.0
left_marker.color.g = 0.5
left_marker.color.b = 0.5
left_marker.color.a = 1.0
right_int_marker = InteractiveMarker()
right_int_marker.header.frame_id = "base_link"
right_int_marker.name = "right_marker"
right_int_marker.description = "right"
right_int_marker.pose.position.y = -0.5
right_int_marker.pose.position.z = 0.75
right_int_marker.pose.orientation.w = 0
right_marker = Marker()
right_marker.ns = "right_marker"
right_marker.type = Marker.CUBE
right_marker.pose.orientation.w = 0
right_marker.scale.x = 0.45
right_marker.scale.y = 0.45
right_marker.scale.z = 0.45
right_marker.color.r = 0.0
right_marker.color.g = 0.5
right_marker.color.b = 0.5
right_marker.color.a = 1.0
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
button_control.markers.append(forward_marker)
button_control.markers.append(backward_marker)
button_control.markers.append(left_marker)
button_control.markers.append(right_marker)
forward_int_marker.controls.append(button_control)
backward_int_marker.controls.append(button_control)
left_int_marker.controls.append(button_control)
right_int_marker.controls.append(button_control)
self._server.insert(forward_int_marker, self.click_forward)
self._server.insert(backward_int_marker, self.click_backward)
self._server.insert(left_int_marker, self.click_left)
self._server.insert(right_int_marker, self.click_right)
self._server.applyChanges()
class FakeMarkerServer():
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')
#Service: rosservice call /fake_object_markers/print_templates
#Returns a string of template names
#ex) list: ['test_template1','test_template2','test_template3']
def get_templates(self, req):
temp_list = []
if self.templates.keys() is None:
self.publish_feedback('No templates')
return
for obj in self.templates.keys():
temp_list.append(str(obj))
#print temp_list
return PrintTemplatesResponse(temp_list)
def store_grasp(self, msg):
self.last_grasp = msg.result.grasp
# Given a mesh_id creates a name with format 'object + mesh_id'
# ex.)Given '1234', creates 'object_1234' name
def create_name(self, mesh_id):
return 'object_' + str(mesh_id)
# Creates a mesh of the given object with the given pose to be visualized by template maker
def create_mesh(self, mesh_id, pose):
response = self.get_mesh(mesh_id)
mesh = response.mesh
# build the mesh marker
marker = Marker()
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.66
marker.frame_locked = False
marker.type = Marker.TRIANGLE_LIST
# add the mesh
for j in range(len(mesh.triangles)):
marker.points.append(
mesh.vertices[mesh.triangles[j].vertex_indices[0]])
marker.points.append(
mesh.vertices[mesh.triangles[j].vertex_indices[1]])
marker.points.append(
mesh.vertices[mesh.triangles[j].vertex_indices[2]])
# create the interactive marker
name = self.create_name(mesh_id)
self.server.insert(self.create_im(marker, pose, name),
self.process_feedback)
self.server.setCallback(name, self.release,
InteractiveMarkerFeedback.MOUSE_UP)
self.server.applyChanges()
# Creates an interactive marker
# - at the given location and pose
# - with a given name
# - for given marker object
def create_im(self, marker, pose, name):
# create the new interactive marker
int_marker = InteractiveMarker()
int_marker.pose = copy.deepcopy(pose)
int_marker.header.frame_id = 'base_link'
int_marker.name = name
# move freely on the X-Y plane
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.markers.append(marker)
control.always_visible = True
int_marker.controls.append(control)
return int_marker
def process_feedback(self, feedback):
self.last_feedback = feedback
# Returns true if given (x,y) coordinates are within "Graspable/Placeable(?)" range
def check_pose(self, x, y):
return x >= OFFSET + DEPTH_START and x <= OFFSET + DEPTH_END and y >= WIDTH_START and y <= WIDTH_END
# Checks position of hallucinated interactive markers
# Changes color and sets position when user releases mouse click (MOUSE_UP) on object
def release(self, feedback):
im = self.server.get(feedback.marker_name)
# copy the mesh information
marker = copy.deepcopy(im.controls[0].markers[0])
# determine color based on pose
if self.check_pose(feedback.pose.position.x, feedback.pose.position.y):
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.66
else:
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.66
# create the new interactive marker
self.server.insert(
self.create_im(marker, feedback.pose, feedback.marker_name),
self.process_feedback)
self.server.setCallback(feedback.marker_name, self.release,
InteractiveMarkerFeedback.MOUSE_UP)
self.server.applyChanges()
# updates server
def update(self):
self.server.applyChanges()
# **Run by save_template service**
# Saves given template information to file location
def save(self, req):
# go through each object and check if they are in the valid range
to_save = []
for obj_id in self.objects:
im = self.server.get(self.create_name(obj_id))
pose = im.pose
if (self.check_pose(pose.position.x, pose.position.y)):
to_save.append(copy.deepcopy(im))
# check if we have anything to save
if len(to_save) > 0:
self.templates[req.name] = to_save
# PICKLE IT!
pickle.dump(self.templates, open(SAVE_FILE, 'wb'))
self.play('/home/rctoris/wav/t3_affirmative.wav')
self.reset_objects()
return SaveTemplateResponse(True)
else:
return SaveTemplateResponse(False)
# Publishes feedback of current tasks
def publish_feedback(self, msg):
rospy.loginfo(msg)
self.load_server.publish_feedback(LoadFeedback(msg))
# Publishes final result of action
def publish_result(self, msg):
rospy.loginfo(msg)
self.load_server.set_succeeded(LoadResult(msg))
# Returns how many objects were recognized
def proc_grasp_list(self, msg):
objects = []
# start by going through each
size = len(msg.graspable_objects)
for i in range(size):
obj = msg.graspable_objects[i]
# only take recognized objects
if len(obj.potential_models) is not 0:
objects.append(copy.deepcopy(obj))
rospy.loginfo('Found ' + str(len(objects)) + ' object(s).')
self.recognition = objects
# Drives to and aligns with counter
# Segments objects
def look_for_objects(self):
self.publish_feedback('Driving robot to counter')
# drive the robot
nav_goal = navigateGoal('Snack Nav', True)
self.nav.send_goal_and_wait(nav_goal)
res = self.nav.get_result()
if not res.success:
self.publish_feedback('Counter alignment failed.')
return False
self.publish_feedback('Aligned robot to counter')
self.publish_feedback('Looking for objects')
self.play('/home/rctoris/wav/GLaDOS_10_part1_entry-2.wav')
self.recognition = None
# Segment the table
self.segclient.send_goal(UserCommandGoal(request=1, interactive=False))
self.segclient.wait_for_result()
while self.recognition is None:
time.sleep(1)
# Recognize objects
self.recognition = None
self.segclient.send_goal(UserCommandGoal(request=2, interactive=False))
self.segclient.wait_for_result()
while self.recognition is None:
time.sleep(1)
return True
# **Run by load_template service**
# Identifies remaining objects needed in template
# Moves to and aligns with counter
# Scans and recognizes objects on counter that match template
# Picks up one object
# Backs up from counter
# Drives to table
# Places object in given template location
# Repeats
def load(self, goal):
name = goal.name
self.publish_feedback('Loading template ' + name)
# if requested template does not exist...
if name not in self.templates.keys():
self.publish_result(name + ' template does not exist')
return
template = copy.deepcopy(self.templates[name])
self.publish_feedback('Loaded template ' + name)
self.play('/home/rctoris/wav/help.wav')
# look for any objects we need
while len(template) is not 0:
pickup_arm = None
# if it does not see any objects/could not drive to counter
if not self.look_for_objects():
self.publish_result('Object looking failed.')
return
# for each object in template array...
for template_im in template:
# for each recognized object
for rec_obj in self.recognition:
if template_im.name == self.create_name(
rec_obj.potential_models[0].model_id):
# create the object info for it
obj_info = self.create_object_info(rec_obj)
# pick it up
pickup_arm = self.pickup(rec_obj)
# if neither arm can could pick up object...
if pickup_arm is None:
self.publish_result('Pickup failed.')
return
# make sure we have a grasp
self.publish_feedback('Waiting for grasp')
while self.last_grasp is None:
rospy.sleep(1)
# store the grasp
obj_info.grasp = self.last_grasp
self.last_grasp = None
self.publish_feedback('Grasp found')
# good job robot, place that object
to_place = Pose()
# location of object in template on table
to_place.position.z = TABLE_HEIGHT - PLACE_HEIGHT_OFFSET
to_place.position.x = template_im.pose.position.x
to_place.position.y = template_im.pose.position.y
placed = self.place_object(obj_info, pickup_arm,
to_place)
# if the object could not be placed
if not placed:
self.publish_result('Place failed.')
return
self.publish_feedback('Placed the object!')
if len(template) is not 1:
self.play('/home/rctoris/wav/ill-be-back.wav')
# removes object from list of objects to pick up from template
template.remove(template_im)
# if no objects are found...
if pickup_arm is None:
# No objects found :(
self.publish_result('No objects found that we need :(')
return
# We completed the task!
self.play('/home/rctoris/wav/down.wav')
self.publish_result('Great success!')
# resets collision map of world and rescan
def reset_collision_map(self):
self.publish_feedback('Reseting collision map')
goal = IMGUIGoal()
goal.command.command = 3
goal.options.reset_choice = 4
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
self.publish_feedback('Collision map reset')
# reset hallucinated interactive marker objects' positions on visualized table
def reset_objects(self):
pose = Pose()
pose.position.z = TABLE_HEIGHT
pose.position.x = OFFSET * 3
pose.position.y = -0.25
for obj_id in self.objects:
self.create_mesh(obj_id, pose)
pose.position.y = pose.position.y + 0.25
# Picks up object that matches obj
def pickup(self, obj):
# start by picking up the object
options = IMGUIOptions()
options.collision_checked = True
options.grasp_selection = 1
options.adv_options.lift_steps = 10
options.adv_options.retreat_steps = 10
options.adv_options.reactive_force = False
options.adv_options.reactive_grasping = False
options.adv_options.reactive_place = False
options.adv_options.lift_direction_choice = 0
# check which arm is closer
if obj.potential_models[0].pose.pose.position.y > 0:
options.arm_selection = 1
else:
options.arm_selection = 0
goal = IMGUIGoal()
goal.options = options
goal.options.grasp_selection = 1
goal.options.selected_object = obj
goal.command.command = goal.command.PICKUP
# send it to IMGUI
self.publish_feedback('Attempting to pick up')
self.reset_collision_map()
self.imgui.send_goal(goal)
self.play('/home/rctoris/wav/humnbehv.wav')
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
# try the other arm
if options.arm_selection is 0:
options.arm_selection = 1
else:
options.arm_selection = 0
self.publish_feedback('Initial pickup failed, trying other arm')
self.reset_collision_map()
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
return None
else:
# now check if feedback to see if we actually got it
if options.arm_selection is 0:
arm = 'right'
else:
arm = 'left'
self.publish_feedback('Checking if object was grasped')
resp = self.grasp_check(arm)
if resp.isGrasping is True:
self.publish_feedback('Object was grasped')
# attempt to back up
backup_goal = BackupGoal()
self.backup.send_goal_and_wait(backup_goal)
res = self.backup.get_result()
# if robot could not back up
if not res.success:
self.publish_feedback('Backup failed.')
return None
return options.arm_selection
else:
self.move_arm_to_side(options.arm_selection)
return None
# moves arm to sides
def move_arm_to_side(self, arm_selection):
goal = IMGUIGoal()
goal.command.command = 4
goal.options.arm_selection = arm_selection
goal.options.arm_action_choice = 0
goal.options.arm_planner_choice = 1
self.publish_feedback('Moving arm to the side using planner')
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
# try open loop
self.publish_feedback('Planned arm move failed, trying open loop')
goal.options.arm_planner_choice = 0
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
self.publish_feedback('Arm move failed.')
return False
else:
self.publish_feedback('Arm move was successful')
return True
# place object in given arm to given pose
def place_object(self, obj_info_orig, arm_selection, pose):
#drive to the table
self.publish_feedback('Driving robot to table')
nav_goal = navigateGoal('Dining Table Nav', True)
self.play('/home/rctoris/wav/run.wav')
self.nav.send_goal_and_wait(nav_goal)
res = self.nav.get_result()
if not res.success:
self.publish_feedback('Table alignment failed.')
return False
self.publish_feedback('Aligned robot to table')
self.reset_collision_map()
self.publish_feedback('Attempting to place the object')
# make a copy
obj_info = copy.deepcopy(obj_info_orig)
obj = obj_info.obj
goal = PlaceGoal()
# set the arm
if arm_selection is 0:
goal.arm_name = 'right_arm'
prefix = 'r'
else:
goal.arm_name = 'left_arm'
prefix = 'l'
# rotate and "gu-chunk"
orig_z = pose.position.z
pose.orientation.x = 0
pose.orientation.y = 0
goal.place_locations = []
#iterating through possible x-locations to place object
for x in range(0, 10):
pose.position.x = pose.position.x + ((x - 5) * 0.0025)
#iterating through possible y-locations to place object
for y in range(0, 10):
pose.position.y = pose.position.y + ((y - 5) * 0.0025)
# 'i' is for some rotations
for i in range(0, 10):
rads = (pi * (i / 10.0))
pose.orientation.z = sin(-rads / 2.0)
pose.orientation.w = cos(-rads / 2.0)
# 'j' is for the 'z' height
for j in range(0, 6):
pose.position.z = orig_z + (j * 0.0025)
pose_mat = pose_to_mat(pose)
to_base_link_mat = pose_mat * obj_info.obj_origin_to_bounding_box
grasp_mat = pose_to_mat(obj_info.grasp.grasp_pose.pose)
gripper_mat = to_base_link_mat * grasp_mat
gripper_pose = stamp_pose(mat_to_pose(gripper_mat),
'base_link')
goal.place_locations.append(gripper_pose)
# use the identity as the grasp
obj_info.grasp.grasp_pose.pose = Pose()
obj_info.grasp.grasp_pose.pose.orientation.w = 1
goal.grasp = obj_info.grasp
goal.desired_retreat_distance = 0.1
goal.min_retreat_distance = 0.05
# set the approach
goal.approach = GripperTranslation()
goal.approach.desired_distance = .1
goal.approach.min_distance = 0.05
goal.approach.direction = create_vector3_stamped([0., 0., -1.],
'base_link')
# set the collision info
goal.collision_object_name = obj.collision_name
goal.collision_support_surface_name = 'table'
goal.place_padding = 0.0
goal.additional_link_padding = self.create_gripper_link_padding(prefix)
goal.collision_support_surface_name = 'all'
goal.allow_gripper_support_collision = True
goal.use_reactive_place = False
# send the goal
self.place.send_goal(goal)
# wait for result
finished_within_time = self.place.wait_for_result(rospy.Duration(240))
if not finished_within_time:
self.place.cancel_goal()
return False
# check the result
res = self.place.get_result()
if res.manipulation_result.value == -6 or res.manipulation_result.value == 1:
# attempt to back up
backup_goal = BackupGoal()
self.backup.send_goal_and_wait(backup_goal)
backup_res = self.backup.get_result()
# if robot could not back up
if not backup_res.success:
self.publish_feedback('Backup failed.')
return False
self.move_arm_to_side(arm_selection)
return True
else:
return False
def create_gripper_link_padding(self, prefix):
link_name_list = [
'_gripper_palm_link', '_gripper_r_finger_tip_link',
'_gripper_l_finger_tip_link', '_gripper_l_finger_link',
'_gripper_r_finger_link', '_wrist_roll_link'
]
pad = 0.
arm_link_names = [prefix + link_name for link_name in link_name_list]
link_padding_list = [
LinkPadding(link_name, pad) for link_name in arm_link_names
]
return link_padding_list
def create_object_info(self, obj):
# get the pose
pose_stamped = obj.potential_models[0].pose
# change the frame
obj_frame_pose_stamped = change_pose_stamped_frame(
self.tf_listener, pose_stamped, obj.reference_frame_id)
return ObjectInfo(obj, obj_frame_pose_stamped, self.tf_listener)
def main():
rospy.init_node('base_demo')
wait_for_time()
server = InteractiveMarkerServer("drive_marker")
int_f_marker = InteractiveMarker()
int_f_marker.header.frame_id = "base_link"
int_f_marker.name = "foward_marker"
int_f_marker.description = "Forward Control"
int_f_marker.pose.position.x = 2
int_f_marker.pose.orientation.w = 1
f_marker = Marker()
f_marker.type = Marker.CUBE
f_marker.pose.orientation.w = 2
f_marker.scale.x = 0.45
f_marker.scale.y = 0.45
f_marker.scale.z = 0.45
f_marker.color.r = 0.0
f_marker.color.g = 0.5
f_marker.color.b = 0.5
f_marker.color.a = 1.0
int_cw_marker = InteractiveMarker()
int_cw_marker.header.frame_id = "base_link"
int_cw_marker.name = "cw_marker"
int_cw_marker.description = "Clockwise Turn Control"
int_cw_marker.pose.position.y = -2
int_cw_marker.pose.orientation.w = 1
f_button_control = InteractiveMarkerControl()
f_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
f_button_control.always_visible = True
f_button_control.markers.append(f_marker)
int_f_marker.controls.append(f_button_control)
cw_marker = Marker()
cw_marker.type = Marker.SPHERE
cw_marker.pose.orientation.w = 2
cw_marker.scale.x = 0.45
cw_marker.scale.y = 0.45
cw_marker.scale.z = 0.45
cw_marker.color.r = 0.0
cw_marker.color.g = 0.5
cw_marker.color.b = 0.5
cw_marker.color.a = 1.0
cw_button_control = InteractiveMarkerControl()
cw_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
cw_button_control.always_visible = True
cw_button_control.markers.append(cw_marker)
int_cw_marker.controls.append(cw_button_control)
int_ccw_marker = InteractiveMarker()
int_ccw_marker.header.frame_id = "base_link"
int_ccw_marker.name = "ccw_marker"
int_ccw_marker.description = "Counter-Clockwise Turn Control"
int_ccw_marker.pose.position.y = 2
int_ccw_marker.pose.orientation.w = 1
ccw_marker = Marker()
ccw_marker.type = Marker.SPHERE
ccw_marker.pose.orientation.w = 1
ccw_marker.scale.x = 0.45
ccw_marker.scale.y = 0.45
ccw_marker.scale.z = 0.45
ccw_marker.color.r = 0.5
ccw_marker.color.g = 0.0
ccw_marker.color.b = 0.5
ccw_marker.color.a = 1.0
ccw_button_control = InteractiveMarkerControl()
ccw_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
ccw_button_control.always_visible = True
ccw_button_control.markers.append(ccw_marker)
int_ccw_marker.controls.append(ccw_button_control)
server.insert(int_f_marker, handle_f_input)
server.insert(int_cw_marker, handle_cw_input)
server.insert(int_ccw_marker, handle_ccw_input)
rospy.sleep(0.5)
server.applyChanges()
rospy.spin()
class World:
'''Object recognition and localization related stuff'''
tf_listener = None
objects = []
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 receive_ar_markers(self, data):
'''Callback function to receive marker info'''
self._lock.acquire()
if len(data.markers) > 0:
rospy.loginfo('Received non-empty marker list.')
for i in range(len(data.markers)):
marker = data.markers[i]
self._add_new_object(marker.pose.pose, self.marker_dims, marker.id)
self._lock.release()
def _reset_objects(self):
'''Function that removes all objects'''
self._lock.acquire()
for i in range(len(World.objects)):
self._im_server.erase(World.objects[i].int_marker.name)
self._im_server.applyChanges()
if self.surface != None:
self._remove_surface()
self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def get_tool_id(self):
if (len(self.objects) == 0):
rospy.warning('There are no fiducials, cannot get tool ID.')
return None
elif (len(self.objects) > 1):
rospy.warning('There are more than one fiducials, returning the first as tool ID.')
return World.objects[0].marker_id
def get_surface(self):
if (len(self.objects) < 4):
rospy.warning('There are not enough fiducials to detect surface.')
return None
elif (len(self.objects) > 4):
rospy.warning('There are more than four fiducials for surface, will use first four.')
return
points = [World.objects[0].position, World.objects[1].position,
World.objects[2].position, World.objects[3].position]
s = Surface(points)
return s
def receive_table_marker(self, marker):
'''Callback function for markers to determine table'''
if (marker.type == Marker.LINE_STRIP):
if (len(marker.points) == 6):
rospy.loginfo('Received a TABLE marker.')
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface,
self.marker_feedback_cb)
self._im_server.applyChanges()
def receive_object_info(self, object_list):
'''Callback function to receive object info'''
self._lock.acquire()
rospy.loginfo('Received recognized object list.')
if (len(object_list.graspable_objects) > 0):
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if (len(models) > 0):
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if (best_confidence < models[j].confidence):
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if (object_pose != None):
rospy.logwarn('Adding the recognized object ' +
'with most confident model.')
self._add_new_object(object_pose,
Vector3(0.2, 0.2, 0.2), i,
object_list.meshes[i])
else:
rospy.logwarn('... this is not a recognition result, ' +
'it is probably just segmentation.')
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if (cluster_pose != None):
rospy.loginfo('Adding unrecognized object with pose:' +
World.pose_to_string(cluster_pose) + '\n' +
'In ref frame' + str(bbox.pose.header.frame_id))
self._add_new_object(cluster_pose, bbox.box_dims, i)
else:
rospy.logwarn('... but the list was empty.')
self._lock.release()
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix'''
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 get_matrix_from_pose(pose):
'''Returns the transformation matrix for given 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_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state'''
if (arm_state.refFrame == ArmState.OBJECT):
arm_state_copy = ArmState(arm_state.refFrame,
Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameObject)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def _get_mesh_marker(marker, mesh):
'''Function that generated a marker from a mesh'''
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while (index + 2 < len(mesh.triangles)):
if ((mesh.triangles[index] < len(mesh.vertices))
and (mesh.triangles[index + 1] < len(mesh.vertices))
and (mesh.triangles[index + 2] < len(mesh.vertices))):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr('Mesh contains invalid triangle!')
break
return marker
def _add_new_object(self, pose, dimensions, id=None, mesh=None):
'''Function to add new objects'''
dist_threshold = 0.02
to_remove = None
for i in range(len(World.objects)):
if (World.pose_distance(World.objects[i].object.pose, pose)
< dist_threshold):
rospy.loginfo('Previously detected object at the same' +
'location, will not add this object.')
return False
n_objects = len(World.objects)
World.objects.append(WorldObject(pose, n_objects,
dimensions, id))
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
def _remove_object(self, to_remove):
'''Function to remove object by index'''
obj = World.objects.pop(to_remove)
rospy.loginfo('Removing object ' + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
def _remove_surface(self):
'''Function to request removing surface'''
rospy.loginfo('Removing surface')
self._im_server.erase('surface')
self._im_server.applyChanges()
self.surface = None
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose)
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + 0.06)
button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=index, scale=Vector3(0, 0, 0.03),
text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
scale=Vector3(0, 0, 0.03), text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def get_frame_list():
'''Function that returns the list of ref. frames'''
objects = []
for i in range(len(World.objects)):
objects.append(World.objects[i].object)
return objects
@staticmethod
def has_objects():
'''Function that checks if there are any objects'''
return len(World.objects) > 0
@staticmethod
def get_ref_from_name(ref_name):
'''Ref. frame type from ref. frame name'''
if ref_name == 'base_link':
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
'''Transforms an arm frame to a new ref. frame'''
if ref_frame == ArmState.ROBOT_BASE:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
pass
#rospy.logwarn('No reference frame transformations ' +
#'needed (both absolute).')
elif (arm_frame.refFrame == ArmState.OBJECT):
abs_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, 'base_link')
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
elif ref_frame == ArmState.OBJECT:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rel_ee_pose = World.transform(arm_frame.ee_pose,
'base_link', ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif (arm_frame.refFrame == ArmState.OBJECT):
if (arm_frame.refFrameObject.name == ref_frame_obj.name):
pass
#rospy.logwarn('No reference frame transformations ' +
#'needed (same object).')
else:
rel_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
@staticmethod
def has_object(object_name):
'''Checks if the world contains the object'''
for obj in World.objects:
if obj.object.name == object_name:
return True
return False
@staticmethod
def is_frame_valid(object_name):
'''Checks if the frame is valid for transforms'''
return (object_name == 'base_link') or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
''' Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms'''
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(from_frame,
to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(to_frame,
pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr('TF exception during transform.')
return pose
except rospy.ServiceException:
rospy.logerr('Exception during transform.')
return pose
else:
rospy.logwarn('One of the frame objects might not exist: ' +
from_frame + ' or ' + to_frame)
return pose
@staticmethod
def pose_to_string(pose):
'''For printing a pose to stdout'''
return ('Position: ' + str(pose.position.x) + ", " +
str(pose.position.y) + ', ' + str(pose.position.z) + '\n' +
'Orientation: ' + str(pose.orientation.x) + ", " +
str(pose.orientation.y) + ', ' + str(pose.orientation.z) +
', ' + str(pose.orientation.w) + '\n')
def _publish_tf_pose(self, pose, name, parent):
''' Publishes a TF for object pose'''
if (pose != None):
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
self._tf_broadcaster.sendTransform(pos, rot,
rospy.Time.now(), name, parent)
def update_object_pose(self):
''' Function to externally update an object pose'''
Response.perform_gaze_action(GazeGoal.LOOK_DOWN)
while (Response.gaze_client.get_state() == GoalStatus.PENDING or
Response.gaze_client.get_state() == GoalStatus.ACTIVE):
time.sleep(0.1)
if (Response.gaze_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not look down to take table snapshot')
return False
rospy.loginfo('Looking at table now.')
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
self._reset_objects()
if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not reset recognition.')
return False
# Do segmentation
goal = UserCommandGoal(UserCommandGoal.SEGMENT, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Table segmentation is complete.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not segment.')
return False
# Do recognition
goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Objects on the table have been recognized.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
# Record the result
if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED):
wait_time = 0
total_wait_time = 5
while (not World.has_objects() and wait_time < total_wait_time):
time.sleep(0.1)
wait_time += 0.1
if (not World.has_objects()):
rospy.logerr('Timeout waiting for a recognition result.')
return False
else:
rospy.loginfo('Got the object list.')
return True
else:
rospy.logerr('Could not recognize.')
return False
def clear_all_objects(self):
'''Removes all objects from the world'''
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('Successfully reset object localization pipeline.')
self._reset_objects()
self._remove_surface()
def get_nearest_object(self, arm_pose):
'''Gives a pointed to the nearest object'''
dist_threshold = 0.4
def chObj(cur, obj):
dist = World.pose_distance(obj.object.pose, arm_pose)
return (dist, obj.object) if (dist < cur[0]) else cur
return reduce(chObj, World.objects, (dist_threshold, None))[1]
@staticmethod
def pose_distance(pose1, pose2, is_on_table=True):
'''Distance between two world poses'''
if pose1 == [] or pose2 == []:
return 0.0
else:
if (is_on_table):
arr1 = array([pose1.position.x, pose1.position.y])
arr2 = array([pose2.position.x, pose2.position.y])
else:
arr1 = array([pose1.position.x,
pose1.position.y, pose1.position.z])
arr2 = array([pose2.position.x,
pose2.position.y, pose2.position.z])
dist = norm(arr1 - arr2)
if dist < 0.0001:
dist = 0
return dist
def marker_feedback_cb(self, feedback):
'''Callback for when feedback from a marker is received'''
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Clicked on object ' + str(feedback.marker_name))
rospy.loginfo('Number of objects ' + str(len(World.objects)))
else:
rospy.loginfo('Unknown event' + str(feedback.event_type))
def update(self):
'''Update function called in a loop'''
# Visualize the detected object
is_world_changed = False
self._lock.acquire()
if (World.has_objects()):
to_remove = None
for i in range(len(World.objects)):
self._publish_tf_pose(World.objects[i].object.pose,
World.objects[i].get_name(), 'base_link')
if (World.objects[i].is_removed):
to_remove = i
if to_remove != None:
self._remove_object(to_remove)
is_world_changed = True
self._lock.release()
return is_world_changed
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()
# 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 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.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]
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.')
joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
print 'joint: ' + str(joint_positions)
if(joint_positions != None):
self.toggle_arm(self.side_prefix, 'Freeze', False)
#joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
print 'returned from IK: ' + str(joint_positions)
#print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
#joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
print 'done'
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
print 'toggle'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities, time_from_start=rospy.Duration(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)
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]))
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
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_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)
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 POIServer(object):
'''
Node to act as a server to hold a list of points of interest which
can be modified by services or interactive markers
'''
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 transform_position(self, ps):
'''
Attempty to transform a PointStamped message into the global frame, returning
the position of the transformed point or None if transform failed.
'''
# If no frame, assume user wanted it in the global frame
if ps.header.frame_id == "":
return ps.point
try:
ps_tf = self.tf_buffer.transform(ps,
self.global_frame,
timeout=rospy.Duration(5))
return ps_tf.point
except tf2_ros.TransformException as e:
rospy.logwarn('Error transforming "{}" to "{}": {}'.format(
ps.header.frame_id, self.global_frame, e))
return None
def process_feedback(self, feedback):
'''
Process interactive marker feedback, moving markers internally inresponse to markers moved in RVIZ
'''
# Only look at changes when mouse button is unclicked
if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP:
return
# Transform new position into global frame
ps = PointStamped()
ps.header = feedback.header
ps.point = feedback.pose.position
position = self.transform_position(ps)
if position is None:
return
# Update position of marker
self.update_position(feedback.marker_name, feedback.pose.position)
@thread_lock(lock)
def save_to_param_cb(self, req):
rospy.set_param('~global_frame', self.global_frame)
d = {}
for poi in self.pois.pois:
d[poi.name] = [
float(poi.position.x),
float(poi.position.y),
float(poi.position.z)
]
rospy.set_param('~initial_pois', d)
return {'success': True}
def add_poi_cb(self, req):
'''
Callback for AddPOI service
'''
name = req.name
# If frame is empty, just initialize it to zero
if len(req.position.header.frame_id) == 0:
position = numpy_to_point([0., 0., 0.])
# Otherwise transform position into global frame
else:
position = self.transform_position(req.position)
if position is None:
return {'success': False, 'message': 'tf error (bad poi)'}
if not self.add_poi(name, position):
return {'success': False, 'message': 'alread exists (bad poi)'}
return {'success': True, 'message': 'good poi'}
def delete_poi_cb(self, req):
'''
Callback for DeletePOI service
'''
# Return error if marker did not exist
if not self.remove_poi(req.name):
return {'success': False, 'message': 'does not exist (bad poi)'}
return {'success': True, 'message': 'good poi'}
def move_poi_cb(self, req):
'''
Callback for MovePOI service
'''
name = req.name
# Transform position into global frame
position = self.transform_position(req.position)
if position is None:
return {'success': False, 'message': 'tf error (bad poi)'}
# Return error if marker did not exist
if not self.update_position(name, position):
return {'success': False, 'message': 'does not exist (bad poi)'}
return {'success': True, 'message': 'good poi'}
@thread_lock(lock)
def add_poi(self, name, position):
'''
Add a POI, updating clients / rviz when done
@return False if POI already exists
'''
if not self._add_poi(name, position):
return False
self.update()
return True
@thread_lock(lock)
def remove_poi(self, name):
'''
Remove a POI, updating clients / rviz when done
@return False if POI with name does not exist
'''
if not self._remove_poi(name):
return False
self.update()
return True
@thread_lock(lock)
def update_position(self, name, position):
'''
Update the position of a POI, updating clients / rviz when done
@param position: a Point message of the new position in global frame
@return False if POI with name does not exist
'''
if not self._update_position(name, position):
return False
self.update()
return True
def update(self):
'''
Update interactive markers server and POI publish of changes
'''
self.pois.header.stamp = rospy.Time.now()
self.pub.publish(self.pois)
self.interactive_marker_server.applyChanges()
def _update_position(self, name, position):
'''
Internal implementation of update_position, which is NOT thread safe and does NOT update clients of change
'''
# Find poi with specified name
for poi in self.pois.pois:
if poi.name == name:
pose = Pose()
pose.orientation.w = 1.0
pose.position = position
if not self.interactive_marker_server.setPose(name, pose):
return False
# Set pose in message
poi.position = position
return True
return False
def _remove_poi(self, name):
'''
Internal implementation of remove_poi, which is NOT thread safe and does NOT update clients of change
'''
# Return false if marker with that name not added to interactive marker server
if not self.interactive_marker_server.erase(name):
return False
# Find POI with specifed name and delete it from list
for i, poi in enumerate(self.pois.pois):
if poi.name == name:
del self.pois.pois[i]
return True
return False
def _add_poi(self, name, position):
'''
Internal implementation of add_poi, which is NOT thread safe and does NOT update clients of change
'''
if self.interactive_marker_server.get(name) is not None:
return False
poi = POI()
poi.name = name
poi.position = position
self.pois.pois.append(poi)
point_marker = Marker()
point_marker.type = Marker.SPHERE
point_marker.scale.x = self.marker_scale
point_marker.scale.y = self.marker_scale
point_marker.scale.z = self.marker_scale
point_marker.color.r = 1.0
point_marker.color.g = 1.0
point_marker.color.b = 1.0
point_marker.color.a = 1.0
text_marker = Marker()
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.orientation.w = 1.0
text_marker.pose.position.x = 1.5
text_marker.text = poi.name
text_marker.scale.z = 1.0
text_marker.color.r = 1.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
text_marker.color.a = 1.0
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.global_frame
int_marker.pose.orientation.w = 1.0
int_marker.pose.position = poi.position
int_marker.scale = 1
int_marker.name = poi.name
# insert a box
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.always_visible = True
control.markers.append(point_marker)
control.markers.append(text_marker)
int_marker.controls.append(control)
self.interactive_marker_server.insert(int_marker,
self.process_feedback)
return True
class InteractiveMarkerInterfaceServer:
def __init__(self):
rospy.init_node('interactive_marker_node')
self._server = InteractiveMarkerServer("interactive_marker_interface")
# create the interative markers
self._forward_marker = InteractiveMarker()
self._forward_marker.header.frame_id = 'base_link'
self._forward_marker.name = "forward_marker"
self._forward_marker.description = "Click to go forward 0.5m"
self._forward_marker.pose.position = Point(-0.5,0,-0.5)
self._forward_marker.pose.orientation.w = 1
self._cw_marker = InteractiveMarker()
self._cw_marker.header.frame_id = 'base_link'
self._cw_marker.name = "cw_marker"
self._cw_marker.description = "Click to turn 30 degrees clockwise"
self._cw_marker.pose.position = Point(-0.5,1,-0.5)
self._cw_marker.pose.orientation.w = 1
self._ccw_marker = InteractiveMarker()
self._ccw_marker.header.frame_id = 'base_link'
self._ccw_marker.name = "ccw_marker"
self._ccw_marker.description = "Click to turn 30 degrees counterclockwise"
self._ccw_marker.pose.position = Point(-0.5,-1,-0.5)
self._ccw_marker.pose.orientation.w = 1
# create a cube for the interactive marker
self._forward_box_marker = Marker(
type=Marker.CUBE,
id=1,
scale=Vector3(0.45, 0.45, 0.45),
pose=Pose(Point(-0.5,0,0),Quaternion(0,0,0,1)),
header=Header(frame_id='base_link', stamp = rospy.Time.now()),
color=ColorRGBA(0, 0.5, 0.5, 1.0))
self._cw_box_marker = Marker(
type=Marker.CUBE,
id=2,
scale=Vector3(0.45, 0.45, 0.45),
pose=Pose(Point(-0.5,1,0),Quaternion(0,0,0,1)),
header=Header(frame_id='base_link', stamp = rospy.Time.now()),
color=ColorRGBA(0, 0.5, 0.5, 1.0))
self._ccw_box_marker = Marker(
type=Marker.CUBE,
id=3,
scale=Vector3(0.45, 0.45, 0.45),
pose=Pose(Point(-0.5,-1,0),Quaternion(0,0,0,1)),
header=Header(frame_id='base_link', stamp = rospy.Time.now()),
color=ColorRGBA(0, 0.5, 0.5, 1.0))
# create a control for the interactive marker
self._forward_control = InteractiveMarkerControl()
self._forward_control.interaction_mode = InteractiveMarkerControl.BUTTON
self._forward_control.always_visible = True
self._forward_control.markers.append(self._forward_box_marker)
self._forward_marker.controls.append(self._forward_control)
self._cw_control = InteractiveMarkerControl()
self._cw_control.interaction_mode = InteractiveMarkerControl.BUTTON
self._cw_control.always_visible = True
self._cw_control.markers.append(self._cw_box_marker)
self._cw_marker.controls.append(self._cw_control)
self._ccw_control = InteractiveMarkerControl()
self._ccw_control.interaction_mode = InteractiveMarkerControl.BUTTON
self._ccw_control.always_visible = True
self._ccw_control.markers.append(self._ccw_box_marker)
self._ccw_marker.controls.append(self._ccw_control)
self._server.insert(self._forward_marker, self.handle_input)
self._server.insert(self._cw_marker, self.handle_input)
self._server.insert(self._ccw_marker, self.handle_input)
self._server.applyChanges()
self._base = robot_api.Base()
def handle_input(self, input):
if input.event_type == InteractiveMarkerFeedback.MOUSE_UP and input.marker_name == "forward_marker":
self._base.go_forward(0.5, speed=1)
elif input.event_type == InteractiveMarkerFeedback.MOUSE_UP and input.marker_name == "cw_marker":
self._base.turn(-30.0 * math.pi / 180.0, speed=1)
elif input.event_type == InteractiveMarkerFeedback.MOUSE_UP and input.marker_name == "ccw_marker":
self._base.turn(30.0 * math.pi / 180.0, speed=1)
else:
# rospy.loginfo('Cannot handle this InteractiveMarker event')
pass
class TopologyVisualiser(object):
"""Visualise topology graph in rviz (and edit it)"""
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 save(self):
with open(self.network_file, 'w') as file_obj:
node_dict = [node.to_dict() for node in self.nodes.values()]
yaml.dump({'nodes': node_dict}, file_obj, default_flow_style=False)
yaml.dump({'connections': self.connections},
file_obj,
default_flow_style=False)
def command_cb(self, msg):
command = msg.data
if 'add connection' in command or 'del connection' in command:
command_list = command.split()
if len(command_list) != 4:
self.print_command_warning()
return
try:
conn1 = int(command_list[2])
conn2 = int(command_list[3])
except Exception as e:
self.print_command_warning()
return
if conn1 not in self.nodes:
rospy.logwarn(command_list[2] + ' not in nodes list')
return
if conn1 not in self.nodes:
rospy.logwarn(command_list[2] + ' not in nodes list')
return
if command_list[0] == 'add':
self.connections.append([conn1, conn2])
rospy.loginfo('Added connection')
else:
connection = [conn1, conn2]
if connection in self.connections:
self.connections.remove(connection)
elif connection[::-1] in self.connections:
self.connections.remove(connection[::-1])
else:
rospy.logwarn(str(connection) + ' not in connections list')
return
rospy.loginfo('Deleted connection')
self._topology_pub.publish(self.get_marker_array())
rospy.sleep(0.5)
else:
self.print_command_warning()
def print_command_warning(self):
rospy.logwarn('Invalid command format.')
print('Valid commands are:')
for i in ["add connection", "del connection"]:
print('-', i)
print('Examples:')
print('- "add connection 123 234"')
print('- "del connection 123 234"')
def clicked_point_cb(self, msg):
rospy.loginfo(msg)
self.node_id_counter += 1
node_id = self.node_id_counter
self.nodes[node_id] = Node(node_id, msg.point.x, msg.point.y,
"BRSU_area", "area")
self._topology_pub.publish(self.get_marker_array())
self._add_interactive_marker(msg.point.x, msg.point.y, node_id)
rospy.loginfo('Added node successfully')
rospy.sleep(0.5)
def get_marker_array(self):
"""Return a MarkerArray object representing the graph formed by topology
nodes and connections
:returns: visualization_msgs.MarkerArray
"""
marker_array = MarkerArray()
for i, node in enumerate(self.nodes.values()):
marker_msg_text = Marker(type=Marker.TEXT_VIEW_FACING, id=node.id)
marker_msg_text.text = str(node.id) + " (" + str(
node.area_type) + ")"
marker_msg_text.scale.z = 0.5
marker_msg_text.color.a = 1.0
marker_msg_text.pose.position.x = node.x
marker_msg_text.pose.position.y = node.y
marker_msg_text.header.stamp = rospy.Time.now()
marker_msg_text.header.frame_id = self.frame
marker_array.markers.append(marker_msg_text)
for i, conn in enumerate(self.connections):
start_node = self.nodes[conn[0]]
end_node = self.nodes[conn[1]]
marker = Marker(type=Marker.LINE_STRIP, id=10000 + i)
marker.points.append(Point(x=start_node.x, y=start_node.y))
marker.points.append(Point(x=end_node.x, y=end_node.y))
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = self.frame
marker.color.b = marker.color.a = 1.0
marker.scale.x = 0.05
marker_array.markers.append(marker)
return marker_array
def _add_interactive_marker(self, x, y, node_id):
"""adds a interactive marker at the position.
:x: float
:y: float
:node_id: int or None
:returns: None
"""
marker = Utils.get_2_dof_interactive_marker(str(node_id), self.frame,
x, y)
self.interactive_marker_server.insert(marker,
self.interactive_marker_cb)
self.interactive_marker_server.applyChanges()
def interactive_marker_cb(self, feedback):
"""Updates the control point when interactive marker is moved.
:feedback: InteractiveMarkerFeedback
:returns: None
"""
if not feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
return
p = feedback.pose.position
node_id = int(feedback.marker_name)
if node_id in self.nodes:
self.nodes[node_id].x = round(p.x, 2)
self.nodes[node_id].y = round(p.y, 2)
else:
rospy.logerr("Incorrect interactive marker id " +
feedback.marker_name) # should never come here
self._topology_pub.publish(self.get_marker_array())
rospy.sleep(0.5)
class InteractiveMarkerManager(object):
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 _process_feedback(self, feedback):
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
self._process_menu_select(feedback)
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: # TODO: I really want to use the other type of feedback
rospy.logdebug(s + ": pose changed")
self._update_waypoint_position(feedback.marker_name, feedback.pose)
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
pass
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
pass
self.server.applyChanges()
def _process_menu_select(self, feedback):
menu_entry_id = feedback.menu_entry_id
if menu_entry_id == self._menu_cmds['del_wp']:
self._delete_waypoint(feedback)
elif menu_entry_id == self._menu_cmds['ins_wp_before']:
self._insert_waypoint(feedback, before=True)
elif menu_entry_id == self._menu_cmds['ins_wp_after']:
self._insert_waypoint(feedback, before=False)
elif menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']:
self._process_menu_time_cmds(feedback)
else:
pass
def _delete_waypoint(self, feedback):
marker = self.name_to_marker_dict[feedback.marker_name]
waypoint = self.name_to_waypoint_dict[marker.name]
self.parent_instance_handle.delete_waypoint(waypoint)
# Update dictionaries
del self.name_to_marker_dict[marker.name]
del self.waypoint_to_marker_dict[waypoint]
del self.name_to_waypoint_dict[marker.name]
del self._menu_handlers[marker.name]
del self._menu_cmds[marker.name]
# Erase marker from server
self.server.erase(feedback.marker_name)
self.server.applyChanges()
self.marker_cnt -=1
self._int_marker_name_list.remove(marker.name)
self._update_wp_marker_descriptions()
self.server.applyChanges()
def _insert_waypoint(self, feedback, before=True):
ref_marker = self.name_to_marker_dict[feedback.marker_name]
ref_waypoint = self.name_to_waypoint_dict[ref_marker.name]
self.parent_instance_handle.insert_waypoint(ref_waypoint, before=before)
self._update_wp_marker_descriptions()
self.server.applyChanges()
def _process_menu_time_cmds(self, feedback):
menu_entry_handle = feedback.menu_entry_id
menu_handler = self._menu_handlers[feedback.marker_name]
check_state = menu_handler.getCheckState(menu_entry_handle)
if check_state == MenuHandler.UNCHECKED:
menu_handler.setCheckState(menu_entry_handle, MenuHandler.CHECKED)
# Uncheck all other menu entries
for menu_entry_id in self._menu_cmds[feedback.marker_name]['menu_time_cmds']:
if menu_entry_id != menu_entry_handle:
menu_handler.setCheckState(menu_entry_id, MenuHandler.UNCHECKED)
# Update waypoints
waypoint = self.name_to_waypoint_dict[feedback.marker_name]
waypoint.time = self._menu_cmds[feedback.marker_name]['menu_time_cmds'][menu_entry_handle]
# Apply marker changes
menu_handler.reApply(self.server)
self.server.applyChanges()
def _update_waypoint_pose(self):
# TODO maybe handle pose changes differently?
pass
def _update_waypoint_position(self, marker_name, marker_pose):
waypoint = self.name_to_waypoint_dict[marker_name]
eff_pos = marker_pose.position
eff_orient = marker_pose.orientation
target_jt_angles = self.ik_solver.get_ik(waypoint.joint_state.positions,
eff_pos.x, eff_pos.y, eff_pos.z,
eff_orient.w, eff_orient.x, eff_orient.y, eff_orient.z,
self.ik_position_xyz_bounds[0],
self.ik_position_xyz_bounds[1],
self.ik_position_xyz_bounds[2],
self.ik_position_rpy_bounds[0],
self.ik_position_rpy_bounds[1],
self.ik_position_rpy_bounds[2])
if target_jt_angles is not None:
waypoint.end_effector_pose = copy.deepcopy(marker_pose)
waypoint.joint_state.positions = target_jt_angles
self._change_wp_marker_color(marker_name, "white", 0.9)
else:
self._change_wp_marker_color(marker_name, "red", 0.9)
def _change_wp_marker_color(self, wp_marker_name, color, opacity=1.0):
assert 0.0 <= opacity <= 1.0
# set color
int_marker = self.server.get(wp_marker_name)
marker = int_marker.controls[0].markers[0]
replacement_int_marker = copy.deepcopy(int_marker)
replacement_marker = replacement_int_marker.controls[0].markers[0]
rgb = None
if color == "white":
rgb = (1.0, 1.0, 1.0)
elif color == "red":
rgb = (1.0, 0.0, 0.0)
# TODO: Additional colors here
changed = False
if rgb is not None:
if marker.color.r != rgb[0]:
replacement_marker.color.r = rgb[0]
changed = True
print("change to ", rgb)
if marker.color.g != rgb[1]:
replacement_marker.color.g = rgb[1]
changed = True
if marker.color.b != rgb[2]:
replacement_marker.color.b = rgb[2]
changed = True
if marker.color.a != opacity:
replacement_marker.color.a = opacity
changed = True
# update wp marker
if changed:
self._replace_wp_marker(int_marker, replacement_int_marker)
def _update_wp_marker_descriptions(self):
for index, marker_name in enumerate(self._int_marker_name_list):
marker = self.name_to_marker_dict[marker_name]
marker_description = int(marker.description)
if marker_description != index+1:
marker.description = str(index+1)
replacement_marker = copy.deepcopy(self.server.get(marker.name))
replacement_marker.description = marker.description
self._replace_wp_marker(marker, replacement_marker)
def _replace_wp_marker(self, old_int_marker, replacement_int_marker):
# Update dictionaries
self.name_to_marker_dict[old_int_marker.name] = replacement_int_marker
self.waypoint_to_marker_dict[self.name_to_waypoint_dict[old_int_marker.name]] = replacement_int_marker
# Erase marker from server
self.server.erase(old_int_marker.name)
self.server.insert(replacement_int_marker, self._process_feedback)
self.server.applyChanges()
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)
def clear_waypoint_markers(self):
self.server.clear()
self.server.applyChanges()
self._menu_handlers = {}
self._menu_cmds = {}
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 = {}
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 Projection:
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 setup_marker(self, frame="velodyne", name = "capture vehicle", translation=True):
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.name = name
int_marker.description = name
int_marker.scale = 3
marker_control = InteractiveMarkerControl()
marker_control.always_visible = True
marker_control.markers.append(self.marker)
int_marker.controls.append(marker_control)
control = InteractiveMarkerControl()
control.name = "rotate_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "rotate_z"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "rotate_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
if not translation :
#int_marker.pose.position = Point(0,0,0)
return int_marker
control = InteractiveMarkerControl()
control.name = "move_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "move_z"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "move_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
def add_bbox(self):
inputName = '/image_raw'
rospy.Subscriber(inputName, Image, self.handle_img_msg, queue_size=1)
def handle_img_msg(self, img_msg):
now = rospy.get_rostime()
img = None
bridge = cv_bridge.CvBridge()
try:
img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr( 'image message to cv conversion failed :' )
rospy.logerr( e )
print( e )
return
self.frame_index = self.timestamp_map[img_msg.header.stamp.to_nsec()]
f = self.frame_map[self.frame_index][0]
obs_centroid = np.array(f.trans) + np.array(self.offset)
R = tf.transformations.quaternion_matrix(self.rotation_offset)
rotated_centroid = R.dot(list(obs_centroid)+[1])
obs_centroid = rotated_centroid[:3]
#self.orient = list(f.rotq)
self.marker.header.stamp = now
self.marker.pose.position = Point(*list(obs_centroid))
self.marker.pose.orientation = Quaternion(*self.orient)
self.obs_marker.pose.position = Point(*list(obs_centroid))
self.obs_marker.pose.orientation = Quaternion(*self.orient)
self.add_bbox_lidar()
md = self.metadata
dims = np.array([md['l'], md['w'], md['h']])
outputName = '/image_bbox'
if obs_centroid is None:
rospy.loginfo("Couldn't find obstacle centroid")
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# print centroid info
rospy.loginfo(str(obs_centroid))
# case when obstacle is not in camera frame
if obs_centroid[0]<2.5 :
self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# get bbox
R = tf.transformations.quaternion_matrix(self.orient)
corners = [0.5*np.array([i,j,k])*dims for i in [-1,1]
for j in [-1,1] for k in [-1,1]]
corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners]
projected_pts = []
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(img, projected_pts)
self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
def processFeedback(self, feedback ):
p = feedback.pose.orientation
self.rotation_offset = [p.x, p.y, p.z, p.w]
p = feedback.pose.position
self.offset = [p.x, p.y, p.z]
self.server.applyChanges()
def obs_processFeedback(self, feedback ):
p = feedback.pose.orientation
self.orient = (p.x, p.y, p.z, p.w)
self.marker.pose.orientation = Quaternion(*self.orient)
self.server.applyChanges()
def add_bbox_lidar(self):
#if obs_centroid is None :
# return
now = rospy.get_rostime()
self.velodyne_marker.header.stamp = now
self.obs_marker.header.stamp = now
# tell the server to call processFeedback() when feedback arrives for it
self.server.insert(self.velodyne_marker, self.processFeedback)
self.server.applyChanges()
self.server.insert(self.obs_marker, self.obs_processFeedback)
self.server.applyChanges()
class InteractiveMarkerGoal(object):
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 transformPose(self, target_frame, pose, time=None):
transform = self.tfBuffer.lookup_transform(
target_frame, pose.header.frame_id,
pose.header.stamp if time is not None else rospy.Time(0),
rospy.Duration(1.0))
new_pose = do_transform_pose(pose, transform)
return new_pose
def makeBox(self, msg):
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = msg.scale * 0.2
marker.scale.y = msg.scale * 0.2
marker.scale.z = msg.scale * 0.2
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeBox(msg))
msg.controls.append(control)
return control
def make6DofMarker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
p = PoseStamped()
p.header.frame_id = tip_link
p.pose.orientation.w = 1
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.pose = self.transformPose(root_link, p).pose
int_marker.header.frame_id = root_link
int_marker.scale = .2
int_marker.name = "eef_{}_to_{}".format(root_link, tip_link)
# insert a box
self.makeBoxControl(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: "MOVE_3D",
InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D",
InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D"
}
int_marker.name += "_" + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
class process_feedback(object):
def __init__(self, i_server, client, root_link, tip_link):
self.i_server = i_server
self.client = client
self.tip_link = tip_link
self.root_link = root_link
def __call__(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
print('interactive goal update')
goal = ControllerListGoal()
goal.type = ControllerListGoal.STANDARD_CONTROLLER
# asd = 'asd'
# translation
controller = Controller()
controller.type = Controller.TRANSLATION_3D
controller.tip_link = self.tip_link
controller.root_link = self.root_link
controller.goal_pose.header = feedback.header
controller.goal_pose.pose = feedback.pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.05
controller.weight = 1.0
goal.controllers.append(controller)
# rotation
controller = Controller()
controller.type = Controller.ROTATION_3D
controller.tip_link = self.tip_link
controller.root_link = self.root_link
controller.goal_pose.header = feedback.header
controller.goal_pose.pose = feedback.pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.2
controller.weight = 1.0
goal.controllers.append(controller)
self.client.send_goal(goal)
self.i_server.applyChanges()
class TrajectoryAnalyzer():
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)
def get_poses_persecond(self):
average_poses = 0
for uuid in self._traj:
traj = self._traj[uuid]
inner_counter = 1
outer_counter = 1
prev_sec = traj.secs[0]
for i, sec in enumerate(traj.secs[1:]):
if prev_sec == sec:
inner_counter += 1
else:
prev_sec = sec
outer_counter += 1
average_poses += round(inner_counter/outer_counter)
return round(average_poses/len(self._traj))
def _retrieve_logs(self):
logs = self._client.message_store.people_perception.find()
for log in logs:
for i, uuid in enumerate(log['uuids']):
if uuid not in self._traj:
t = Trajectory(uuid)
t.append_pose(log['people'][i],
log['header']['stamp']['secs'],
log['header']['stamp']['nsecs'])
self._traj[uuid] = t
else:
t = self._traj[uuid]
t.append_pose(log['people'][i],
log['header']['stamp']['secs'],
log['header']['stamp']['nsecs'])
def visualize_trajectories(self, mode="all", average_length=0,
longest_length=0):
counter = 0
for uuid in self._traj:
if len(self._traj[uuid].pose) > 1:
if mode == "average":
if abs(self._traj[uuid].length - average_length) \
< (average_length / 10):
self.visualize_trajectory(self._traj[uuid])
counter += 1
elif mode == "longest":
if abs(self._traj[uuid].length - longest_length) \
< (longest_length / 10):
self.visualize_trajectory(self._traj[uuid])
counter += 1
elif mode == "shortest":
if self._traj[uuid].length < 1:
self.visualize_trajectory(self._traj[uuid])
counter += 1
else:
self.visualize_trajectory(self._traj[uuid])
counter += 1
rospy.loginfo("Total Trajectories: " + str(len(self._traj)))
rospy.loginfo("Printed trajectories: " + str(counter))
self.delete_trajectory(self._traj[uuid])
def _update_cb(self, feedback):
return
def visualize_trajectory(self, traj):
int_marker = self.create_trajectory_marker(traj)
self._server.insert(int_marker, self._update_cb)
self._server.applyChanges()
def delete_trajectory(self, traj):
self._server.erase(traj.uuid)
self._server.applyChanges()
def create_trajectory_marker(self, traj):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = traj.uuid
# int_marker.description = traj.uuid
pose = Pose()
pose.position.x = traj.pose[0]['position']['x']
pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.05
# random.seed(traj.uuid)
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
MOD = 3
for i, point in enumerate(traj.pose):
if i % MOD == 0:
x = point['position']['x']
y = point['position']['y']
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
line_marker.points.append(p)
line_marker.colors = []
for i, vel in enumerate(traj.vel):
if i % MOD == 0:
color = ColorRGBA()
if traj.max_vel == 0:
val = vel / 0.01
else:
val = vel / traj.max_vel
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def trajectory_visualization(self, mode):
average_length = 0
longest_length = -1
short_trajectories = 0
average_max_vel = 0
highest_max_vel = -1
minimal_frame = self.get_poses_persecond() * 5
for k, v in self._traj.items():
v.sort_pose()
v.calc_stats()
# Delete non-moving objects
if (v.max_vel < 0.1 or v.length < 0.1) and k in self._traj:
del self._traj[k]
# Delete trajectories that appear less than 5 seconds
if len(v.pose) < minimal_frame and k in self._traj:
del self._traj[k]
for k, v in self._traj.iteritems():
average_length += v.length
average_max_vel += v.max_vel
if v.length < 1:
short_trajectories += 1
if longest_length < v.length:
longest_length = v.length
if highest_max_vel < v.max_vel:
highest_max_vel = v.max_vel
average_length /= len(self._traj)
average_max_vel /= len(self._traj)
rospy.loginfo("Average length of tracks is " + str(average_length))
rospy.loginfo("Longest length of tracks is " + str(longest_length))
rospy.loginfo("Short trajectories are " + str(short_trajectories))
rospy.loginfo("Average maximum velocity of tracks is " +
str(average_max_vel))
rospy.loginfo("Highest maximum velocity of tracks is " +
str(highest_max_vel))
self.visualize_trajectories(mode, average_length, longest_length)
class InteractiveMarkerPoseStampedPublisher():
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 processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.loginfo(s + ": menu item " + str(feedback.menu_entry_id) +
" clicked" + mp + ".")
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.loginfo(s + ": pose changed")
ps = PoseStamped()
ps.header.frame_id = self.frame_id
ps.pose = feedback.pose
self.pub.publish(ps)
# TODO
# << "\nposition = "
# << feedback.pose.position.x
# << ", " << feedback.pose.position.y
# << ", " << feedback.pose.position.z
# << "\norientation = "
# << feedback.pose.orientation.w
# << ", " << feedback.pose.orientation.x
# << ", " << feedback.pose.orientation.y
# << ", " << feedback.pose.orientation.z
# << "\nframe: " << feedback.header.frame_id
# << " time: " << feedback.header.stamp.sec << "sec, "
# << feedback.header.stamp.nsec << " nsec" )
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.loginfo(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.loginfo(s + ": mouse up" + mp + ".")
self.server.applyChanges()
def makeArrow(self, msg):
marker = Marker()
marker.type = Marker.ARROW
marker.scale.x = 0.15 # msg.scale * 0.3
marker.scale.y = 0.08 # msg.scale * 0.1
marker.scale.z = 0.03 # msg.scale * 0.03
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.frame_id
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "" # "EEF 6DOF control"
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
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)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
int_marker.controls.append(control)
# self.menu_handler.insert("Do stuff", callback=self.processFeedback)
# This makes the floating text appear
# make one control using default visuals
# control = InteractiveMarkerControl()
# control.interaction_mode = InteractiveMarkerControl.MENU
# control.description="Menu"
# control.name = "menu_only_control"
# int_marker.controls.append(copy.deepcopy(control))
# marker = self.makeArrow( int_marker )
# control.markers.append( marker )
# control.always_visible = False
# int_marker.controls.append(control)
self.server.insert(int_marker, self.processFeedback)
class FakeMarkerServer():
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')
#Service: rosservice call /fake_object_markers/print_templates
#Returns a string of template names
#ex) list: ['test_template1','test_template2','test_template3']
def get_templates(self, req):
temp_list = []
if self.templates.keys() is None:
self.publish_feedback('No templates')
return
for obj in self.templates.keys():
temp_list.append(str(obj))
#print temp_list
return PrintTemplatesResponse(temp_list)
def store_grasp(self, msg):
self.last_grasp = msg.result.grasp
# Given a mesh_id creates a name with format 'object + mesh_id'
# ex.)Given '1234', creates 'object_1234' name
def create_name(self, mesh_id):
return 'object_' + str(mesh_id)
# Creates a mesh of the given object with the given pose to be visualized by template maker
def create_mesh(self, mesh_id, pose):
response = self.get_mesh(mesh_id)
mesh = response.mesh
# build the mesh marker
marker = Marker()
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.66
marker.frame_locked = False
marker.type = Marker.TRIANGLE_LIST
# add the mesh
for j in range(len(mesh.triangles)):
marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[0]])
marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[1]])
marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[2]])
# create the interactive marker
name = self.create_name(mesh_id)
self.server.insert(self.create_im(marker, pose, name), self.process_feedback)
self.server.setCallback(name, self.release, InteractiveMarkerFeedback.MOUSE_UP)
self.server.applyChanges()
# Creates an interactive marker
# - at the given location and pose
# - with a given name
# - for given marker object
def create_im(self, marker, pose, name):
# create the new interactive marker
int_marker = InteractiveMarker()
int_marker.pose = copy.deepcopy(pose)
int_marker.header.frame_id = 'base_link'
int_marker.name = name
# move freely on the X-Y plane
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.markers.append(marker)
control.always_visible = True
int_marker.controls.append(control)
return int_marker
def process_feedback(self, feedback):
self.last_feedback = feedback
# Returns true if given (x,y) coordinates are within "Graspable/Placeable(?)" range
def check_pose(self, x, y):
return x >= OFFSET + DEPTH_START and x <= OFFSET + DEPTH_END and y >= WIDTH_START and y <= WIDTH_END
# Checks position of hallucinated interactive markers
# Changes color and sets position when user releases mouse click (MOUSE_UP) on object
def release(self, feedback):
im = self.server.get(feedback.marker_name)
# copy the mesh information
marker = copy.deepcopy(im.controls[0].markers[0])
# determine color based on pose
if self.check_pose(feedback.pose.position.x, feedback.pose.position.y):
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.66
else:
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.66
# create the new interactive marker
self.server.insert(self.create_im(marker, feedback.pose, feedback.marker_name), self.process_feedback)
self.server.setCallback(feedback.marker_name, self.release, InteractiveMarkerFeedback.MOUSE_UP)
self.server.applyChanges()
# updates server
def update(self):
self.server.applyChanges()
# **Run by save_template service**
# Saves given template information to file location
def save(self, req):
# go through each object and check if they are in the valid range
to_save = []
for obj_id in self.objects:
im = self.server.get(self.create_name(obj_id))
pose = im.pose
if (self.check_pose(pose.position.x, pose.position.y)):
to_save.append(copy.deepcopy(im))
# check if we have anything to save
if len(to_save) > 0:
self.templates[req.name] = to_save
# PICKLE IT!
pickle.dump(self.templates, open(SAVE_FILE, 'wb'))
self.play('/home/rctoris/wav/t3_affirmative.wav')
self.reset_objects()
return SaveTemplateResponse(True)
else:
return SaveTemplateResponse(False)
# Publishes feedback of current tasks
def publish_feedback(self, msg):
rospy.loginfo(msg)
self.load_server.publish_feedback(LoadFeedback(msg))
# Publishes final result of action
def publish_result(self, msg):
rospy.loginfo(msg)
self.load_server.set_succeeded(LoadResult(msg))
# Returns how many objects were recognized
def proc_grasp_list(self, msg):
objects = []
# start by going through each
size = len(msg.graspable_objects)
for i in range(size):
obj = msg.graspable_objects[i]
# only take recognized objects
if len(obj.potential_models) is not 0:
objects.append(copy.deepcopy(obj))
rospy.loginfo('Found ' + str(len(objects)) + ' object(s).')
self.recognition = objects
# Drives to and aligns with counter
# Segments objects
def look_for_objects(self):
self.publish_feedback('Driving robot to counter')
# drive the robot
nav_goal = navigateGoal('Snack Nav', True)
self.nav.send_goal_and_wait(nav_goal)
res = self.nav.get_result()
if not res.success:
self.publish_feedback('Counter alignment failed.')
return False
self.publish_feedback('Aligned robot to counter')
self.publish_feedback('Looking for objects')
self.play('/home/rctoris/wav/GLaDOS_10_part1_entry-2.wav')
self.recognition = None
# Segment the table
self.segclient.send_goal(UserCommandGoal(request=1,interactive=False))
self.segclient.wait_for_result()
while self.recognition is None:
time.sleep(1)
# Recognize objects
self.recognition = None
self.segclient.send_goal(UserCommandGoal(request=2,interactive=False))
self.segclient.wait_for_result()
while self.recognition is None:
time.sleep(1)
return True
# **Run by load_template service**
# Identifies remaining objects needed in template
# Moves to and aligns with counter
# Scans and recognizes objects on counter that match template
# Picks up one object
# Backs up from counter
# Drives to table
# Places object in given template location
# Repeats
def load(self, goal):
name = goal.name
self.publish_feedback('Loading template ' + name)
# if requested template does not exist...
if name not in self.templates.keys():
self.publish_result(name + ' template does not exist')
return
template = copy.deepcopy(self.templates[name])
self.publish_feedback('Loaded template ' + name)
self.play('/home/rctoris/wav/help.wav')
# look for any objects we need
while len(template) is not 0:
pickup_arm = None
# if it does not see any objects/could not drive to counter
if not self.look_for_objects():
self.publish_result('Object looking failed.')
return
# for each object in template array...
for template_im in template:
# for each recognized object
for rec_obj in self.recognition:
if template_im.name == self.create_name(rec_obj.potential_models[0].model_id):
# create the object info for it
obj_info = self.create_object_info(rec_obj)
# pick it up
pickup_arm = self.pickup(rec_obj)
# if neither arm can could pick up object...
if pickup_arm is None:
self.publish_result('Pickup failed.')
return
# make sure we have a grasp
self.publish_feedback('Waiting for grasp')
while self.last_grasp is None:
rospy.sleep(1)
# store the grasp
obj_info.grasp = self.last_grasp
self.last_grasp = None
self.publish_feedback('Grasp found')
# good job robot, place that object
to_place = Pose()
# location of object in template on table
to_place.position.z = TABLE_HEIGHT - PLACE_HEIGHT_OFFSET
to_place.position.x = template_im.pose.position.x
to_place.position.y = template_im.pose.position.y
placed = self.place_object(obj_info, pickup_arm, to_place)
# if the object could not be placed
if not placed:
self.publish_result('Place failed.')
return
self.publish_feedback('Placed the object!')
if len(template) is not 1:
self.play('/home/rctoris/wav/ill-be-back.wav')
# removes object from list of objects to pick up from template
template.remove(template_im)
# if no objects are found...
if pickup_arm is None:
# No objects found :(
self.publish_result('No objects found that we need :(')
return
# We completed the task!
self.play('/home/rctoris/wav/down.wav')
self.publish_result('Great success!')
# resets collision map of world and rescan
def reset_collision_map(self):
self.publish_feedback('Reseting collision map')
goal = IMGUIGoal()
goal.command.command = 3
goal.options.reset_choice = 4
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
self.publish_feedback('Collision map reset')
# reset hallucinated interactive marker objects' positions on visualized table
def reset_objects(self):
pose = Pose()
pose.position.z = TABLE_HEIGHT
pose.position.x = OFFSET * 3
pose.position.y = -0.25
for obj_id in self.objects:
self.create_mesh(obj_id, pose)
pose.position.y = pose.position.y + 0.25
# Picks up object that matches obj
def pickup(self, obj):
# start by picking up the object
options = IMGUIOptions()
options.collision_checked = True
options.grasp_selection = 1
options.adv_options.lift_steps = 10
options.adv_options.retreat_steps = 10
options.adv_options.reactive_force = False
options.adv_options.reactive_grasping = False
options.adv_options.reactive_place = False
options.adv_options.lift_direction_choice = 0
# check which arm is closer
if obj.potential_models[0].pose.pose.position.y > 0:
options.arm_selection = 1
else:
options.arm_selection = 0
goal = IMGUIGoal()
goal.options = options
goal.options.grasp_selection = 1
goal.options.selected_object = obj
goal.command.command = goal.command.PICKUP
# send it to IMGUI
self.publish_feedback('Attempting to pick up')
self.reset_collision_map()
self.imgui.send_goal(goal)
self.play('/home/rctoris/wav/humnbehv.wav')
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
# try the other arm
if options.arm_selection is 0:
options.arm_selection = 1
else:
options.arm_selection = 0
self.publish_feedback('Initial pickup failed, trying other arm')
self.reset_collision_map()
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
return None
else:
# now check if feedback to see if we actually got it
if options.arm_selection is 0:
arm = 'right'
else:
arm = 'left'
self.publish_feedback('Checking if object was grasped')
resp = self.grasp_check(arm)
if resp.isGrasping is True:
self.publish_feedback('Object was grasped')
# attempt to back up
backup_goal = BackupGoal()
self.backup.send_goal_and_wait(backup_goal)
res = self.backup.get_result()
# if robot could not back up
if not res.success:
self.publish_feedback('Backup failed.')
return None
return options.arm_selection
else:
self.move_arm_to_side(options.arm_selection)
return None
# moves arm to sides
def move_arm_to_side(self, arm_selection):
goal = IMGUIGoal()
goal.command.command = 4
goal.options.arm_selection = arm_selection
goal.options.arm_action_choice = 0
goal.options.arm_planner_choice = 1
self.publish_feedback('Moving arm to the side using planner')
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
# try open loop
self.publish_feedback('Planned arm move failed, trying open loop')
goal.options.arm_planner_choice = 0
self.imgui.send_goal(goal)
self.imgui.wait_for_result()
# check the result
res = self.imgui.get_result()
if res.result.value is not 1:
self.publish_feedback('Arm move failed.')
return False
else:
self.publish_feedback('Arm move was successful')
return True
# place object in given arm to given pose
def place_object(self, obj_info_orig, arm_selection, pose):
#drive to the table
self.publish_feedback('Driving robot to table')
nav_goal = navigateGoal('Dining Table Nav', True)
self.play('/home/rctoris/wav/run.wav')
self.nav.send_goal_and_wait(nav_goal)
res = self.nav.get_result()
if not res.success:
self.publish_feedback('Table alignment failed.')
return False
self.publish_feedback('Aligned robot to table')
self.reset_collision_map()
self.publish_feedback('Attempting to place the object')
# make a copy
obj_info = copy.deepcopy(obj_info_orig)
obj = obj_info.obj
goal = PlaceGoal()
# set the arm
if arm_selection is 0:
goal.arm_name = 'right_arm'
prefix = 'r'
else:
goal.arm_name = 'left_arm'
prefix = 'l'
# rotate and "gu-chunk"
orig_z = pose.position.z
pose.orientation.x = 0
pose.orientation.y = 0
goal.place_locations = []
#iterating through possible x-locations to place object
for x in range(0, 10):
pose.position.x = pose.position.x + ((x - 5) * 0.0025)
#iterating through possible y-locations to place object
for y in range(0, 10):
pose.position.y = pose.position.y + ((y - 5) * 0.0025)
# 'i' is for some rotations
for i in range(0, 10):
rads = (pi * (i/10.0))
pose.orientation.z = sin(-rads/2.0)
pose.orientation.w = cos(-rads/2.0);
# 'j' is for the 'z' height
for j in range (0, 6):
pose.position.z = orig_z + (j * 0.0025)
pose_mat = pose_to_mat(pose)
to_base_link_mat = pose_mat * obj_info.obj_origin_to_bounding_box
grasp_mat = pose_to_mat(obj_info.grasp.grasp_pose.pose)
gripper_mat = to_base_link_mat * grasp_mat
gripper_pose = stamp_pose(mat_to_pose(gripper_mat), 'base_link')
goal.place_locations.append(gripper_pose)
# use the identity as the grasp
obj_info.grasp.grasp_pose.pose = Pose()
obj_info.grasp.grasp_pose.pose.orientation.w = 1
goal.grasp = obj_info.grasp
goal.desired_retreat_distance = 0.1
goal.min_retreat_distance = 0.05
# set the approach
goal.approach = GripperTranslation()
goal.approach.desired_distance = .1
goal.approach.min_distance = 0.05
goal.approach.direction = create_vector3_stamped([0. , 0. , -1.], 'base_link')
# set the collision info
goal.collision_object_name = obj.collision_name
goal.collision_support_surface_name = 'table'
goal.place_padding = 0.0
goal.additional_link_padding = self.create_gripper_link_padding(prefix)
goal.collision_support_surface_name = 'all'
goal.allow_gripper_support_collision = True
goal.use_reactive_place = False
# send the goal
self.place.send_goal(goal)
# wait for result
finished_within_time = self.place.wait_for_result(rospy.Duration(240))
if not finished_within_time:
self.place.cancel_goal()
return False
# check the result
res = self.place.get_result()
if res.manipulation_result.value == -6 or res.manipulation_result.value == 1:
# attempt to back up
backup_goal = BackupGoal()
self.backup.send_goal_and_wait(backup_goal)
backup_res = self.backup.get_result()
# if robot could not back up
if not backup_res.success:
self.publish_feedback('Backup failed.')
return False
self.move_arm_to_side(arm_selection)
return True
else:
return False
def create_gripper_link_padding(self, prefix):
link_name_list = ['_gripper_palm_link', '_gripper_r_finger_tip_link', '_gripper_l_finger_tip_link',
'_gripper_l_finger_link', '_gripper_r_finger_link', '_wrist_roll_link']
pad = 0.
arm_link_names = [prefix + link_name for link_name in link_name_list]
link_padding_list = [LinkPadding(link_name, pad) for link_name in arm_link_names]
return link_padding_list
def create_object_info(self, obj):
# get the pose
pose_stamped = obj.potential_models[0].pose
# change the frame
obj_frame_pose_stamped = change_pose_stamped_frame(self.tf_listener, pose_stamped, obj.reference_frame_id)
return ObjectInfo(obj, obj_frame_pose_stamped, self.tf_listener)
class InteractiveMarkerPoseStampedPublisher():
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 processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) +
" clicked" + mp + ".")
if feedback.menu_entry_id == 1:
rospy.logdebug("Start publishing transform...")
if self.tf_br is None:
self.tf_br = tf.TransformBroadcaster()
elif feedback.menu_entry_id == 2:
rospy.logdebug("Stop publishing transform...")
self.tf_br = None
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.logdebug(s + ": pose changed")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
# TODO: Print here the commands
# tf static transform
self.print_commands(feedback.pose)
self.last_pose = deepcopy(feedback.pose)
self.server.applyChanges()
def print_commands(self, pose, decimals=4):
p = pose.position
o = pose.orientation
print "\n---------------------------"
print "Static transform publisher command (with roll pitch yaw):"
common_part = "rosrun tf static_transform_publisher "
pos_part = str(round(p.x, decimals)) + " " + str(round(
p.y, decimals)) + " " + str(round(p.z, decimals))
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
ori_part = str(round(r, decimals)) + " " + str(round(
p, decimals)) + " " + str(round(y, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Static transform publisher command (with quaternion):"
ori_q = str(round(o.x, decimals)) + " " + str(round(
o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(
round(o.w, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Roslaunch line of static transform publisher (rpy):"
node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf"
roslaunch_part = ' <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\
pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />'
print roslaunch_part
print
print "URDF format:" # <origin xyz="0.04149 -0.01221 0.001" rpy="0 0 0" />
print ' <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />'
print "\n---------------------------"
def makeArrow(self, msg):
marker = Marker()
# An arrow that's squashed to easier view the orientation on roll
marker.type = Marker.ARROW
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
def run(self):
r = pi / 2.0
p = pi
y = pi / 2.0
o = quaternion_from_euler(r, p, y)
r = rospy.Rate(20)
while not rospy.is_shutdown():
if self.tf_br is not None:
pos = self.last_pose.position
ori = self.last_pose.orientation
self.tf_br.sendTransform(
(pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w),
rospy.Time.now(), self.to_frame, self.from_frame)
ps = PoseStamped()
ps.pose = self.last_pose
ps.header.frame_id = self.from_frame
ps.header.stamp = rospy.Time.now()
self.pub.publish(ps)
r.sleep()
class Projection:
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 setup_marker(self, frame="velodyne", name = "capture vehicle", translation=True):
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.name = name
int_marker.description = name
int_marker.scale = 3
marker_control = InteractiveMarkerControl()
marker_control.always_visible = True
marker_control.markers.append(self.marker)
int_marker.controls.append(marker_control)
control = InteractiveMarkerControl()
control.name = "rotate_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "rotate_z"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "rotate_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
if not translation :
#int_marker.pose.position = Point(0,0,0)
return int_marker
control = InteractiveMarkerControl()
control.name = "move_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "move_z"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.name = "move_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
def reset(self):
self.last_cap_r = None
self.last_cap_f = None
self.last_cap_yaw = None
self.obs_centroid = None
def track_obstacle(self) :
obj_topics = {
'cap_r': '/objects/capture_vehicle/rear/gps/rtkfix',
'cap_f': '/objects/capture_vehicle/front/gps/rtkfix',
'obs_r': '/objects/obs1/rear/gps/rtkfix'
}
for obj in obj_topics:
rospy.Subscriber(obj_topics[obj],
Odometry,
self.handle_msg,
obj)
def handle_msg(self, msg, who):
assert isinstance(msg, Odometry)
now = rospy.get_rostime()
if now < self.current_time :
self.reset()
self.current_time = now
if who == 'cap_r':
self.last_cap_r = rtk_position_to_numpy(msg)
elif who == 'cap_f' and self.last_cap_r is not None:
cap_f = rtk_position_to_numpy(msg)
cap_r = self.last_cap_r
self.last_cap_f = cap_f
self.last_cap_yaw = get_yaw(cap_f, cap_r)
elif who == 'obs_r' and self.last_cap_f is not None and self.last_cap_yaw is not None:
md = self.metadata
# find obstacle rear RTK to centroid vector
lrg_to_gps = [md['rear_gps_l'], -md['rear_gps_w'], md['rear_gps_h']]
lrg_to_centroid = [md['l'] / 2., -md['w'] / 2., md['h'] / 2.]
obs_r_to_centroid = np.subtract(lrg_to_centroid, lrg_to_gps)
# in the fixed GPS frame
cap_f = self.last_cap_f
obs_r = rtk_position_to_numpy(msg)
# in the capture vehicle velodyne frame
cap_to_obs = np.dot(rotMatZ(-self.last_cap_yaw), obs_r - cap_f)
cap_to_obs_centroid = cap_to_obs + obs_r_to_centroid
velo_to_front = np.array([-1.0922, 0, -0.0508])
cap_to_obs_centroid += velo_to_front
self.obs_centroid = cap_to_obs_centroid + np.array(self.offset)
R = tf.transformations.quaternion_matrix(self.rotation_offset)
rotated_centroid = R.dot(list(self.obs_centroid)+[1])
self.obs_centroid = rotated_centroid[:3]
#br = tf.TransformBroadcaster()
now = rospy.get_rostime()
self.br.sendTransform(tuple(self.obs_centroid), (0,0,0,1), now,
'obs_centroid', 'velodyne')
self.obs_marker.header.frame_id = 'obs_centroid'
self.obs_marker.pose.position = Point(0,0,0)
self.obs_marker.pose.orientation = Quaternion(*self.orient)
self.add_bbox_lidar()
def add_bbox(self):
inputName = '/image_raw'
rospy.Subscriber(inputName, Image, self.handle_img_msg, queue_size=1)
def handle_img_msg(self, img_msg):
img = None
bridge = cv_bridge.CvBridge()
try:
img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr( 'image message to cv conversion failed :' )
rospy.logerr( e )
print( e )
return
#tx, ty, tz, yaw, pitch, roll = [0.00749025, -0.40459941, -0.51372948,
# -1.66780896, -1.59875352, -3.05415572]
#translation = [tx, ty, tz, 1]
#rotationMatrix = tf.transformations.euler_matrix(roll, pitch, yaw)
#rotationMatrix[:, 3] = translation
md = self.metadata
dims = np.array([md['l'], md['w'], md['h']])
outputName = '/image_bbox'
imgOutput = rospy.Publisher(outputName, Image, queue_size=1)
obs_centroid = self.obs_centroid
if self.obs_centroid is None:
rospy.loginfo("Couldn't find obstacle centroid")
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# print centroid info
rospy.loginfo(str(obs_centroid))
# case when obstacle is not in camera frame
if obs_centroid[0]<2.5 :
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# get bbox
R = tf.transformations.quaternion_matrix(self.orient)
corners = [0.5*np.array([i,j,k])*dims for i in [-1,1]
for j in [-1,1] for k in [-1,1]]
corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners]
projected_pts = []
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
#for pt in corners:
# rotated_pt = rotationMatrix.dot(list(pt)+[1])
# projected_pts.append(cameraModel.project3dToPixel(rotated_pt))
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(img, projected_pts)
imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
def processFeedback(self, feedback ):
p = feedback.pose.orientation
self.rotation_offset = [p.x, p.y, p.z, p.w]
p = feedback.pose.position
self.offset = [p.x, p.y, p.z]
self.server.applyChanges()
def obs_processFeedback(self, feedback ):
p = feedback.pose.orientation
self.orient = (p.x, p.y, p.z, p.w)
now = rospy.get_rostime()
self.br.sendTransform(tuple(self.obs_centroid), (0,0,0,1), now,
'obs_centroid', 'velodyne')
self.marker.pose.orientation = Quaternion(*self.orient)
self.server.applyChanges()
def add_bbox_lidar(self):
if self.obs_centroid is None :
return
now = rospy.get_rostime()
self.velodyne_marker.header.stamp = now #rospy.get_rostime()
self.obs_marker.header.stamp = now #rospy.get_rostime()
# tell the server to call processFeedback() when feedback arrives for it
self.server.insert(self.velodyne_marker, self.processFeedback)
self.server.applyChanges()
self.server.insert(self.obs_marker, self.obs_processFeedback)
#self.menu_handler.apply(self.server, self.int_marker.name)
# 'commit' changes and send to all clients
self.server.applyChanges()
class GripperMarkers:
_offset = 0.09
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 get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
if self._tf_listener.frameExists(from_frame) and self._tf_listener.frameExists(to_frame):
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
# t = rospy.Time.now()
(pos, rot) = self._tf_listener.lookupTransform(to_frame, from_frame, t) # Note argument order :(
else:
rospy.logerr('TF frames do not exist; could not get end effector pose.')
except Exception as err:
rospy.logerr('Could not get end effector pose through TF.')
rospy.logerr(err)
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, feedback):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
'''Moves the arm to the desired joints'''
print feedback
time_to_joint = 2.0
positions = self.ik.get_ik_for_ee(feedback.pose)
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities, time_from_start=rospy.Duration(time_to_joint)))
if (self.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)
pass
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_' + self.side_prefix
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]))
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
print self
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution is 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
class LookAtMarkerServer:
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
# self._head = robot_api.Head(tf_listener=self._tf_listener)
def makeBox(self, msg):
marker = Marker()
marker.type = Marker.CUBE
marker.scale.x = msg.scale * 0.45
marker.scale.y = msg.scale * 0.45
marker.scale.z = msg.scale * 0.45
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeBox(msg))
msg.controls.append(control)
return control
def makeQuadrocopterMarker(self, position):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "odom"
int_marker.pose.position = position
int_marker.scale = 1
int_marker.name = "look_at_point"
int_marker.description = "Point to look at"
self.makeBoxControl(int_marker)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self._server.insert(int_marker, self.handleInput)
self._server.applyChanges()
def makeStaticMarker(self, position):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "odom"
int_marker.pose.position = position
int_marker.scale = 1
int_marker.name = "movement_controller"
int_marker.description = "Click to make Kuri move"
box_marker = self.makeBox(int_marker)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.markers.append(box_marker)
int_marker.controls.append(copy.deepcopy(control))
self._server.insert(int_marker, self.handleInput)
self._server.applyChanges()
def handleInput(self, input):
if input.event_type == InteractiveMarkerFeedback.MOUSE_UP:
if input.marker_name == "look_at_point":
ps = PointStamped(header=input.header,
point=input.pose.position)
self._look_at_target = ps
self._head.look_at(self._look_at_target)
pass
elif input.marker_name == "movement_controller":
if self._look_at_target == None:
rospy.loginfo("position of target unknown")
else:
self._head.move_to(self._look_at_target)
pass
else:
pass
control_rotate_y = makeInteractiveMarkerControl(
interactive_marker, InteractiveMarkerControl.ROTATE_AXIS)
control_rotate_z = makeInteractiveMarkerControl(
interactive_marker, InteractiveMarkerControl.ROTATE_AXIS)
control_sphere = makeInteractiveMarkerControl(
interactive_marker, InteractiveMarkerControl.MOVE_ROTATE_3D)
marker = Marker()
marker.color.r = 0.2
marker.color.g = 0.3
marker.color.b = 0.7
marker.color.a = 0.5
marker.type = Marker.SPHERE
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
control_sphere.markers.append(marker)
setOrientation(1,1,0,0, control_slide_x)
setOrientation(1,0,1,0, control_slide_y)
setOrientation(1,0,0,1, control_slide_z)
setOrientation(1,1,0,0, control_rotate_x)
setOrientation(1,0,1,0, control_rotate_y)
setOrientation(1,0,0,1, control_rotate_z)
setOrientation(1,1,0,0, control_sphere)
server.insert(interactive_marker, feedback)
server.applyChanges()
rospy.spin()
class GripperMarkers:
_offset = -0.09
_tf_listener = None
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 get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
GripperMarkers._tf_listener.waitForTransform(from_frame, to_frame, rospy.Time(0), rospy.Duration(5))
t = GripperMarkers._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = GripperMarkers._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.marker_pose, GripperMarkers._offset)
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
rospy.logwarn('No IK solutions for this pose, cannot move.')
else:
self.ik.move_to_joints(ik_solution, 2.0)
pass
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.marker_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.marker_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)
text = self.side_prefix + '_arm'
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.05),
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
offset = -GripperMarkers._offset
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - 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 = -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 - 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, x_offset):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [x_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.marker_pose, GripperMarkers._offset)
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)
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 CircleMarker(object):
def __init__(self):
self._server = InteractiveMarkerServer("simple_marker")
def callback_marker(self, input):
pass
def align_marker(self, feedback):
pose = feedback.pose
self._server.setPose(feedback.marker_name, pose)
print("Arrow Marker's name is ", feedback.marker_name)
self._server.setPose("orientation_ring", pose)
self._server.applyChanges()
def orientation_ring_callback(self, feedback):
if (feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE):
print("Updating arrow marker's position ")
self._server.setPose("int_arrow_marker", feedback.pose)
self._server.applyChanges()
def create_draggable_marker(self, position):
int_arrow_marker = InteractiveMarker()
int_arrow_marker.header.frame_id = "map"
int_arrow_marker.name = "int_arrow_marker"
int_arrow_marker.description = "right"
int_arrow_marker.pose.position.y = position.y
int_arrow_marker.pose.position.x = position.x
int_arrow_marker.pose.position.z = position.z + 0.1
int_arrow_marker.scale = 1
arrow_marker = Marker()
arrow_marker.ns = "arrow_marker"
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 0
arrow_marker.scale.x = 0.5
arrow_marker.scale.y = 0.05
arrow_marker.scale.z = 0.05
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
button_control.always_visible = True
button_control.orientation.w = 1
button_control.orientation.x = 0
button_control.orientation.y = 1
button_control.orientation.z = 0
button_control.markers.append(arrow_marker)
int_arrow_marker.controls.append(button_control)
orientation_ring_marker = InteractiveMarker()
orientation_ring_marker.header.frame_id = "map"
orientation_ring_marker.scale = 1
orientation_ring_marker.pose.position.y = position.y
orientation_ring_marker.pose.position.z = position.z + 0.1
orientation_ring_marker.pose.position.x = position.x
orientation_ring_marker.name = "orientation_ring"
orientation_ring_marker.description = "Orientation Ring"
orientation_ring_marker_control = InteractiveMarkerControl()
orientation_ring_marker_control.always_visible = True
orientation_ring_marker_control.orientation.w = 1
orientation_ring_marker_control.orientation.x = 0
orientation_ring_marker_control.orientation.y = 1
orientation_ring_marker_control.orientation.z = 0
orientation_ring_marker_control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
orientation_ring_marker.controls.append(orientation_ring_marker_control)
self._server.insert(orientation_ring_marker, self.orientation_ring_callback)
self._server.insert(int_arrow_marker, self.callback_marker)
print"inserted"
self._server.setCallback("int_arrow_marker", self.align_marker, InteractiveMarkerFeedback.POSE_UPDATE)
self._server.applyChanges()
class ParkingSpotServer(Node):
def __init__(self):
super(ParkingSpotServer, self).__init__('parking_spot_server')
self.declare_parameter('map_yaml')
self.markers = {}
self.poses = {}
self.marker_server = InteractiveMarkerServer(self, 'markers')
map_param = self.get_parameter('map_yaml').value
self.map_path = Path(map_param)
self.load_map()
self.name_counter = 0
# Create a timeout to throttle saving data on feedback
# We reset it on marker feedback so that once the user stops editing, save is triggered
self.save_timer = self.create_timer(0.1, self.save_map)
self.save_timer.cancel()
self.add_srv = self.create_service(AddParkingSpot, 'add_parking_spot',
self.add_spot)
self.del_srv = self.create_service(DeleteParkingSpot,
'delete_parking_spot',
self.delete_spot)
self.get_srv = self.create_service(GetParkingSpot, 'get_parking_spot',
self.get_spot)
self.rename_srv = self.create_service(RenameParkingSpot,
'rename_parking_spot',
self.rename_spot)
def save_map(self):
self.save_timer.cancel()
dump = {}
for name, marker in self.markers.items():
pose = marker.pose
euler = euler_from_quaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
yaw = euler[2]
dump[name] = [
pose.position.x,
pose.position.y,
yaw,
]
self.map_data['parking'] = dump
with self.map_path.open('w') as map_yaml:
yaml.dump(self.map_data, map_yaml)
def load_map(self):
with self.map_path.open('r') as map_yaml:
self.map_data = yaml.safe_load(map_yaml)
self.poses = {
name: Pose2D(x=pose[0], y=pose[1], theta=pose[2])
for name, pose in self.map_data.get('parking', {}).items()
}
self.markers = {
name: self.add_marker(name,
Pose2D(x=pose[0], y=pose[1], theta=pose[2]))
for name, pose in self.map_data.get('parking', {}).items()
}
def marker_feedback(self, event: InteractiveMarkerFeedback) -> None:
self.save_timer.reset()
def add_marker(self, name: str, pose: Pose2D) -> InteractiveMarker:
marker = InteractiveMarker(name=name,
pose=Pose(
position=Point(x=pose.x, y=pose.y,
z=0.),
orientation=quaternion_from_euler(
pose.theta, 0., 0.),
))
marker.header.frame_id = 'map'
sc = 0.2
z = 0.1
name_marker = Marker(
type=Marker.TEXT_VIEW_FACING,
scale=Vector3(x=sc, y=sc, z=sc),
color=ColorRGBA(r=1., g=1., b=1., a=1.),
text=name,
)
name_marker.pose.position.x = sc * -0.1
name_marker.pose.position.z = z + sc * -0.1
marker.controls = [
InteractiveMarkerControl(
name='name',
orientation_mode=InteractiveMarkerControl.VIEW_FACING,
interaction_mode=InteractiveMarkerControl.NONE,
independent_marker_orientation=False,
always_visible=True,
markers=[name_marker],
),
InteractiveMarkerControl(
name='xaxis',
orientation_mode=InteractiveMarkerControl.FIXED,
orientation=Quaternion(x=0., y=0., z=0.7068252, w=0.7068252),
interaction_mode=InteractiveMarkerControl.MOVE_AXIS,
),
InteractiveMarkerControl(
name='yaxis',
orientation_mode=InteractiveMarkerControl.FIXED,
interaction_mode=InteractiveMarkerControl.MOVE_AXIS,
),
InteractiveMarkerControl(
name='turn',
interaction_mode=InteractiveMarkerControl.ROTATE_AXIS,
orientation=Quaternion(x=0., y=0.7068252, z=0., w=0.7068252),
)
]
self.marker_server.insert(marker,
feedback_callback=self.marker_feedback)
self.marker_server.applyChanges()
return marker
def add_spot(self, request, response):
print(request)
name = 'park{:02}'.format(self.name_counter)
while name in self.poses:
self.name_counter += 1
name = 'park{:02}'.format(self.name_counter)
marker = self.add_marker(name, request.pose)
self.poses[name] = request.pose
self.markers[name] = marker
response.success = True
self.save_map()
return response
def delete_spot(self, request, response):
try:
del self.poses[request.name]
self.marker_server.erase(request.name)
self.marker_server.applyChanges()
del self.markers[request.name]
response.success = True
self.save_map()
except KeyError:
response.success = False
return response
def get_spot(self, request, response):
try:
response.pose = self.poses[request.name]
response.success = True
except KeyError:
response.success = False
return response
def rename_spot(self, request, response):
if request.name not in self.poses:
response.success = False
response.msg = 'Spot does not exist'
elif request.new_name in self.poses:
response.success = False
response.msg = 'New name already taken'
else:
response.success = True
name = request.name
pose = self.poses[name]
del self.markers[name]
del self.poses[name]
self.poses[request.new_name] = pose
self.marker_server.erase(name)
marker = self.add_marker(request.new_name, pose)
self.markers[request.new_name] = marker
self.save_map()
return response
control_rotate_y = makeInteractiveMarkerControl(
interactive_marker, InteractiveMarkerControl.ROTATE_AXIS)
control_rotate_z = makeInteractiveMarkerControl(
interactive_marker, InteractiveMarkerControl.ROTATE_AXIS)
control_sphere = makeInteractiveMarkerControl(
interactive_marker, InteractiveMarkerControl.MOVE_ROTATE_3D)
marker = Marker()
marker.color.r = 0.2
marker.color.g = 0.3
marker.color.b = 0.7
marker.color.a = 0.5
marker.type = Marker.SPHERE
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
control_sphere.markers.append(marker)
setOrientation(1,1,0,0, control_slide_x)
setOrientation(1,0,1,0, control_slide_y)
setOrientation(1,0,0,1, control_slide_z)
setOrientation(1,1,0,0, control_rotate_x)
setOrientation(1,0,1,0, control_rotate_y)
setOrientation(1,0,0,1, control_rotate_z)
setOrientation(1,1,0,0, control_sphere)
server.insert(interactive_marker, feedback)
server.applyChanges()
rospy.spin()
class World:
"""Object recognition and localization related stuff"""
tf_listener = None
objects = []
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 _reset_objects(self):
"""Function that removes all objects"""
self._lock.acquire()
for i in range(len(World.objects)):
self._im_server.erase(World.objects[i].int_marker.name)
self._im_server.applyChanges()
if self.surface != None:
self._remove_surface()
self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def receieve_table_marker(self, marker):
"""Callback function for markers to determine table"""
if marker.type == Marker.LINE_STRIP:
if len(marker.points) == 6:
rospy.loginfo("Received a TABLE marker.")
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface, self.marker_feedback_cb)
self._im_server.applyChanges()
def receieve_object_info(self, object_list):
"""Callback function to receive object info"""
self._lock.acquire()
rospy.loginfo("Received recognized object list.")
if len(object_list.graspable_objects) > 0:
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if len(models) > 0:
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if best_confidence < models[j].confidence:
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if object_pose != None:
rospy.logwarn("Adding the recognized object " + "with most confident model.")
self._add_new_object(object_pose, Vector3(0.2, 0.2, 0.2), True, object_list.meshes[i])
else:
rospy.logwarn("... this is not a recognition result, " + "it is probably just segmentation.")
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if cluster_pose != None:
rospy.loginfo(
"Adding unrecognized object with pose:"
+ World.pose_to_string(cluster_pose)
+ "\n"
+ "In ref frame"
+ str(bbox.pose.header.frame_id)
)
self._add_new_object(cluster_pose, bbox.box_dims, False)
else:
rospy.logwarn("... but the list was empty.")
self._lock.release()
@staticmethod
def get_pose_from_transform(transform):
"""Returns pose for transformation matrix"""
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 get_matrix_from_pose(pose):
"""Returns the transformation matrix for given 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_absolute_pose(arm_state):
"""Returns absolute pose of an end effector state"""
if arm_state.refFrame == ArmState.OBJECT:
arm_state_copy = ArmState(
arm_state.refFrame,
Pose(arm_state.ee_pose.position, arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameObject,
)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def get_most_similar_obj(ref_object, ref_frame_list):
"""Finds the most similar object in the world"""
best_dist = 10000
chosen_obj_index = -1
for i in range(len(ref_frame_list)):
dist = World.object_dissimilarity(ref_frame_list[i], ref_object)
if dist < best_dist:
best_dist = dist
chosen_obj_index = i
if chosen_obj_index == -1:
rospy.logwarn("Did not find a similar object..")
return None
else:
print "Object dissimilarity is --- ", best_dist
if best_dist > 0.075:
rospy.logwarn("Found some objects, but not similar enough.")
return None
else:
rospy.loginfo("Most similar to new object " + str(chosen_obj_index))
return ref_frame_list[chosen_obj_index]
@staticmethod
def _get_mesh_marker(marker, mesh):
"""Function that generated a marker from a mesh"""
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while index + 2 < len(mesh.triangles):
if (
(mesh.triangles[index] < len(mesh.vertices))
and (mesh.triangles[index + 1] < len(mesh.vertices))
and (mesh.triangles[index + 2] < len(mesh.vertices))
):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr("Mesh contains invalid triangle!")
break
return marker
def _add_new_object(self, pose, dimensions, is_recognized, mesh=None):
"""Function to add new objects"""
dist_threshold = 0.02
to_remove = None
if is_recognized:
# Temporary HACK for testing.
# Will remove all recognition completely if this works.
return False
# Check if there is already an object
for i in range(len(World.objects)):
distance = World.pose_distance(World.objects[i].object.pose, pose)
if distance < dist_threshold:
if World.objects[i].is_recognized:
rospy.loginfo(
"Previously recognized object at " + "the same location, will not add this object."
)
return False
else:
rospy.loginfo(
"Previously unrecognized object at "
+ "the same location, will replace it with the "
+ "recognized object."
)
to_remove = i
break
if to_remove != None:
self._remove_object(to_remove)
n_objects = len(World.objects)
World.objects.append(WorldObject(pose, n_objects, dimensions, is_recognized))
int_marker = self._get_object_marker(len(World.objects) - 1, mesh)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server, int_marker.name)
self._im_server.applyChanges()
return True
else:
for i in range(len(World.objects)):
if World.pose_distance(World.objects[i].object.pose, pose) < dist_threshold:
rospy.loginfo("Previously detected object at the same" + "location, will not add this object.")
return False
n_objects = len(World.objects)
World.objects.append(WorldObject(pose, n_objects, dimensions, is_recognized))
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server, int_marker.name)
self._im_server.applyChanges()
return True
def _remove_object(self, to_remove):
"""Function to remove object by index"""
obj = World.objects.pop(to_remove)
rospy.loginfo("Removing object " + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
# if (obj.is_recognized):
# for i in range(len(World.objects)):
# if ((World.objects[i].is_recognized)
# and World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_recognized -= 1
# else:
# for i in range(len(World.objects)):
# if ((not World.objects[i].is_recognized) and
# World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_unrecognized -= 1
def _remove_surface(self):
"""Function to request removing surface"""
rospy.loginfo("Removing surface")
self._im_server.erase("surface")
self._im_server.applyChanges()
self.surface = None
def _get_object_marker(self, index, mesh=None):
"""Generate a marker for world objects"""
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = "base_link"
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id="base_link"),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose,
)
if mesh != None:
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06
button_control.markers.append(
Marker(
type=Marker.TEXT_VIEW_FACING,
id=index,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id="base_link"),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)),
)
)
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def _get_surface_marker(pose, dimensions):
""" Function that generates a surface marker"""
int_marker = InteractiveMarker()
int_marker.name = "surface"
int_marker.header.frame_id = "base_link"
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id="base_link"),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose,
)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(
type=Marker.TEXT_VIEW_FACING,
id=2001,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id="base_link"),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)),
)
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def get_frame_list():
"""Function that returns the list of ref. frames"""
objects = []
for i in range(len(World.objects)):
objects.append(World.objects[i].object)
return objects
@staticmethod
def has_objects():
"""Function that checks if there are any objects"""
return len(World.objects) > 0
@staticmethod
def object_dissimilarity(obj1, obj2):
"""Distance between two objects"""
dims1 = obj1.dimensions
dims2 = obj2.dimensions
return norm(array([dims1.x, dims1.y, dims1.z]) - array([dims2.x, dims2.y, dims2.z]))
@staticmethod
def get_ref_from_name(ref_name):
"""Ref. frame type from ref. frame name"""
if ref_name == "base_link":
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
"""Transforms an arm frame to a new ref. frame"""
if ref_frame == ArmState.ROBOT_BASE:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
pass
# rospy.logwarn('No reference frame transformations ' +
#'needed (both absolute).')
elif arm_frame.refFrame == ArmState.OBJECT:
abs_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, "base_link")
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr(
"Unhandled reference frame conversion:" + str(arm_frame.refFrame) + " to " + str(ref_frame)
)
elif ref_frame == ArmState.OBJECT:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
rel_ee_pose = World.transform(arm_frame.ee_pose, "base_link", ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif arm_frame.refFrame == ArmState.OBJECT:
if arm_frame.refFrameObject.name == ref_frame_obj.name:
pass
# rospy.logwarn('No reference frame transformations ' +
#'needed (same object).')
else:
rel_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr(
"Unhandled reference frame conversion:" + str(arm_frame.refFrame) + " to " + str(ref_frame)
)
return arm_frame
@staticmethod
def has_object(object_name):
"""Checks if the world contains the object"""
for obj in World.objects:
if obj.object.name == object_name:
return True
return False
@staticmethod
def is_frame_valid(object_name):
"""Checks if the frame is valid for transforms"""
return (object_name == "base_link") or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
""" Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms"""
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(from_frame, to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(to_frame, pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr("TF exception during transform.")
return pose
except rospy.ServiceException:
rospy.logerr("Exception during transform.")
return pose
else:
rospy.logwarn("One of the frame objects might not exist: " + from_frame + " or " + to_frame)
return pose
@staticmethod
def pose_to_string(pose):
"""For printing a pose to stdout"""
return (
"Position: "
+ str(pose.position.x)
+ ", "
+ str(pose.position.y)
+ ", "
+ str(pose.position.z)
+ "\n"
+ "Orientation: "
+ str(pose.orientation.x)
+ ", "
+ str(pose.orientation.y)
+ ", "
+ str(pose.orientation.z)
+ ", "
+ str(pose.orientation.w)
+ "\n"
)
def _publish_tf_pose(self, pose, name, parent):
""" Publishes a TF for object pose"""
if pose != None:
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(), name, parent)
def update_object_pose(self):
""" Function to externally update an object pose"""
Response.perform_gaze_action(GazeGoal.LOOK_DOWN)
while (
Response.gaze_client.get_state() == GoalStatus.PENDING
or Response.gaze_client.get_state() == GoalStatus.ACTIVE
):
time.sleep(0.1)
if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("Could not look down to take table snapshot")
return False
rospy.loginfo("Looking at table now.")
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Object recognition has been reset.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
self._reset_objects()
if self._object_action_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("Could not reset recognition.")
return False
# Do segmentation
goal = UserCommandGoal(UserCommandGoal.SEGMENT, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Table segmentation is complete.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
if self._object_action_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("Could not segment.")
return False
# Do recognition
goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Objects on the table have been recognized.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
# Record the result
if self._object_action_client.get_state() == GoalStatus.SUCCEEDED:
wait_time = 0
total_wait_time = 5
while not World.has_objects() and wait_time < total_wait_time:
time.sleep(0.1)
wait_time += 0.1
if not World.has_objects():
rospy.logerr("Timeout waiting for a recognition result.")
return False
else:
rospy.loginfo("Got the object list.")
return True
else:
rospy.logerr("Could not recognize.")
return False
def clear_all_objects(self):
"""Removes all objects from the world"""
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Object recognition has been reset.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
if self._object_action_client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("Successfully reset object localization pipeline.")
self._reset_objects()
self._remove_surface()
def get_nearest_object(self, arm_pose):
"""Gives a pointed to the nearest object"""
dist_threshold = 0.4
def chObj(cur, obj):
dist = World.pose_distance(obj.object.pose, arm_pose)
return (dist, obj.object) if (dist < cur[0]) else cur
return reduce(chObj, World.objects, (dist_threshold, None))[1]
@staticmethod
def pose_distance(pose1, pose2, is_on_table=True):
"""Distance between two world poses"""
if pose1 == [] or pose2 == []:
return 0.0
else:
if is_on_table:
arr1 = array([pose1.position.x, pose1.position.y])
arr2 = array([pose2.position.x, pose2.position.y])
else:
arr1 = array([pose1.position.x, pose1.position.y, pose1.position.z])
arr2 = array([pose2.position.x, pose2.position.y, pose2.position.z])
dist = norm(arr1 - arr2)
if dist < 0.0001:
dist = 0
return dist
def marker_feedback_cb(self, feedback):
"""Callback for when feedback from a marker is received"""
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo("Clicked on object " + str(feedback.marker_name))
rospy.loginfo("Number of objects " + str(len(World.objects)))
else:
rospy.loginfo("Unknown event" + str(feedback.event_type))
def update(self):
"""Update function called in a loop"""
# Visualize the detected object
is_world_changed = False
self._lock.acquire()
if World.has_objects():
to_remove = None
for i in range(len(World.objects)):
self._publish_tf_pose(World.objects[i].object.pose, World.objects[i].get_name(), "base_link")
if World.objects[i].is_removed:
to_remove = i
if to_remove != None:
self._remove_object(to_remove)
is_world_changed = True
self._lock.release()
return is_world_changed
class ActuatorServer(object):
def __init__(self):
self.goto_pub = rospy.Publisher('/move_base_simple/goal',
geometry_msgs.msg.PoseStamped)
self.pose_pub = rospy.Publisher('/pose_names',
order_system.msg.PoseNames,
latch=True)
rospy.Subscriber('/amcl_pose',
geometry_msgs.msg.PoseWithCovarianceStamped,
self._handle_pose_callback)
self.names = {}
try:
with open(
"/home/team3/catkin_ws/src/cse481c/order_service/LOL.pickle",
"r") as f:
self.names = pickle.load(f)
except EOFError:
self.names = {}
self.pose = None
self.server = InteractiveMarkerServer('simple_marker')
self.name_markers = {}
self.name_controls = {}
def _create_marker(self, name):
self.name_markers[name] = InteractiveMarker()
self.name_markers[name].header.frame_id = "map"
self.name_markers[name].name = name
self.name_markers[name].description = name
self.name_markers[
name].pose.position.x = self.pose.pose.pose.position.x
self.name_markers[
name].pose.position.y = self.pose.pose.pose.position.y
# TODO(emersonn): REMEMBER THAT THIS IS 1 IF IT IS CRAZY LOL
self.name_markers[name].pose.position.z = 1
box_marker = create_box_marker()
self.name_controls[name] = InteractiveMarkerControl()
self.name_controls[
name].interaction_mode = InteractiveMarkerControl.MOVE_PLANE
self.name_controls[name].always_visible = True
self.name_controls[name].orientation.w = 1
self.name_controls[name].orientation.x = 0
self.name_controls[name].orientation.y = 1
self.name_controls[name].orientation.z = 0
new_controller_lol = InteractiveMarkerControl()
new_controller_lol.orientation.w = 1
new_controller_lol.orientation.x = 0
new_controller_lol.orientation.y = 1
new_controller_lol.orientation.z = 0
new_controller_lol.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
new_controller_lol.orientation_mode = InteractiveMarkerControl.FIXED
self.name_markers[name].controls.append(new_controller_lol)
# self.name_controls[name].orientation.w = self.pose.pose.pose.orientation.w
# self.name_controls[name].orientation.x = self.pose.pose.pose.orientation.x
# self.name_controls[name].orientation.y = self.pose.pose.pose.orientation.y
# self.name_controls[name].orientation.z = self.pose.pose.pose.orientation.z
self.name_controls[name].markers.append(box_marker)
self.name_markers[name].controls.append(self.name_controls[name])
self.server.insert(self.name_markers[name], self._callback)
self.server.applyChanges()
def _callback(self, dog):
name = dog.marker_name
pose = dog.pose
rospy.logerr(str(self.names[name]))
self.names[name].pose.pose.position.x = pose.position.x
self.names[name].pose.pose.position.y = pose.position.y
self.names[name].pose.pose.orientation = pose.orientation
rospy.logerr(str(self.names[name]))
def _delete_marker(self, name):
self.server.erase(name)
self.server.applyChanges()
def handle_send_fetch(self, request):
rospy.logerr("yo we got fam" + str(request))
if request.command == request.CREATE:
self._handle_create(request.name)
self._create_marker(request.name)
elif request.command == request.DELETE:
self._handle_delete(request.name)
self._delete_marker(request.name)
elif request.command == request.GOTO:
self._handle_goto(request.name)
elif request.command == request.LIST:
rospy.loginfo('hey Karen')
else:
rospy.logerr('none of these work')
pose_message = order_system.msg.PoseNames()
pose_message.poses = list(self.names.keys())
self.pose_pub.publish(pose_message)
fetch_response = SendFetchResponse()
fetch_response.names = list(self.names.keys())
return fetch_response
def _handle_create(self, name):
rospy.loginfo(str(self.pose))
self.names[name] = self.pose
def _handle_delete(self, name):
if name not in self.names:
rospy.loginfo('wtf bro')
else:
del self.names[name]
def _handle_goto(self, name):
new_goto = geometry_msgs.msg.PoseStamped()
rospy.loginfo(str(new_goto))
new_goto.header = self.names[name].header
new_goto.pose = self.names[name].pose.pose
rospy.logerr("wtf" + str(self.names[name].pose.pose))
rospy.logerr("bro" + str(new_goto))
self.goto_pub.publish(new_goto)
def _handle_pose_callback(self, data):
self.pose = copy.deepcopy(data)
def pickle_it_up_bro(self):
with open("/home/team3/catkin_ws/src/cse481c/map_annotator/LOL.pickle",
"w") as f:
pickle.dump(self.names, f)
class World:
'''Object recognition and localization related stuff'''
tf_listener = None
objects = []
world = None
segmentation_service = rospy.get_param("/pr2_pbd_interaction/tabletop_segmentation_service")
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
@staticmethod
def get_world():
if World.world is None:
World.world = World()
return World.world
def _reset_objects(self):
'''Function that removes all objects'''
self._lock.acquire()
for i in range(len(World.objects)):
self._im_server.erase(World.objects[i].int_marker.name)
self._im_server.applyChanges()
if self.surface != None:
self._remove_surface()
#self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def receieve_table_marker(self, marker):
'''Callback function for markers to determine table'''
if (marker.type == Marker.LINE_STRIP):
if (len(marker.points) == 6):
rospy.loginfo('Received a TABLE marker.')
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface,
self.marker_feedback_cb)
self._im_server.applyChanges()
def receive_ar_markers(self, data):
'''Callback function to receive marker info'''
self._lock.acquire()
if self.is_looking_for_markers:
if len(data.markers) > 0:
rospy.loginfo('Received non-empty marker list.')
for i in range(len(data.markers)):
marker = data.markers[i]
self._add_new_object(marker.pose.pose, self.marker_dims, False, id=marker.id)
self._lock.release()
def receieve_object_info(self, object_list):
'''Callback function to receive object info'''
self._lock.acquire()
rospy.loginfo('Received recognized object list.')
if (len(object_list.graspable_objects) > 0):
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if (len(models) > 0):
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if (best_confidence < models[j].confidence):
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if (object_pose != None):
rospy.logwarn('Adding the recognized object ' +
'with most confident model.')
self._add_new_object(object_pose,
Vector3(0.2, 0.2, 0.2), True,
object_list.meshes[i])
else:
rospy.logwarn('... this is not a recognition result, ' +
'it is probably just segmentation.')
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if (cluster_pose != None):
rospy.loginfo('Adding unrecognized object with pose:' +
World.pose_to_string(cluster_pose) + '\n' +
'In ref frame' + str(bbox.pose.header.frame_id))
self._add_new_object(cluster_pose, bbox.box_dims,
False)
else:
rospy.logwarn('... but the list was empty.')
self._lock.release()
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix'''
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 get_matrix_from_pose(pose):
'''Returns the transformation matrix for given 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_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state'''
if (arm_state.refFrame == ArmState.OBJECT):
arm_state_copy = ArmState(arm_state.refFrame,
Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameObject)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def get_most_similar_obj(ref_object, ref_frame_list, threshold=0.075):
'''Finds the most similar object in the world'''
best_dist = 10000
chosen_obj_index = -1
for i in range(len(ref_frame_list)):
dist = World.object_dissimilarity(ref_frame_list[i], ref_object)
if (dist < best_dist):
best_dist = dist
chosen_obj_index = i
if chosen_obj_index == -1:
rospy.logwarn('Did not find a similar object..')
return None
else:
print 'Object dissimilarity is --- ', best_dist
if best_dist > threshold:
rospy.logwarn('Found some objects, but not similar enough.')
return None
else:
rospy.loginfo('Most similar to object '
+ ref_frame_list[chosen_obj_index].name)
return ref_frame_list[chosen_obj_index]
@staticmethod
def get_map_of_most_similar_obj(object_list, ref_frame_list, threshold=0.075):
if None in object_list:
object_list.remove(None)
if len(object_list) == 0 or len(ref_frame_list) == 0:
return None
markers_dict = {}
marker_names = []
for obj in object_list:
object_name = obj.name
if object_name.startswith("marker"):
marker_names.append(object_name)
found_match = False
for ref_frame in ref_frame_list:
if ref_frame.name == object_name:
markers_dict[object_name] = ref_frame
found_match = True
break
if not found_match:
return None
ref_frame_list = [x for x in ref_frame_list if x.name not in marker_names]
object_list = [x for x in object_list if x.name not in marker_names]
if len(object_list) == 0 or len(ref_frame_list) == 0:
return markers_dict if len(markers_dict) > 0 else None
orderings = []
World.permute(object_list, orderings)
costs = []
assignments = []
for ordering in orderings:
cur_cost = 0
cur_ref_frame_list = ref_frame_list[:]
cur_assignment = []
for object in ordering:
most_similar_object = World.get_most_similar_obj(object, cur_ref_frame_list, threshold)
if most_similar_object is None:
return None
cur_ref_frame_list.remove(most_similar_object)
cur_assignment.append(most_similar_object)
cur_cost += World.object_dissimilarity(object, most_similar_object)
costs.append(cur_cost)
assignments.append(cur_assignment)
min_cost, min_idx = min((val, idx) for (idx, val) in enumerate(costs))
names = [x.name for x in orderings[min_idx]]
object_dict = dict(zip(names, assignments[min_idx]))
object_dict.update(markers_dict)
return object_dict
@staticmethod
def permute(a, results):
if len(a) == 1:
results.insert(len(results), a)
else:
for i in range(0, len(a)):
element = a[i]
a_copy = [a[j] for j in range(0, len(a)) if j != i]
subresults = []
World.permute(a_copy, subresults)
for subresult in subresults:
result = [element] + subresult
results.insert(len(results), result)
@staticmethod
def _get_mesh_marker(marker, mesh):
'''Function that generated a marker from a mesh'''
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while (index + 2 < len(mesh.triangles)):
if ((mesh.triangles[index] < len(mesh.vertices))
and (mesh.triangles[index + 1] < len(mesh.vertices))
and (mesh.triangles[index + 2] < len(mesh.vertices))):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr('Mesh contains invalid triangle!')
break
return marker
def _add_new_object(self, pose, dimensions, is_recognized, mesh=None, id=None):
'''Function to add new objects'''
dist_threshold = 0.02
to_remove = None
if (is_recognized):
# Temporary HACK for testing.
# Will remove all recognition completely if this works.
return False
# Check if there is already an object
for i in range(len(World.objects)):
distance = World.pose_distance(World.objects[i].object.pose,
pose)
if (distance < dist_threshold):
if (World.objects[i].is_recognized):
rospy.loginfo('Previously recognized object at ' +
'the same location, will not add this object.')
return False
else:
rospy.loginfo('Previously unrecognized object at ' +
'the same location, will replace it with the ' +
'recognized object.')
to_remove = i
break
if (to_remove != None):
self._remove_object(to_remove)
n_objects = len(World.objects)
new_object = WorldObject(pose, n_objects,
dimensions, is_recognized)
if id is not None:
new_object.assign_name("marker" + str(id))
new_object.is_marker = True
World.objects.append(new_object)
int_marker = self._get_object_marker(len(World.objects) - 1, mesh)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
else:
for i in range(len(World.objects)):
if (World.pose_distance(World.objects[i].object.pose, pose)
< dist_threshold):
rospy.loginfo('Previously detected object at the same' +
'location, will not add this object.')
return False
if id is not None and World.objects[i].get_name() == "marker" + str(id):
rospy.loginfo('Previously added marker with the same id, will not add this object.')
return False
n_objects = len(World.objects)
new_object = WorldObject(pose, n_objects,
dimensions, is_recognized)
if id is not None:
new_object.assign_name("marker" + str(id))
new_object.is_marker = True
World.objects.append(new_object)
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
def _add_new_marker(self, id, pose):
'''Function to add new markers'''
#dist_threshold = 0.01
#to_remove = None
#for i in range(len(World.markers)):
# if (World.pose_distance(World.markers[i].pose, pose)
# < dist_threshold):
# rospy.loginfo('Previously detected marker at the same' +
# 'location, will not add this marker.')
# return False
#n_markers = len(World.markers)
#World.markers.append(WorldMarker(id, pose))
#int_marker = self._get_object_marker(len(World.objects) - 1)
#World.markers[-1].int_marker = int_marker
#self._im_server.insert(int_marker, self.marker_feedback_cb)
#self._im_server.applyChanges()
#World.markers[-1].menu_handler.apply(self._im_server,
# int_marker.name)
#self._im_server.applyChanges()
return True
def _remove_object(self, to_remove):
'''Function to remove object by index'''
obj = World.objects.pop(to_remove)
rospy.loginfo('Removing object ' + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
# if (obj.is_recognized):
# for i in range(len(World.objects)):
# if ((World.objects[i].is_recognized)
# and World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_recognized -= 1
# else:
# for i in range(len(World.objects)):
# if ((not World.objects[i].is_recognized) and
# World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_unrecognized -= 1
def _remove_surface(self):
'''Function to request removing surface'''
rospy.loginfo('Removing surface')
self._im_server.erase('surface')
self._im_server.applyChanges()
self.surface = None
def _get_object_reachability_marker(self, world_object):
radius = self.relative_frame_threshold
pointsList = []
nx = 8
ny = 8
pointsList.append(Point(0, 0, radius))
pointsList.append(Point(0, 0, -radius))
for x in range(nx):
theta = 2 * pi * (x * 1.0 / nx)
for y in range(1, ny):
phi = pi * (y * 1.0 / ny)
destx = radius * cos(theta) * sin(phi)
desty = radius * sin(theta) * sin(phi)
destz = radius * cos(phi)
pointsList.append(Point(destx, desty, destz))
id = abs(hash(world_object.get_name() + "_reachability")) % (10 ** 8)
marker = Marker(type=Marker.SPHERE_LIST, id=id,
lifetime=rospy.Duration(nsecs=10 ** 8),
scale=Vector3(0.01, 0.01, 0.01),
points=set(pointsList),
header=Header(frame_id='base_link'),
color=ColorRGBA(1, 1, 1, 0.5),
pose=world_object.object.pose)
return marker
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
color = WorldObject.default_color if not World.objects[index].is_fake else WorldObject.fake_color
object_marker = Marker(type=Marker.CUBE, id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=color,
pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)))
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = 0
text_pos.y = 0
text_pos.z = World.objects[index].object.dimensions.z / 2 + 0.06
button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=index, scale=Vector3(0.05, 0.05, 0.05),
text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)))
button_control.markers.append(object_marker)
text_pos = Point()
dimensions = dimensions
text_pos.x = dimensions.x / 2 - 0.06
text_pos.y = - dimensions.y / 2 + 0.06
text_pos.z = dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
scale=Vector3(0.05, 0.05, 0.05), text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
def get_frame_list(self):
'''Function that returns the list of ref. frames'''
self._lock.acquire()
objects = []
for i in range(len(World.objects)):
objects.append(World.objects[i].object)
self._lock.release()
return objects
@staticmethod
def has_objects():
'''Function that checks if there are any objects'''
return len(World.objects) > 0
@staticmethod
def has_marker_objects():
'''Function that checks if there are any markers'''
for o in World.objects:
if o.is_marker:
return True
return False
@staticmethod
def has_non_marker_objects():
'''Function that checks if there are any objects'''
for o in World.objects:
if not o.is_marker:
return True
return False
@staticmethod
def object_dissimilarity(obj1, obj2):
'''Distance between two objects'''
dims1 = obj1.dimensions
dims2 = obj2.dimensions
return norm(array([dims1.x, dims1.y, dims1.z]) -
array([dims2.x, dims2.y, dims2.z]))
@staticmethod
def get_ref_from_name(ref_name):
'''Ref. frame type from ref. frame name'''
if ref_name == 'base_link':
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
'''Transforms an arm frame to a new ref. frame'''
if ref_frame == ArmState.ROBOT_BASE:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rospy.logwarn('No reference frame transformations ' +
'needed (both absolute).')
elif (arm_frame.refFrame == ArmState.OBJECT):
abs_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, 'base_link')
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
elif ref_frame == ArmState.OBJECT:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rel_ee_pose = World.transform(arm_frame.ee_pose,
'base_link', ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif (arm_frame.refFrame == ArmState.OBJECT):
if (arm_frame.refFrameObject.name == ref_frame_obj.name):
rospy.logwarn('No reference frame transformations ' +
'needed (same object).')
else:
rel_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
@staticmethod
def has_object(object_name):
'''Checks if the world contains the object'''
for obj in World.objects:
if obj.object.name == object_name:
return True
return False
@staticmethod
def is_frame_valid(object_name):
'''Checks if the frame is valid for transforms'''
return (object_name == 'base_link') or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
''' Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms'''
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(from_frame,
to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(to_frame,
pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr('TF exception during transform.')
return pose
except rospy.ServiceException:
rospy.logerr('Exception during transform.')
return pose
else:
# rospy.logwarn('One of the frame objects might not exist: ' +
# from_frame + ' or ' + to_frame)
return pose
@staticmethod
def pose_to_string(pose):
'''For printing a pose to stdout'''
return ('Position: ' + str(pose.position.x) + ", " +
str(pose.position.y) + ', ' + str(pose.position.z) + '\n' +
'Orientation: ' + str(pose.orientation.x) + ", " +
str(pose.orientation.y) + ', ' + str(pose.orientation.z) +
', ' + str(pose.orientation.w) + '\n')
def _publish_tf_pose(self, pose, name, parent):
''' Publishes a TF for object pose'''
if (pose != None):
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
self._tf_broadcaster.sendTransform(pos, rot,
rospy.Time.now(), name, parent)
def update_object_pose(self):
''' Function to externally update an object pose'''
rospy.loginfo("waiting for segmentation service")
rospy.wait_for_service(self.segmentation_service)
try:
get_segmentation = rospy.ServiceProxy(self.segmentation_service, TabletopSegmentation)
resp = get_segmentation()
rospy.loginfo("Adding landmarks")
self._reset_objects()
# add the table
xmin = resp.table.x_min
ymin = resp.table.y_min
xmax = resp.table.x_max
ymax = resp.table.y_max
depth = xmax - xmin
width = ymax - ymin
pose = resp.table.pose.pose
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface,
self.marker_feedback_cb)
self._im_server.applyChanges()
for cluster in resp.clusters:
points = cluster.points
if (len(points) == 0):
return Point(0, 0, 0)
[minX, maxX, minY, maxY, minZ, maxZ] = [
points[0].x, points[0].x, points[0].y, points[0].y,
points[0].z, points[0].z]
for pt in points:
minX = min(minX, pt.x)
minY = min(minY, pt.y)
minZ = min(minZ, pt.z)
maxX = max(maxX, pt.x)
maxY = max(maxY, pt.y)
maxZ = max(maxZ, pt.z)
self._add_new_object(Pose(Point((minX + maxX) / 2, (minY + maxY) / 2,
(minZ + maxZ) / 2), Quaternion(0, 0, 0, 1)),
Point(maxX - minX, maxY - minY, maxZ - minZ), False)
return True
except rospy.ServiceException, e:
print "Call to segmentation service failed: %s" % e
return False
class ToolCalib(object):
def __init__(self):
rospy.loginfo("Constructor got called")
self.frame_id = rospy.get_param("~frame_id")
self.mesh_resource = rospy.get_param("~mesh_resource")
self.scale = rospy.get_param("~marker_scale")
self.server = InteractiveMarkerServer("tool_calib")
self.server.insert(self.create_6dof_marker(), self.marker_callback)
self.server.applyChanges()
self.tool_marker = Marker()
self.tool_marker.header.frame_id = self.frame_id
self.tool_marker.header.stamp = rospy.get_rostime()
self.tool_marker.pose.orientation.w = 1
self.tool_marker.ns = "tool_marker"
self.tool_marker.id = 1
self.tool_marker.action = Marker.ADD
self.tool_marker.type = Marker.MESH_RESOURCE
self.tool_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.tool_marker.scale = Vector3(self.scale, self.scale, self.scale)
self.tool_marker.frame_locked = True
self.tool_marker.mesh_resource = self.mesh_resource
self.tool_marker.mesh_use_embedded_materials = True
self.marker_pub = rospy.Publisher("/visualization_marker",
Marker,
queue_size=1)
def create_6dof_marker(self):
imarker = InteractiveMarker()
imarker.header.frame_id = self.frame_id
imarker.pose.orientation.w = 1
imarker.name = "tool_calib"
imarker.name = "Tool Calibration"
imarker.scale = 0.2
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
imarker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
imarker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
imarker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
imarker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
imarker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
imarker.controls.append(control)
return imarker
def marker_callback(self, data):
if data.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.loginfo("Current Marker Pose for %s:", self.mesh_resource)
self.tool_marker.header = data.header
self.tool_marker.pose = data.pose
self.marker_pub.publish(self.tool_marker)
rospy.loginfo("\n%s", PoseStamped(data.header, data.pose))
self.server.applyChanges()
vehicleMarker.name = 'quadcopter'
q = quaternion_from_euler(0, 0, 0)
vehicleMarker.pose.orientation.x = q[0]
vehicleMarker.pose.orientation.y = q[1]
vehicleMarker.pose.orientation.z = q[2]
vehicleMarker.pose.orientation.w = q[3]
normalizeQuaternion(vehicleMarker.pose.orientation)
vehicleMesh = Marker()
vehicleMesh.type = Marker.MESH_RESOURCE
vehicleMesh.mesh_resource = MARKER_RESOURCE
vehicleMesh.scale.x, vehicleMesh.scale.y, vehicleMesh.scale.z = (tuple([vehicleMarker.scale*MARKER_SCALE]))*3
vehicleMesh.color.r, vehicleMesh.color.g, vehicleMesh.color.b = MARKER_COLOR
vehicleMesh.color.a = 1.0
vehicleControl = InteractiveMarkerControl()
vehicleControl.always_visible = True
vehicleControl.markers.append(vehicleMesh)
vehicleMarker.controls.append(vehicleControl)
server.insert(vehicleMarker, processFeedback)
server.applyChanges()
#rospy.spin()
while not rospy.core.is_shutdown():
print('*********************')
rospy.rostime.wallsleep(0.5)
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 MeasurementVisServer(object):
def __init__(self):
self.marker_pub = rospy.Publisher('measurement_markers', MarkerArray, queue_size=50)
self.positions_pub = rospy.Publisher('object_positions', ObjectMeasurement, queue_size=50)
self.init_positions_pub = rospy.Publisher('object_initialization_positions', ObjectMeasurement, queue_size=50)
self.target_pub = rospy.Publisher('get_target_poses', PoseArray, queue_size=50)
#self.object_pub = rospy.Publisher('measurement_markers', MarkerArray, queue_size=10)'
self.nbr_targets = rospy.get_param('~number_targets', 2)
self.nbr_noise = rospy.get_param('~number_noise', 4)
self.markers = MarkerArray()
self.object_counters = np.zeros((self.nbr_targets,), dtype=int)
self.initialized = np.zeros((self.nbr_targets,), dtype=bool)
self.marker_server = InteractiveMarkerServer("object_interactive_markers")
#self.room_server = InteractiveMarkerServer("room_interactive_markers")
self.marker_poses = [Pose() for j in range(0, self.nbr_targets)]
self.previous_poses = [Pose() for j in range(0, self.nbr_targets)]
self.did_move = np.zeros((self.nbr_targets,), dtype=bool)
self.marker_times = np.zeros((self.nbr_targets,), dtype=np.int64)
self.marker_clicked_times = np.zeros((self.nbr_targets,), dtype=np.int64)
self.regions, self.centers = get_regions()
self.room_time = 0
self.room_id = 0
for i, region in enumerate(self.regions):
print self.centers[i]
#self.clear_markers()
self.timestep = 0
self.measurement_counter = 0
self.clear_markers()
#rospy.Timer(rospy.Duration(0.1), callback=self.maybe_publish_poses)
rospy.Timer(rospy.Duration(0.1), callback=self.maybe_publish_rooms)
rospy.Subscriber("filter_measurements", ObjectMeasurement, self.callback)
rospy.Subscriber("sim_filter_measurements", ObjectMeasurement, self.callback)
rospy.Subscriber("set_target_poses", PoseArray, self.set_target_poses)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.clicked_callback)
self.initialize_room_markers()
def set_target_poses(self, poses):
for j, p in enumerate(poses.poses):
if p.position.x == 0 and p.position.x == 0:
continue
if not self.initialized[j]:
self.initialized[j] = True
self.initialize_object_marker(j, p)
else:
name = "object_marker_" + str(j)
p.position.z = 0.15
self.marker_poses[j] = p
self.marker_server.setPose(name, p)
self.marker_server.applyChanges()
def publish_target_poses(self):
poses = PoseArray()
for j in range(0, self.nbr_targets):
poses.poses.append(self.marker_poses[j])
self.target_pub.publish(poses)
def maybe_publish_rooms(self, event):
self.publish_target_poses()
seconds = rospy.Time.now().to_sec()
for j in range(0, self.nbr_targets):
mtime = self.marker_clicked_times[j]
if mtime != 0 and seconds - mtime > 1:
print "Publishing initial position for object ", j
self.marker_clicked_times[j] = 0
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose = self.marker_poses[j]
object_pos = ObjectMeasurement()
object_pos.pose = pose
object_pos.initialization_id = j
object_pos.timestep = 0
object_pos.observation_id = 0
object_pos.negative_observation = False
self.init_positions_pub.publish(object_pos)
seconds = rospy.Time.now().to_sec()
if self.room_time == 0 or seconds - self.room_time < 1:
return
room = self.regions[self.room_id]
# ok, time to see if we have any objects within this region:
vertices = np.zeros((len(room.posearray.poses), 2), dtype=float)
for i, pose in enumerate(room.posearray.poses):
vertices[i] = np.array([pose.position.x, pose.position.y])
#hull = ConvexHull(vertices)
#print vertices
#if not isinstance(hull, Delaunay):
# hull = Delaunay(hull)
hull = Delaunay(vertices)
#hull = Delaunay(ConvexHull(vertices))
published = False
shuffled = np.arange(self.nbr_targets)
np.random.shuffle(shuffled)
for j in shuffled:#range(0, self.nbr_targets):
if not self.initialized[j]:
continue
pose = [self.marker_poses[j].position.x, self.marker_poses[j].position.y]
if hull.find_simplex(pose) >= 0:
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose = self.marker_poses[j]
object_pos = ObjectMeasurement()
object_pos.pose = pose
object_pos.initialization_id = j
object_pos.timestep = self.timestep
object_pos.observation_id = self.measurement_counter
object_pos.location_id = self.room_id
object_pos.negative_observation = False
self.measurement_counter += 1
self.positions_pub.publish(object_pos)
self.room_time = 0
published = True
# if self.did_move[j]:
# pose = [self.previous_poses[j].position.x, self.previous_poses[j].position.y]
# if hull.find_simplex(pose) >= 0:
# print "Previous pose was inside, PUBLISHING!"
# #self.did_move[j] = False # need to fix a history?
# #self.previous_poses[j] = self.marker_poses[j]
# pose = PoseStamped()
# pose.header.frame_id = "map"
# pose.header.stamp = rospy.Time.now()
# pose.pose = self.previous_poses[j]
# object_pos = ObjectMeasurement()
# object_pos.pose = pose
# object_pos.initialization_id = j
# object_pos.timestep = self.timestep
# object_pos.observation_id = self.measurement_counter
# object_pos.location_id = self.room_id
# object_pos.negative_observation = True
# self.positions_pub.publish(object_pos)
# published = True
# compute min an max vertices in x and y to be able to sample uniform
# then use rejection sampling to get points within the polygon
maxs = np.amax(vertices, axis=0)
mins = np.amin(vertices, axis=0)
for i in range(0, self.nbr_noise):
while True:
x = random.uniform(mins[0], maxs[0])
y = random.uniform(mins[1], maxs[1])
pose = [x, y]
if hull.find_simplex(pose) >= 0:
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = 0.
pose.pose.orientation.x = 0.
pose.pose.orientation.x = 0.
pose.pose.orientation.x = 0.
pose.pose.orientation.w = 1.
object_pos = ObjectMeasurement()
object_pos.pose = pose
object_pos.initialization_id = -1
object_pos.timestep = self.timestep
object_pos.observation_id = self.measurement_counter
object_pos.location_id = self.room_id
object_pos.negative_observation = False
self.measurement_counter += 1
self.positions_pub.publish(object_pos)
self.room_time = 0
published = True
break
if published:
self.timestep = self.timestep + 1
def maybe_publish_poses(self, event):
seconds = rospy.Time.now().to_sec()
for j in range(0, self.nbr_targets):
mtime = self.marker_times[j]
#if mtime != 0:
# print "Time diff for ", j , " is: ", mtime - seconds
if mtime != 0 and seconds - mtime > 1:
self.did_move[j] = True
self.marker_times[j] = 0
#pose = PoseStamped()
#pose.header.frame_id = "map"
#pose.header.stamp = rospy.Time.now()
#pose.pose = self.marker_poses[j]
#object_pos = ObjectMeasurement()
#object_pos.pose = pose
#object_pos.initialization_id = j
#object_pos.observation_id = self.measurement_counter
#self.measurement_counter += 1
#self.positions_pub.publish(object_pos)
#self.marker_times[j] = 0
def object_id_color(self, object_id):
colors = {"vivid_yellow": (255, 179, 0),
"strong_purple": (128, 62, 117),
"vivid_orange": (255, 104, 0),
"very_light_blue": (166, 189, 215),
"vivid_red": (193, 0, 32),
"grayish_yellow": (206, 162, 98),
"medium_gray": (129, 112, 102),
# these aren't good for people with defective color vision:
"vivid_green": (0, 125, 52),
"strong_purplish_pink": (246, 118, 142),
"strong_blue": (0, 83, 138),
"strong_yellowish_pink": (255, 122, 92),
"strong_violet": (83, 55, 122),
"vivid_orange_yellow": (255, 142, 0),
"strong_purplish_red": (179, 40, 81),
"vivid_greenish_yellow": (244, 200, 0),
"strong_reddish_brown": (127, 24, 13),
"vivid_yellowish_green": (147, 170, 0),
"deep_yellowish_brown": (89, 51, 21),
"vivid_reddish_orange": (241, 58, 19),
"dark_olive_green": (35, 44, 22)}
color = np.array(colors[colors.keys()[object_id]], dtype=float) / 255.0
return color
def clear_markers(self):
clear_markers = MarkerArray()
pose = Pose()
pose.position.x = 0.
pose.position.y = 0.
pose.position.z = 0.
pose.orientation.x = 0.
pose.orientation.y = 0.
pose.orientation.z = 0.
pose.orientation.w = 1.
for i in range(0, 1000):
clear_marker = Marker()
clear_marker.header.frame_id = "map"
clear_marker.header.stamp = rospy.Time.now()
clear_marker.type = Marker.SPHERE
clear_marker.action = Marker.DELETE
clear_marker.ns = "my_namespace"
clear_marker.id = i
clear_marker.pose = pose
#clear_marker.lifetime = rospy.Time(0)
clear_markers.markers.append(clear_marker)
self.marker_pub.publish(clear_markers)
def initialize_room_markers(self):
for room_id in range(0, len(self.regions)):
pose = Pose()
pose.position.x = self.centers[room_id, 0]
pose.position.y = self.centers[room_id, 1]
pose.position.z = 0.2
pose.orientation.x = 0.
pose.orientation.y = 0.
pose.orientation.z = 0.
pose.orientation.w = 1.
marker = InteractiveMarker()
marker.header.frame_id = "map"
marker.name = "room_marker_" + str(room_id)
marker.description = "Room " + str(room_id)
# the marker in the middle
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.35
box_marker.scale.y = 0.35
box_marker.scale.z = 0.35
box_marker.color.r = 0.
box_marker.color.g = 0.
box_marker.color.b = 1.
box_marker.color.a = 1.
box_marker.id = 1000
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
#box_control.always_visible = False
box_control.markers.append(box_marker)
box_control.name = "button"
box_control.interaction_mode = InteractiveMarkerControl.BUTTON
marker.controls.append(box_control)
#marker.controls.append(box_control)
# move x
#control = InteractiveMarkerControl()
#control.orientation.w = 1
#control.orientation.x = 0
#control.orientation.y = 1
#control.orientation.z = 0
#control.always_visible = True
# control.name = "move_x"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.marker_server.insert(marker, self.room_feedback)
self.marker_server.applyChanges()
self.marker_server.setPose( marker.name, pose )
self.marker_server.applyChanges()
def room_feedback(self, feedback):
room_id = int(feedback.marker_name.rsplit('_', 1)[-1])
print "Room id: ", room_id
self.room_id = room_id
self.room_time = rospy.Time.now().to_sec()
def initialize_object_marker(self, object_id, pose):
print "Adding interactive marker for object: ", object_id
color = self.object_id_color(object_id)
marker = InteractiveMarker()
marker.header.frame_id = "map"
marker.name = "object_marker_" + str(object_id)
marker.description = "Object " + str(object_id)
click_marker = InteractiveMarker()
click_marker.header.frame_id = "map"
click_marker.name = "button_object_marker_" + str(object_id)
click_marker.description = ""
# the marker in the middle
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.25
box_marker.scale.y = 0.25
box_marker.scale.z = 0.25
box_marker.color.r = color[0]
box_marker.color.g = color[1]
box_marker.color.b = color[2]
box_marker.color.a = 1.
box_marker.id = 1000
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
#box_control.always_visible = False
box_control.markers.append(box_marker)
box_control.name = "button"
box_control.interaction_mode = InteractiveMarkerControl.BUTTON
marker.controls.append(box_control)
# move x
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.always_visible = True
# control.name = "move_x"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.name = "move_plane"
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
marker.controls.append(control)
self.marker_poses[object_id] = pose
self.previous_poses[object_id] = pose
self.marker_server.insert(marker, self.marker_feedback)
self.marker_server.applyChanges()
pose.position.z = 0.15
self.marker_server.setPose( marker.name, pose )
self.marker_server.applyChanges()
def marker_feedback(self, feedback):
if feedback.control_name == "button":
object_id = int(feedback.marker_name.rsplit('_', 1)[-1])
self.marker_clicked_times[object_id] = rospy.Time.now().to_sec()
print "Marker id:", feedback.marker_name
print "Object id: ", object_id
elif feedback.control_name == "move_plane":
#self.in_feedback=True
#vertex_name = feedback.marker_name.rsplit('-', 1)
object_id = int(feedback.marker_name.rsplit('_', 1)[-1])
print "Marker id: ", object_id
#self.topo_map.update_node_vertex(node_name, vertex_index, feedback.pose)
#self.update_needed=True
# just do something if there has been no updates for the
# last x seconds
feedback.pose.position.z = 0.15
self.marker_poses[object_id] = feedback.pose
self.marker_times[object_id] = rospy.Time.now().to_sec()
def clicked_callback(self, clicked_pose):
non_initialized = np.nonzero(self.initialized == False)[0]
if len(non_initialized) == 0:
return
ind = non_initialized[0]
self.initialized[ind] = True
self.initialize_object_marker(ind, clicked_pose.pose.pose)
def callback(self, clicked_pose):
if clicked_pose.initialization_id != -1 and not self.initialized[clicked_pose.initialization_id]:
self.initialized[clicked_pose.initialization_id] = True
self.initialize_object_marker(clicked_pose.initialization_id, clicked_pose.pose.pose)
print "Got measurement, adding to GMMPoses ", clicked_pose.initialization_id
color = self.object_id_color(clicked_pose.initialization_id)
sphere_marker = Marker()
sphere_marker.header.frame_id = "map"
sphere_marker.header.stamp = rospy.Time.now()
sphere_marker.ns = "my_namespace"
sphere_marker.id = len(self.markers.markers)
sphere_marker.type = Marker.SPHERE
sphere_marker.action = Marker.ADD
sphere_marker.pose.position.x = clicked_pose.pose.pose.position.x
sphere_marker.pose.position.y = clicked_pose.pose.pose.position.y
sphere_marker.pose.position.z = 0.2
sphere_marker.pose.orientation.x = 0.
sphere_marker.pose.orientation.y = 0.
sphere_marker.pose.orientation.z = 0.
sphere_marker.pose.orientation.w = 1.
sphere_marker.scale.x = 0.2
sphere_marker.scale.y = 0.2
sphere_marker.scale.z = 0.2
sphere_marker.color.a = 1. # Don't forget to set the alpha!
sphere_marker.color.r = color[0]
sphere_marker.color.g = color[1]
sphere_marker.color.b = color[2]
#sphere_marker.lifetime = rospy.Time(secs=1000)
text_marker = Marker()
text_marker.header.frame_id = "map"
text_marker.header.stamp = rospy.Time.now()
text_marker.ns = "my_namespace"
text_marker.id = len(self.markers.markers)+1
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.pose.position.x = clicked_pose.pose.pose.position.x
text_marker.pose.position.y = clicked_pose.pose.pose.position.y
text_marker.pose.position.z = 0.4
text_marker.pose.orientation.x = 0.
text_marker.pose.orientation.y = 0.
text_marker.pose.orientation.z = 0.
text_marker.pose.orientation.w = 1.
text_marker.scale.z = 0.2
text_marker.color.a = 1. # Don't forget to set the alpha!
text_marker.color.r = 0.
text_marker.color.g = 0.
text_marker.color.b = 0.
if clicked_pose.negative_observation:
text_marker.text = "Negative " + str(self.object_counters[clicked_pose.initialization_id])
else:
text_marker.text = str(clicked_pose.timestep) #str(self.object_counters[clicked_pose.initialization_id])
#text_marker.lifetime = rospy.Time(secs=1000)
if clicked_pose.initialization_id != -1: # noise!
self.object_counters[clicked_pose.initialization_id] += 1
self.markers.markers.append(sphere_marker)
self.markers.markers.append(text_marker)
self.marker_pub.publish(self.markers)
class GripperMarkers:
_offset = 0.09
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 get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
if self._tf_listener.frameExists(
from_frame) and self._tf_listener.frameExists(to_frame):
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
# t = rospy.Time.now()
(pos, rot) = self._tf_listener.lookupTransform(
to_frame, from_frame, t) # Note argument order :(
else:
rospy.logerr(
'TF frames do not exist; could not get end effector pose.')
except Exception as err:
rospy.logerr('Could not get end effector pose through TF.')
rospy.logerr(err)
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, feedback):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
'''Moves the arm to the desired joints'''
print feedback
time_to_joint = 2.0
positions = self.ik.get_ik_for_ee(feedback.pose)
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (self.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)
pass
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_' + self.side_prefix
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]))
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
print self
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution is 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
class InteractiveMarkerPoseStampedPublisher():
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 processFeedback(self, feedback):
"""
:type feedback: InteractiveMarkerFeedback
"""
s = "Feedback from marker '" + feedback.marker_name
s += "' / control '" + feedback.control_name + "'"
mp = ""
if feedback.mouse_point_valid:
mp = " at " + str(feedback.mouse_point.x)
mp += ", " + str(feedback.mouse_point.y)
mp += ", " + str(feedback.mouse_point.z)
mp += " in frame " + feedback.header.frame_id
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.logdebug(s + ": button click" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
if feedback.menu_entry_id == 1:
rospy.logdebug("Start publishing transform...")
if self.tf_br is None:
self.tf_br = tf.TransformBroadcaster()
elif feedback.menu_entry_id == 2:
rospy.logdebug("Stop publishing transform...")
self.tf_br = None
# When clicking this event triggers!
elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
rospy.logdebug(s + ": pose changed")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
rospy.logdebug(s + ": mouse down" + mp + ".")
elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.logdebug(s + ": mouse up" + mp + ".")
# TODO: Print here the commands
# tf static transform
self.print_commands(feedback.pose)
self.last_pose = deepcopy(feedback.pose)
self.server.applyChanges()
def print_commands(self, pose, decimals=4):
p = pose.position
o = pose.orientation
print "\n---------------------------"
print "Static transform publisher command (with roll pitch yaw):"
common_part = "rosrun tf static_transform_publisher "
pos_part = str(round(p.x, decimals)) + " " + str(round(p.y, decimals)) + " "+ str(round(p.z, decimals))
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
ori_part = str(round(r, decimals)) + " " + str(round(p, decimals)) + " " + str(round(y, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Static transform publisher command (with quaternion):"
ori_q = str(round(o.x, decimals)) + " " + str(round(o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(round(o.w, decimals))
static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50"
print " " + static_tf_cmd
print
print "Roslaunch line of static transform publisher (rpy):"
node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf"
roslaunch_part = ' <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\
pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />'
print roslaunch_part
print
print "URDF format:" # <origin xyz="0.04149 -0.01221 0.001" rpy="0 0 0" />
print ' <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />'
print "\n---------------------------"
def makeArrow(self, msg):
marker = Marker()
# An arrow that's squashed to easier view the orientation on roll
marker.type = Marker.ARROW
marker.scale.x = 0.08
marker.scale.y = 0.08
marker.scale.z = 0.08
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.5
marker.color.a = 1.0
return marker
def makeBoxControl(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.makeArrow(msg))
msg.controls.append(control)
return control
def makeGraspIM(self, pose):
"""
:type pose: Pose
"""
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.from_frame
int_marker.pose = pose
int_marker.scale = 0.3
int_marker.name = "6dof_eef"
int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
# insert a box, well, an arrow
self.makeBoxControl(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_3d"
control.interaction_mode = InteractiveMarkerControl.MOVE_3D
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
self.menu_handler.insert("Publish transform",
callback=self.processFeedback)
self.menu_handler.insert("Stop publishing transform",
callback=self.processFeedback)
self.server.insert(int_marker, self.processFeedback)
self.menu_handler.apply(self.server, int_marker.name)
def run(self):
r = pi/2.0
p = pi
y = pi/2.0
o = quaternion_from_euler(r, p, y)
opt_frame = self.to_frame + "_rgb_optical_frame"
r = rospy.Rate(20)
while not rospy.is_shutdown():
if self.tf_br is not None:
pos = self.last_pose.position
ori = self.last_pose.orientation
self.tf_br.sendTransform(
(pos.x, pos.y, pos.z),
(ori.x, ori.y, ori.z, ori.w),
rospy.Time.now(),
self.to_frame + "_link",
self.from_frame
)
ps = PoseStamped()
ps.pose = self.last_pose
ps.header.frame_id = self.from_frame
ps.header.stamp = rospy.Time.now()
self.pub.publish(ps)
br = tf.TransformBroadcaster()
br.sendTransform((0, 0, 0),
o,
rospy.Time.now(),
opt_frame,
self.to_frame + "_link")
r.sleep()
class AnnotatorServer(object):
def __init__(self):
self._annotator = Annotator()
self._pose_names_pub = rospy.Publisher("/map_annotator/pose_names",
PoseNames, queue_size=10, latch=True)
# self._map_poses_pub = rospy.Publisher("/map_annotator/map_poses",
# InteractiveMarker, queue_size=10, latch=True)
self._int_marker_server = InteractiveMarkerServer("/map_annotator/map_poses")
self.INITIAL_POSE = Pose()
self.INITIAL_POSE.orientation.w = 1
print("Initializing saved markers: " +
str(self._annotator.get_position_names()))
for name, pose in self._annotator.get_position_items():
self.__create_int_marker__(name, pose)
self.__pub_pose_info__()
print("Initialization finished...")
def __pub_pose_info__(self):
pose_names = PoseNames()
pose_names.names = self._annotator.get_position_names()
pose_names.names.sort()
self._pose_names_pub.publish(pose_names)
def __update_marker_pose__(self, input):
if (input.event_type == InteractiveMarkerFeedback.MOUSE_UP):
name = input.marker_name
new_pose = self._int_marker_server.get(name).pose
# Overwrite the previous pose with the new pose
self._annotator.save_position(name, new_pose)
self._int_marker_server.setPose(name, new_pose)
self._int_marker_server.applyChanges()
print("updated pose for: " + name)
def __create_int_marker__(self, name, pose):
print("creating int marker: " + name)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = name
int_marker.description = name
int_marker.pose = pose
# Move it 0.25 meters up to make it easier to click
int_marker.pose.position.z = 0.25
text_marker = Marker()
text_marker.text = name
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.pose.position.z = 2
text_marker.scale.x = 0.4
text_marker.scale.y = 0.4
text_marker.scale.z = 0.4
text_marker.color.r = 0.0
text_marker.color.g = 0.5
text_marker.color.b = 0.5
text_marker.color.a = 1.0
text_control = InteractiveMarkerControl()
text_control.name = "text_control"
text_control.markers.append(text_marker)
text_control.always_visible = True
text_control.interaction_mode = InteractiveMarkerControl.NONE
int_marker.controls.append(text_control)
rotation_ring_control = InteractiveMarkerControl()
rotation_ring_control.name = "position_control"
rotation_ring_control.always_visible = True
rotation_ring_control.orientation.x = 0
rotation_ring_control.orientation.w = 1
rotation_ring_control.orientation.y = 1
rotation_ring_control.orientation.z = 0
rotation_ring_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(rotation_ring_control)
arrow_marker = Marker()
arrow_marker.type = Marker.ARROW
arrow_marker.pose.orientation.w = 1
arrow_marker.pose.position.z = 0.15
arrow_marker.pose.position.x = -0.5
arrow_marker.scale.x = 1
arrow_marker.scale.y = 0.25
arrow_marker.scale.z = 0.25
arrow_marker.color.r = 0.0
arrow_marker.color.g = 0.5
arrow_marker.color.b = 0.5
arrow_marker.color.a = 1.0
position_control = InteractiveMarkerControl()
position_control.name = "rotation_control"
position_control.always_visible = True
position_control.markers.append(arrow_marker)
position_control.orientation.w = 1
position_control.orientation.x = 0
position_control.orientation.y = 1
position_control.orientation.z = 0
position_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
int_marker.controls.append(position_control)
self._int_marker_server.insert(int_marker, self.__update_marker_pose__)
self._int_marker_server.applyChanges()
def create(self, name, pose=None):
print("creating new pose: " + name)
if pose is None:
pose = self.INITIAL_POSE
self._annotator.save_position(name, pose)
self.__create_int_marker__(name, pose)
self.__pub_pose_info__()
def delete(self, name):
if self._annotator.exists(name):
print("deleting pose: " + name)
self._annotator.delete_position(name)
self._int_marker_server.erase(name)
self._int_marker_server.applyChanges()
self.__pub_pose_info__()
def handle_callback(self, user_action_msg):
cmd = user_action_msg.command
name = user_action_msg.name
if cmd == UserAction.CREATE:
self.create(name)
elif cmd == UserAction.DELETE:
self.delete(name)
elif cmd == UserAction.GOTO:
print("going to pose: " + name)
self._annotator.goto_position(name)
class RvizVisualizer(object):
'''Cute tool for drawing both depth and height-from-bottom in RVIZ
'''
def __init__(self):
rospy.init_node('revisualizer')
self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2)
self.kill_server = InteractiveMarkerServer("interactive_kill")
# text marker
# TODO: Clean this up, there should be a way to set all of this inline
self.surface_marker = visualization_msgs.Marker()
self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
self.surface_marker.scale.z = 0.1
self.depth_marker = visualization_msgs.Marker()
self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.depth_marker.scale.z = 0.1
# create marker for displaying current battery voltage
self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0)
self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5)
self.voltage_marker = visualization_msgs.Marker()
self.voltage_marker.header.frame_id = "base_link"
self.voltage_marker.lifetime = rospy.Duration(5)
self.voltage_marker.ns = 'sub'
self.voltage_marker.id = 22
self.voltage_marker.pose.position.x = -2.0
self.voltage_marker.scale.z = 0.2
self.voltage_marker.color.a = 1
self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING
# create an interactive marker to display kill status and change it
self.need_kill_update = True
self.kill_marker = InteractiveMarker()
self.kill_marker.header.frame_id = "base_link"
self.kill_marker.pose.position.x = -2.3
self.kill_marker.name = "kill button"
kill_status_marker = Marker()
kill_status_marker.type = Marker.TEXT_VIEW_FACING
kill_status_marker.text = "UNKILLED"
kill_status_marker.id = 55
kill_status_marker.scale.z = 0.2
kill_status_marker.color.a = 1.0
kill_button_control = InteractiveMarkerControl()
kill_button_control.name = "kill button"
kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
kill_button_control.markers.append(kill_status_marker)
self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
self.kill_marker.controls.append(kill_button_control)
self.killed = False
# connect kill marker to kill alarm
self.kill_listener = AlarmListener("kill")
self.kill_listener.add_callback(self.kill_alarm_callback)
self.kill_alarm = AlarmBroadcaster("kill")
# distance to bottom
self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
# distance to surface
self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
# battery voltage
self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
def update_kill_button(self):
if self.killed:
self.kill_marker.controls[0].markers[0].text = "KILLED"
self.kill_marker.controls[0].markers[0].color.r = 1
self.kill_marker.controls[0].markers[0].color.g = 0
else:
self.kill_marker.controls[0].markers[0].text = "UNKILLED"
self.kill_marker.controls[0].markers[0].color.r = 0
self.kill_marker.controls[0].markers[0].color.g = 1
self.kill_server.insert(self.kill_marker)
self.kill_server.applyChanges()
def kill_alarm_callback(self, alarm):
self.need_kill_update = False
self.killed = alarm.raised
self.update_kill_button()
def kill_buttton_callback(self, feedback):
if not feedback.event_type == 3:
return
if self.need_kill_update:
return
self.need_kill_update = True
if self.killed:
self.kill_alarm.clear_alarm()
else:
self.kill_alarm.raise_alarm()
def voltage_callback(self, voltage):
self.voltage_marker.text = str(round(voltage.data, 2)) + ' volts'
self.voltage_marker.header.stamp = rospy.Time()
if voltage.data < self.low_battery_threshold:
self.voltage_marker.color.r = 1
self.voltage_marker.color.g = 0
elif voltage.data < self.warn_battery_threshold:
self.voltage_marker.color.r = 1
self.voltage_marker.color.g = 1
else:
self.voltage_marker.color.r = 0
self.voltage_marker.color.g = 1
self.rviz_pub_utils.publish(self.voltage_marker)
def depth_callback(self, msg):
'''Handle depth data sent from depth sensor'''
frame = '/depth'
distance = msg.depth
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=(0.0, 1.0, 0.2, 0.7), # green,
frame=frame,
id=0 # Keep these guys from overwriting eachother
)
self.surface_marker.ns = 'sub'
self.surface_marker.header = mil_ros_tools.make_header(frame='/depth')
self.surface_marker.pose = marker.pose
self.surface_marker.text = str(round(distance, 3)) + 'm'
self.surface_marker.id = 0
self.rviz_pub.publish(marker)
self.rviz_pub_t.publish(self.depth_marker)
def range_callback(self, msg):
'''Handle range data grabbed from dvl'''
# future: should be /base_link/dvl, no?
frame = '/dvl'
distance = msg.range
# Color a sharper red if we're in danger
if distance < 1.0:
color = (1.0, 0.1, 0.0, 0.9)
else:
color = (0.2, 0.8, 0.0, 0.7)
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=color, # red,
frame=frame,
up_vector=np.array([0.0, 0.0, -1.0]), # up is down in range world
id=1 # Keep these guys from overwriting eachother
)
self.depth_marker.ns = 'sub'
self.depth_marker.header = mil_ros_tools.make_header(frame='/dvl')
self.depth_marker.pose = marker.pose
self.depth_marker.text = str(round(distance, 3)) + 'm'
self.depth_marker.id = 1
self.rviz_pub_t.publish(self.depth_marker)
self.rviz_pub.publish(marker)
def make_cylinder_marker(self, base, length, color, frame='/base_link',
up_vector=np.array([0.0, 0.0, 1.0]), **kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(
position=mil_ros_tools.numpy_to_point(center),
orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='sub',
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs
)
return marker
class PR2TrajectoryMarkers(object):
"""
A class to create and store a trajectory for one PR2 arm. The created
trajectory can be published as a PoseArray message.
This class published on the following topics:
~trajectory_markers are the main interactive markers.
~trajectory_poses a markerarray to display the trajectory.
~trajectory_poses a posesarray with the resulting pose
The class subscribes to the topic ~overwrite_trajectory_
to change the stored trajectory. This is useful to resume working on a
trajectory after re-starting the node. The message type is PoseArray.
A std_srvs/Empty service named ~execute_trajectory is provided to
externally trigger the execution of the trajectory.
Constructor:
TrajectoryMarkers(whicharm = "left")
or
TrajectoryMarkers(whicharm = "right")
"""
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)
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 main_callback(self, feedback):
"""
If the mouse button is released change the gripper color according to
the IK result. Quite awkward, trying to get a nicer way to do it.
"""
#publish the gripper pose
gripper_pos = PoseStamped()
gripper_pos.header.frame_id = feedback.header.frame_id
gripper_pos.pose = feedback.pose
self.gripper_pose_pub.publish(gripper_pos)
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.last_gripper_pose = feedback.pose
if (self.last_gripper_pose and
feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP):
self.last_gripper_pose = None
pos = PoseStamped()
pos.header.frame_id = feedback.header.frame_id
pos.pose = feedback.pose
if self.whicharm == "right":
ik = self.planner.check_ik_right_arm
else:
ik = self.planner.check_ik_left_arm
if ik(pos):
color = None
else:
color = (1,0,0,1)
int_marker = self.server.get(self.int_marker.name)
int_marker.controls.remove(self.gripper_marker)
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker)
#rospy.loginfo("Control: %s", int_marker.controls)
self.server.insert(int_marker, self.main_callback)
self.server.setPose(int_marker.name, self.last_gripper_pose)
self.server.applyChanges()
def overwrite_trajectory(self, msg):
self.trajectory = msg
def add_point(self, feedback):
"""
Add a point to self.trajectory if it is allowed by IK.
"""
pos = PoseStamped()
pos.header.frame_id = feedback.header.frame_id
pos.pose = feedback.pose
if self.whicharm == "right":
ik = self.planner.check_ik_right_arm
else:
ik = self.planner.check_ik_left_arm
if ik(pos):
rospy.loginfo("Pose is reachable")
self.trajectory.poses.append(feedback.pose)
else:
rospy.logerr("Pose is not reachable!")
def place_gripper(self, feedback):
"""
Move the marker where the gripper is
"""
if self.whicharm == "right":
gripper_pos = self.planner.get_right_gripper_pose()
else:
gripper_pos = self.planner.get_left_gripper_pose()
self.server.setPose(self.int_marker.name, gripper_pos.pose,
gripper_pos.header)
self.server.applyChanges()
def execute_trajectory(self, feedback):
"""
Executes the tracjectory memorized so far. It interpolates between
the poses and creates suitable times and velocities.
"""
traj = self.interpolate_poses()
if len(traj) == 0:
rospy.logerr("Something went wrong when interpolating")
return
times, vels = self.ik_utils.trajectory_times_and_vels(traj)
if len(vels) == 0 or len(times) == 0:
rospy.logerr("Something went wrong when finding the times")
return
self.joint_controller.execute_trajectory(traj, times, vels,
self.whicharm,
wait = True)
return
def execute_trajectory_srv(self, _):
"""Same as execute_trajectory, but as a service.
"""
self.execute_trajectory(None)
return EmptyResponse()
def arm_trajectory_start(self, feedback):
"""
Move the gripper to the first pose in the trajectory.
"""
if len(self.trajectory.poses) == 0:
rospy.logwarn("Empty trajectory!")
return
pose = self.trajectory.poses[0]
if self.whicharm == "right":
moveit = self.planner.move_right_arm_non_collision
else:
moveit = self.planner.move_left_arm_non_collision
pos, quat = create_tuples_from_pose(pose)
res = moveit(pos, quat, self.trajectory.header.frame_id, 1.0)
if not res:
rospy.logerr("Something went wrong when moving")
return
def clear_trajectory(self, feedback):
"""
Removes all the points stored so far
"""
self.trajectory.poses = []
def move_head(self, feedback):
"""
Moves the head to face the marker
"""
frame = feedback.header.frame_id
pos = (feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
print "Moving the head"
self.joint_controller.time_to_reach = 1.0
self.joint_controller.point_head_to(pos, frame)
def plan_arm(self, feedback):
"""
Moves the arm on the marker using motion collision-aware motion
planning.
"""
frame = feedback.header.frame_id
pos = (feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
orientation = (feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w,
)
if self.whicharm == "right":
rospy.loginfo("Moving the right arm")
self.planner.move_right_arm(pos, orientation, frame, self.planning_waiting_time)
else:
rospy.loginfo("Moving the left arm")
self.planner.move_left_arm(pos, orientation, frame, self.planning_waiting_time)
def collision_free_arm(self, feedback):
"""
Moves the rm on the marker using motion NON-collision-aware inverse
kinematiks.
"""
frame = feedback.header.frame_id
pos = (feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z,
)
orientation = (feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z,
feedback.pose.orientation.w,
)
if self.whicharm == "right":
rospy.loginfo("Moving the right arm (non collision)")
self.planner.move_right_arm_non_collision(pos, orientation,
frame, 2.0)
else:
rospy.loginfo("Moving the left arm (non collision)")
self.planner.move_left_arm_non_collision(pos, orientation,
frame, 2.0)
def update_planning(self, feedback):
"""
Updates the planning scene.
"""
self.planner.take_static_map()
self.planner.update_planning_scene()
def publish_trajectory(self, feedback):
"""
Publishes the trajectory as a PoseArray message
"""
self.trajectory_pub.publish(self.trajectory)
def interpolate_poses(self):
"""
Refines the trajectory by interpolating between the joints.
"""
if len(self.trajectory.poses) < 2:
rospy.logerr("At least two points in the trajectory are needed")
return []
if self.whicharm == "right":
starting_angles = self.robot_state.right_arm_positions
else:
starting_angles = self.robot_state.left_arm_positions
all_trajectory = [starting_angles]
for i in xrange(len(self.trajectory.poses) - 1):
start = PoseStamped()
start.header = self.trajectory.header
start.pose = self.trajectory.poses[i]
end = PoseStamped()
end.header = self.trajectory.header
end.pose = self.trajectory.poses[i+1]
traj, errs = self.ik_utils.check_cartesian_path(start, end,
all_trajectory[-1],
#starting_angles,
#pos_spacing = 0.05,
collision_aware = 0,
num_steps = 5,
)
if any(e == 3 for e in errs):
rospy.logerr("Error while running IK, codes are: %s", errs)
return []
filtered_traj = [t for (t,e) in zip(traj,errs) if e == 0]
all_trajectory.extend(filtered_traj)
all_trajectory = normalize_trajectory(all_trajectory, starting_angles)
rospy.loginfo("New interpolated trajectory with %d elements",
len(all_trajectory))
return all_trajectory
def publish_trajectory_markers(self, duration):
"""
Publishes markers to visualize the current trajectory.
Paremeters:
duration: how long should the markers visualization last. If this
function is called from a loop they last at least the loop rate.
"""
if len(self.trajectory.poses) == 0:
return
msg = MarkerArray()
marker_id = 0
#creating the path connecting the axes
path = Marker()
path.header.frame_id = self.trajectory.header.frame_id
path.ns = "path"
path.action = Marker.ADD
path.type = Marker.LINE_STRIP
path.lifetime = rospy.Duration(duration)
path.color.r = 1
path.color.g = 0
path.color.b = 1
path.color.a = 1
path.scale.x = 0.01
path.id = marker_id
marker_id += 1
for pose in self.trajectory.poses:
pos = PoseStamped()
pos.header.frame_id = self.trajectory.header.frame_id
pos.pose = pose
markers = utils.axis_marker(pos, marker_id, "axes")
msg.markers.extend(markers)
path.points.append(pose.position)
marker_id += 3 #3 axes
msg.markers.append(path)
self.visualizer_pub.publish(msg)
class InteractivePose2DTF(Node):
"""
A node that provides an interactive draggable marker, which publishes a tf
"""
pub_marker = Publisher('/marker', MarkerArray, queue_size=1)
world_frame = Param(str, default='map')
target_frame = Param(str, default='base_link')
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 _set_marker(self, color):
self.interact.clear()
marker = InteractiveMarker(
header=Header(
frame_id=self.world_frame
),
scale=1,
name="Robot",
controls=[
InteractiveMarkerControl(
interaction_mode=InteractiveMarkerControl.MOVE_ROTATE,
orientation=Quaternion(*transformations.quaternion_from_euler(0, np.pi/2, 0)),
markers=[
Marker(
type=Marker.ARROW,
scale=Vector3(0.5, 0.05, 0.05),
color=color,
pose=Pose(
orientation=Quaternion(0, 0, 0, 1)
)
)
]
)
],
pose=self.marker_pose
)
self.interact.insert(marker, self.processFeedback)
self.interact.applyChanges()
def processFeedback(self, feedback):
if feedback.event_type != InteractiveMarkerFeedback.POSE_UPDATE:
return
self.marker_pose = feedback.pose
@Timer(rospy.Duration(0.1))
def timer_tf_pub(self, event):
t = TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = self.world_frame
t.child_frame_id = self.target_frame
t.transform.translation = self.marker_pose.position
t.transform.rotation = self.marker_pose.orientation
self.pub_tf.sendTransform(t)
@Subscriber('~color', ColorRGBA)
def sub_color(self, color):
self._set_marker(color)
@Subscriber('/initialpose', PoseWithCovarianceStamped)
def sub_initialpose(self, pose):
""" The default topic used by rviz """
self.marker_pose = pose.pose.pose
self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5))
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()
# 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 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.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]
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.')
joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
print 'joint: ' + str(joint_positions)
if (joint_positions != None):
self.toggle_arm(self.side_prefix, 'Freeze', False)
#joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
print 'returned from IK: ' + str(joint_positions)
#print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
#joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
print 'done'
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
print 'toggle'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(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)
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]))
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
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_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)
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 IMServer(object):
"""
Spawns interactive Marker which send cart goals to action server.
Does not interact with god map.
"""
def __init__(self,
root_tips,
suffix=u'',
giskard_name=u'giskardpy/command'):
"""
: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(giskard_name)
if len(root_tips) > 0:
self.roots, self.tips = zip(*root_tips)
else:
self.roots = []
self.tips = []
self.suffix = suffix
self.giskard_name = giskard_name
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 make_sphere(self, msg):
"""
:param msg:
:return:
:rtype: Marker
"""
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = msg.scale * MARKER_SCALE * 2
marker.scale.y = msg.scale * MARKER_SCALE * 2
marker.scale.z = msg.scale * MARKER_SCALE * 2
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
return marker
def make_sphere_control(self, msg):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self.make_sphere(msg))
msg.controls.append(control)
return control
def make_6dof_marker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.scale = MARKER_SCALE
int_marker.name = u'eef_{}_to_{}'.format(root_link, tip_link)
# insert a box
self.make_sphere_control(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: u'MOVE_3D',
InteractiveMarkerControl.ROTATE_3D: u'ROTATE_3D',
InteractiveMarkerControl.MOVE_ROTATE_3D: u'MOVE_ROTATE_3D'
}
int_marker.name += u'_' + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = u'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = u'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = u'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = u'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = u'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = u'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.markers[int_marker.name] = int_marker
return int_marker
class process_feedback(object):
def __init__(self, i_server, marker_name, root_link, tip_link, giskard,
enable_self_collision):
"""
:param i_server:
:type i_server: InteractiveMarkerServer
:param marker_name:
:type marker_name: str
:param client:
:type client: SimpleActionClient
:param root_link:
:type root_link: str
:param tip_link:
:type tip_link: str
:param giskard:
:type giskard: GiskardWrapper
:param enable_self_collision:
:type enable_self_collision: bool
"""
self.i_server = i_server
self.marker_name = marker_name
self.tip_link = tip_link
self.root_link = root_link
self.giskard = giskard
self.enable_self_collision = enable_self_collision
self.marker_pub = rospy.Publisher(u'visualization_marker_array',
MarkerArray,
queue_size=10)
def __call__(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
logging.loginfo(u'got interactive goal update')
p = PoseStamped()
p.header.frame_id = feedback.header.frame_id
p.pose = feedback.pose
self.giskard.set_cart_goal(self.root_link, self.tip_link, p)
if not self.enable_self_collision:
self.giskard.allow_self_collision()
self.giskard.plan_and_execute(wait=False)
self.pub_goal_marker(feedback.header, feedback.pose)
self.i_server.setPose(self.marker_name, Pose())
self.i_server.applyChanges()
def pub_goal_marker(self, header, pose):
"""
:param header:
:type header: std_msgs.msg._Header.Header
:param pose:
:type pose: Pose
"""
ma = MarkerArray()
m = Marker()
m.action = Marker.ADD
m.type = Marker.CYLINDER
m.header = header
old_q = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
# x
m.pose = deepcopy(pose)
m.scale.x = 0.05 * MARKER_SCALE
m.scale.y = 0.05 * MARKER_SCALE
m.scale.z = MARKER_SCALE
muh = qv_mult(old_q, [m.scale.z / 2, 0, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(np.pi / 2, [0, 1, 0])))
m.color.r = 1
m.color.g = 0
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 0
ma.markers.append(m)
# y
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, m.scale.z / 2, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(-np.pi / 2, [1, 0, 0])))
m.color.r = 0
m.color.g = 1
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 1
ma.markers.append(m)
# z
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, 0, m.scale.z / 2])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.color.r = 0
m.color.g = 0
m.color.b = 1
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 2
ma.markers.append(m)
self.marker_pub.publish(ma)
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_y"
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_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 = 0
control.orientation.z = 1
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
server.insert(int_marker, processFeedback)
server.applyChanges()
rospy.spin()
class TrajectoryAnalyzer():
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 _retrieve_logs(self, marker_name):
#logs = self._client.message_store.people_perception_marathon_uob.find()
logs = self._client.message_store.people_perception.find()
for log in logs:
#print "logs: " + repr(log)
#print "log keys: " + repr(log.keys())
for i, uuid in enumerate(log['uuids']):
#if uuid not in ['21c75fa0-2ed9-5359-b4db-250142fe0f5d', '89c29b5f-e568-56ea-bca2-f3e59ddff3f7', '0824a8d9-cf9c-5aca-89fc-03e08c14275f']:
# continue
if uuid not in self._traj:
t = Trajectory(uuid)
t.append_pose(log['people'][i],
log['header']['stamp']['secs'],
log['header']['stamp']['nsecs'])
self._traj[uuid] = t
else:
t = self._traj[uuid]
t.append_pose(log['people'][i],
log['header']['stamp']['secs'],
log['header']['stamp']['nsecs'])
#print "pose x,y: " + repr(t.uuid) + repr(t.pose[0]['position'][u'x']) + ", " + repr( t.pose[0]['position']['y'])
#print ""
#sys.exit(1)
def visualize_trajectories(self, mode="all", average_length=0,
longest_length=0):
counter = 0
for uuid in self._traj:
if len(self._traj[uuid].pose) > 1:
if mode == "average":
if abs(self._traj[uuid].length - average_length) \
< (average_length / 10):
self.visualize_trajectory(self._traj[uuid])
counter += 1
elif mode == "longest":
if abs(self._traj[uuid].length - longest_length) \
< (longest_length / 10):
self.visualize_trajectory(self._traj[uuid])
counter += 1
elif mode == "shortest":
if self._traj[uuid].length < 1:
self.visualize_trajectory(self._traj[uuid])
counter += 1
else:
self.visualize_trajectory(self._traj[uuid])
#print "uuid: " + repr(uuid)
#raw_input("Press 'Enter' for the next trajectory.")
#self.delete_trajectory(self._traj[uuid])
counter += 1
rospy.loginfo("Total Trajectories: " + str(len(self._traj)))
rospy.loginfo("Printed trajectories: " + str(counter))
self.delete_trajectory(self._traj[uuid])
def _update_cb(self, feedback):
return
def visualize_trajectory(self, traj):
int_marker = self.create_trajectory_marker(traj)
self._server.insert(int_marker, self._update_cb)
self._server.applyChanges()
def delete_trajectory(self, traj):
self._server.erase(traj.uuid)
self._server.applyChanges()
def create_trajectory_marker(self, traj):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = traj.uuid
# int_marker.description = traj.uuid
pose = Pose()
pose.position.x = traj.pose[0]['position']['x']
pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
# for i in range(len(traj.pose)):
# print "Velocity: ", traj.vel[i]
# print "X,Y: ", traj.pose[i]['position']['x'],\
# traj.pose[i]['position']['y']
# print "Time: ", str(traj.secs[i]) + "." + str(traj.nsecs[i])
# print traj.max_vel, traj.length
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.05
# random.seed(traj.uuid)
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
MOD = 1
for i, point in enumerate(traj.pose):
if i % MOD == 0:
x = point['position']['x']
y = point['position']['y']
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
line_marker.points.append(p)
line_marker.colors = []
for i, vel in enumerate(traj.vel):
if i % MOD == 0:
color = ColorRGBA()
if traj.max_vel == 0:
val = vel / 0.01
else:
val = vel / traj.max_vel
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker