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 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 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 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 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 InteractiveMarkerManager(object):
def __init__(self, server_ns='interactive_markers'):
self.server = InteractiveMarkerServer(server_ns)
self._menu_handlers = {}
self._menu_cmds = {}
self.markers_created_cnt = 0
self.marker_cnt = 0
self._int_marker_name_list = []
self.name_to_marker_dict = {}
self.name_to_position_only_flag = {}
def _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_marker_pose(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['position_only']:
self._set_position_only_checkmark(feedback)
else:
pass
def _set_position_only_checkmark(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)
self.name_to_position_only_flag[feedback.marker_name] = True
else:
menu_handler.setCheckState(menu_entry_handle, MenuHandler.UNCHECKED)
self.name_to_position_only_flag[feedback.marker_name] = False
# Apply marker changes
menu_handler.reApply(self.server)
self.server.applyChanges()
def change_marker_color(self, marker_name, color, opacity=1.0):
assert 0.0 <= opacity <= 1.0
# set color
int_marker = self.server.get(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 marker
if changed:
self._replace_marker(int_marker, replacement_int_marker)
def _replace_marker(self, old_int_marker, replacement_int_marker):
# Update dictionaries
self.name_to_marker_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_marker(self, initial_pose, frame="base_link", description=None):
self.markers_created_cnt += 1
self.marker_cnt += 1
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame
int_marker.pose.position = initial_pose.position
int_marker.scale = 0.1
int_marker.name = rospy.get_name() + str(self.markers_created_cnt)
if description is None:
int_marker.description = int_marker.name
else:
int_marker.description = description
self.name_to_marker_dict[int_marker.name] = int_marker
self.name_to_position_only_flag[int_marker.name] = True
# insert a box # TODO: may change geometry / eff mesh
make_box_control_marker(int_marker)
int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.server.insert(int_marker, self._process_feedback)
# add menus
menu_handler = MenuHandler()
pos_opt_handle = menu_handler.insert("Position-Only Ctrl", callback=self._process_feedback)
self._menu_cmds['position_only'] = pos_opt_handle
menu_handler.setCheckState(pos_opt_handle, MenuHandler.CHECKED)
menu_handler.apply(self.server, int_marker.name)
self._menu_handlers[int_marker.name] = menu_handler
self.server.applyChanges()
self._int_marker_name_list.append(int_marker.name)
return int_marker.name
def _update_marker_pose(self, marker_name, new_pose):
self.name_to_marker_dict[marker_name].pose = new_pose
def get_marker_pose(self, marker_name):
return self.name_to_marker_dict[marker_name].pose
def clear_markers(self):
self.server.clear()
self.server.applyChanges()
self._menu_handlers = {}
self._menu_cmds = {}
self.markers_created_cnt = 0
self.marker_cnt = 0
self._int_marker_name_list = []
self.name_to_marker_dict = {}
self.name_to_position_only_flag = {}
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 World:
'''Handles object recognition, localization, and coordinate space
transformations.'''
tf_listener = None
# Type: [WorldObject]
objects = []
def __init__(self):
# Public attributes
if World.tf_listener is None:
World.tf_listener = TransformListener()
self.surface = None
# Private attributes
self._lock = threading.Lock()
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer(TOPIC_IM_SERVER)
rospy.wait_for_service(SERVICE_BB)
self._bb_service = rospy.ServiceProxy(
SERVICE_BB, FindClusterBoundingBox)
self._object_action_client = actionlib.SimpleActionClient(
ACTION_OBJ_DETECTION, UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo(
'Interactive object detection action server has responded.')
# Setup other ROS machinery
rospy.Subscriber(
TOPIC_OBJ_RECOGNITION, GraspableObjectList,
self.receive_object_info)
rospy.Subscriber(TOPIC_TABLE_SEG, Marker, self.receive_table_marker)
# Init
self.clear_all_objects()
# ##################################################################
# Static methods: Public (API)
# ##################################################################
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix.
Args:
transform (Matrix3x3): (I think this is the correct type.
See ActionStepMarker as a reference for how to use.)
Returns:
Pose
'''
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.
Args:
pose (Pose)
Returns:
Matrix3x3: (I think this is the correct type. See
ActionStepMarker as a reference for how to use.)
'''
pp, po = pose.position, pose.orientation
rotation = [po.x, po.y, po.z, po.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pp.x, pp.y, pp.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state (trasnforming
if relative).
Args:
arm_state (ArmState)
Returns:
Pose
'''
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.
Args:
ref_object (?)
ref_frame_list ([Object]): List of objects (as defined by
Object.msg).
Returns:
Object|None: As in one of Object.msg, or None if no object
was found close enough.
'''
best_dist = 10000 # Not a constant; an absurdly high number.
chosen_obj = None
for ref_frame in ref_frame_list:
dist = World.object_dissimilarity(ref_frame, ref_object)
if dist < best_dist:
best_dist = dist
chosen_obj = ref_frame
if chosen_obj is None:
rospy.loginfo('Did not find a similar object.')
else:
rospy.loginfo('Object dissimilarity is --- ' + str(best_dist))
if best_dist > OBJ_SIMILAR_DIST_THRESHHOLD:
rospy.loginfo('Found some objects, but not similar enough.')
chosen_obj = None
else:
rospy.loginfo(
'Most similar to new object: ' + str(chosen_obj.name))
# Regardless, return the "closest object," which may be None.
return chosen_obj
@staticmethod
def get_frame_list():
'''Function that returns the list of reference frames (Objects).
Returns:
[Object]: List of Object (as defined by Object.msg), the
current reference frames.
'''
return [w_obj.object for w_obj in World.objects]
@staticmethod
def has_objects():
'''Returns whetehr there are any objects (reference frames).
Returns:
bool
'''
return len(World.objects) > 0
@staticmethod
def object_dissimilarity(obj1, obj2):
'''Returns distance between two objects.
Returns:
float
'''
d1 = obj1.dimensions
d2 = obj2.dimensions
return norm(array([d1.x, d1.y, d1.z]) - array([d2.x, d2.y, d2.z]))
@staticmethod
def get_ref_from_name(ref_name):
'''Returns the reference frame type from the reference frame
name specified by ref_name.
Args:
ref_name (str): Name of a referene frame.
Returns:
int: One of ArmState.*, the number code of the reference
frame specified by ref_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.
Args:
arm_frame (ArmState)
ref_frame (int): One of ArmState.*
ref_frame_obj (Object): As in Object.msg
Returns:
ArmState: arm_frame (passed in), but modified.
'''
if ref_frame == ArmState.ROBOT_BASE:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
# Transform from robot base to itself (nothing to do).
rospy.logdebug(
'No reference frame transformations needed (both ' +
'absolute).')
elif arm_frame.refFrame == ArmState.OBJECT:
# Transform from object to robot base.
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:
# Transform from robot base to object.
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:
# Transform between the same object (nothign to do).
if arm_frame.refFrameObject.name == ref_frame_obj.name:
rospy.logdebug(
'No reference frame transformations needed (same ' +
'object).')
else:
# Transform between two different objects.
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):
'''Returns whether the world contains an Object with object_name.
Args:
object_name (str)
Returns:
bool
'''
return object_name in [wobj.object.name for wobj in World.objects]
@staticmethod
def is_frame_valid(object_name):
'''Returns whether the frame (object) name is valid for
transforms.
Args:
object_name (str)
Returns:
bool
'''
return object_name == 'base_link' or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
'''Transforms a pose between two reference frames. If there is a
TF exception or object does not exist, it will return the pose
back without any transforms.
Args:
pose (Pose)
from_frame (str)
to_frame (str)
Returns:
Pose
'''
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('ServiceException during transform.')
return pose
else:
rospy.logdebug(
'One of the frame objects might not exist: ' + from_frame +
' or ' + to_frame)
return pose
@staticmethod
def pose_distance(pose1, pose2, is_on_table=True):
'''Returns distance between two world poses.
Args:
pose1 (Pose)
pose2 (Pose)
is_on_table (bool, optional): Whether the objects are on the
table (if so, disregards z-values in computations).
Returns:
float
'''
if pose1 == [] or pose2 == []:
return 0.0
else:
p1p = pose1.position
p2p = pose2.position
if is_on_table:
arr1 = array([p1p.x, p1p.y])
arr2 = array([p2p.x, p2p.y])
else:
arr1 = array([p1p.x, p1p.y, p1p.z])
arr2 = array([p2p.x, p2p.y, p2p.z])
dist = norm(arr1 - arr2)
if dist < OBJ_DIST_ZERO_CLAMP:
dist = 0
return dist
@staticmethod
def log_pose(log_fn, pose):
'''For printing a pose to rosout. We don't do it on one line
becuase that messes up the indentation with the rest of the log.
Args:
log_fn (function(str)): A logging function that takes a
string as an argument. For example, rospy.loginfo.
pose (Pose): The pose to log
'''
p, o = pose.position, pose.orientation
log_fn(' - position: (%f, %f, %f)' % (p.x, p.y, p.z))
log_fn(' - orientation: (%f, %f, %f, %f)' % (o.x, o.y, o.z, o.w))
# ##################################################################
# Static methods: Internal ("private")
# ##################################################################
@staticmethod
def _get_mesh_marker(marker, mesh):
'''Generates and returns a marker from a mesh.
Args:
marker (Marker)
mesh (Mesh)
Returns:
Marker
'''
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
@staticmethod
def _get_surface_marker(pose, dimensions):
'''Returns a surface marker with provided pose and dimensions.
Args:
pose (Pose)
dimensions (Vector3)
Returns:
InteractiveMarker
'''
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=MARKER_DURATION,
scale=dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_SURFACE,
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=SCALE_TEXT, text=int_marker.name,
color=COLOR_TEXT,
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
# ##################################################################
# Instance methods: Public (API)
# ##################################################################
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, SURFACE_HEIGHT)
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 is not None:
rospy.logwarn(
'Adding the recognized object with most ' +
'confident model.')
self._add_new_object(
object_pose,
DIMENSIONS_OBJ,
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 is not None:
rospy.loginfo('Adding unrecognized object with pose:')
World.log_pose(rospy.loginfo, cluster_pose)
rospy.loginfo(
'...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()
def update_object_pose(self):
''' Function to externally update an object pose.'''
# Look down at the table.
rospy.loginfo('Head attempting to look at table.')
Response.perform_gaze_action(GazeGoal.LOOK_DOWN)
while (Response.gaze_client.get_state() == GoalStatus.PENDING or
Response.gaze_client.get_state() == GoalStatus.ACTIVE):
rospy.sleep(PAUSE_SECONDS)
if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr('Could not look down to take table snapshot')
return False
rospy.loginfo('Head is now (successfully) stairing at table.')
# Reset object recognition.
rospy.loginfo('About to attempt to reset object recognition.')
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):
rospy.sleep(PAUSE_SECONDS)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
self._reset_objects() # Also do this internally.
if self._object_action_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr('Could not reset recognition.')
return False
# Do segmentation
rospy.loginfo('About to attempt table 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):
rospy.sleep(PAUSE_SECONDS)
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.logwarn('Could not segment.')
return False
# Do recognition
rospy.loginfo('About to attempt object 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):
rospy.sleep(PAUSE_SECONDS)
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 = rospy.Duration(0.0)
while (not World.has_objects() and
wait_time < RECOGNITION_TIMEOUT_SECONDS):
rospy.sleep(PAUSE_SECONDS)
wait_time += PAUSE_SECONDS
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):
rospy.sleep(PAUSE_SECONDS)
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):
'''Returns the nearest object, if one exists.
Args:
arm_pose (Pose): End-effector pose.
Returns:
Object|None: As in Object.msg, the nearest object (if it
is close enough), or None if there were none close
enough.
'''
# First, find which object is the closest.
distances = []
for wobj in World.objects:
dist = World.pose_distance(wobj.object.pose, arm_pose)
distances.append(dist)
# Then, see if the closest is actually below our threshhold for
# a 'closest object.'
if len(distances) > 0:
if min(distances) < OBJ_NEAREST_DIST_THRESHHOLD:
chosen = distances.index(min(distances))
return World.objects[chosen].object
# We didn't have any objects or none were close enough.
return None
def marker_feedback_cb(self, feedback):
'''Callback for when feedback from a marker is received.
Args:
feedback (InteractiveMarkerFeedback)
'''
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:
# This happens a ton, and doesn't need to be logged like
# normal events (e.g. clicking on most marker controls
# fires here).
rospy.logdebug('Unknown event: ' + str(feedback.event_type))
def update(self):
'''Update function called in a loop.
Returns:
bool: Whether any tracked objects were removed, AKA "is
world changed."
'''
# 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 is not None:
self._remove_object(to_remove)
is_world_changed = True
self._lock.release()
return is_world_changed
# ##################################################################
# Instance methods: Internal ("private")
# ##################################################################
def _reset_objects(self):
'''Removes all objects.'''
self._lock.acquire()
for wobj in World.objects:
self._im_server.erase(wobj.int_marker.name)
self._im_server.applyChanges()
if self.surface is not None:
self._remove_surface()
self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def _add_new_object(self, pose, dimensions, is_recognized, mesh=None):
'''Maybe add a new object with the specified properties to our
object list.
It might not be added if too similar of an object already
exists (and has been added).
Args:
pose (Pose)
dimensions (Vector3)
is_recognized (bool)
mesh (Mesh, optional): A mesh, if it exists. Default is
None.
Returns:
bool: Whether the object was actually added.
'''
to_remove = None
if is_recognized:
# TODO(mbforbes): Re-implement object recognition or remove
# this dead code.
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 < OBJ_ADD_DIST_THRESHHOLD:
# 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
# # Remove any duplicate objects.
# if to_remove is not None:
# self._remove_object(to_remove)
# # Actually add the object.
# self._add_new_object_internal(
# pose, dimensions, is_recognized, mesh)
# return True
else:
# Whether whether we already have an object at ~ the same
# location (and if so, don't add).
for wobj in World.objects:
if (World.pose_distance(wobj.object.pose, pose)
< OBJ_ADD_DIST_THRESHHOLD):
rospy.loginfo(
'Previously detected object at the same location, ' +
'will not add this object.')
return False
# Actually add the object.
self._add_new_object_internal(
pose, dimensions, is_recognized, mesh)
return True
def _add_new_object_internal(self, pose, dimensions, is_recognized, mesh):
'''Does the 'internal' adding of an object with the passed
properties. Call _add_new_object to do all pre-requisite checks
first (it then calls this function).
Args:
pose (Pose)
dimensions (Vector3)
is_recognized (bool)
mesh (Mesh|None): A mesh, if it exists (can be None).
'''
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()
def _remove_object(self, to_remove):
'''Remove an object by index.
Args:
to_remove (int): Index of the object to remove in
World.objects.
'''
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()
# TODO(mbforbes): Re-implement object recognition or remove
# this dead code.
# 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 (from IM).'''
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 and return a marker for world objects.
Args:
index (int): ID for the new marker.
mesh (Mesh, optional): Mesh to use for the marker. Only
utilized if not None. Defaults to None.
Returns:
InteractiveMarker
'''
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=MARKER_DURATION,
scale=World.objects[index].object.dimensions,
header=Header(frame_id=BASE_LINK),
color=COLOR_OBJ,
pose=World.objects[index].object.pose
)
if mesh is not 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 + OFFSET_OBJ_TEXT_Z)
button_control.markers.append(
Marker(
type=Marker.TEXT_VIEW_FACING,
id=index,
scale=SCALE_TEXT,
text=int_marker.name,
color=COLOR_TEXT,
header=Header(frame_id=BASE_LINK),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))
)
)
int_marker.controls.append(button_control)
return int_marker
def _publish_tf_pose(self, pose, name, parent):
''' Publishes a TF for object named name with pose pose and
parent reference frame parent.
Args:
pose (Pose): The object's pose.
name (str): The object's name.
parent (str): The parent reference frame.
'''
if pose is not None:
pp = pose.position
po = pose.orientation
pos = (pp.x, pp.y, pp.z)
rot = (po.x, po.y, po.z, po.w)
# TODO(mbforbes): Is it necessary to change the position
# and orientation into tuples to send to TF?
self._tf_broadcaster.sendTransform(
pos, rot, rospy.Time.now(), name, parent)
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 IntCollisionEnv(CollisionEnvServices):
def __init__(self, setup, world_frame):
super(IntCollisionEnv, self).__init__(setup, world_frame)
self.im_server = InteractiveMarkerServer(self.NS + "markers")
self.check_attached_timer = rospy.Timer(rospy.Duration(0.1),
self.check_attached_timer_cb)
self.create_menus()
def create_menus(self):
self.global_menu_handler = MenuHandler()
g_art = self.global_menu_handler.insert("Artificial")
self.global_menu_handler.insert("Add primitive",
parent=g_art,
callback=self.global_menu_add_prim_cb)
self.global_menu_handler.insert("Clear all",
parent=g_art,
callback=self.global_menu_clear_all_cb)
self.global_menu_handler.insert(
"Clear all (permanent)",
parent=g_art,
callback=self.global_menu_clear_all_perm_cb)
self.global_menu_handler.insert("Save all",
parent=g_art,
callback=self.global_menu_save_all_cb)
self.global_menu_handler.insert("Reload",
parent=g_art,
callback=self.global_menu_reload_cb)
g_det = self.global_menu_handler.insert("Detected")
self.global_menu_handler.insert("Pause",
parent=g_det,
callback=self.global_menu_pause_cb)
self.global_menu_handler.insert(
"Clear all",
parent=g_det,
callback=self.global_menu_det_clear_all_cb)
self.a_obj_menu_handler = MenuHandler()
mov = self.a_obj_menu_handler.insert("Moveable",
callback=self.menu_moveable_cb)
self.a_obj_menu_handler.setCheckState(mov, MenuHandler.UNCHECKED)
sc = self.a_obj_menu_handler.insert("Scaleable",
callback=self.menu_scaleable_cb)
self.a_obj_menu_handler.setCheckState(sc, MenuHandler.UNCHECKED)
self.a_obj_menu_handler.insert("Save", callback=self.menu_save_cb)
self.a_obj_menu_handler.insert("Clear", callback=self.menu_remove_cb)
self.d_obj_menu_handler = MenuHandler()
self.d_obj_menu_handler.insert("Clear", callback=self.d_menu_clear_cb)
int_marker = InteractiveMarker()
int_marker.header.frame_id = self.world_frame
int_marker.pose.position.z = 1.2
int_marker.scale = 0.5
int_marker.name = "global_menu"
int_marker.description = "Global Menu"
control = InteractiveMarkerControl()
control.interaction_mode = InteractiveMarkerControl.BUTTON
control.always_visible = True
marker = Marker()
marker.type = Marker.CUBE
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
marker.color.a = 0.5
control.markers.append(marker)
int_marker.controls.append(control)
self.im_server.insert(int_marker, self.ignored_cb)
self.global_menu_handler.apply(self.im_server, int_marker.name)
self.im_server.applyChanges()
def ignored_cb(self, feedback):
pass
def d_menu_clear_cb(self, f):
pass
def global_menu_det_clear_all_cb(self, f):
for name in self.clear_all_det():
self.im_server.erase(name)
self.im_server.applyChanges()
def global_menu_pause_cb(self, f):
handle = f.menu_entry_id
state = self.global_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.global_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
self.paused = False
else:
self.global_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self.paused = True
self.global_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def global_menu_save_all_cb(self, f):
self.save_primitives()
def global_menu_add_prim_cb(self, feedback):
p = CollisionPrimitive()
p.name = self._generate_name()
p.bbox.type = SolidPrimitive.BOX
p.bbox.dimensions = [0.1, 0.1, 0.1]
p.pose.header.frame_id = self.world_frame
p.pose.pose.orientation.w = 1
p.pose.pose.position.z = 0.5
p.setup = self.setup
self.add_primitive(p)
def global_menu_reload_cb(self, feedback):
self.reload()
def global_menu_clear_all_cb(self, feedback):
self.clear_all(permanent=False)
def global_menu_clear_all_perm_cb(self, feedback):
self.clear_all()
def menu_save_cb(self, f):
self.save_primitive(f.marker_name)
def menu_scaleable_cb(self, f):
handle = f.menu_entry_id
state = self.a_obj_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.a_obj_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
# TODO
else:
self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
# TODO
self.a_obj_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def menu_moveable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.a_obj_menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.a_obj_menu_handler.setCheckState(handle,
MenuHandler.UNCHECKED)
# if feedback.marker_name in self.moveable:
# self.moveable.remove(feedback.marker_name)
self.im_server.erase(feedback.marker_name)
self.im_server.insert(
make_def(self.artificial_objects[feedback.marker_name]),
self.process_im_feedback)
else:
self.a_obj_menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self.make_interactive(
self.artificial_objects[feedback.marker_name])
self.a_obj_menu_handler.reApply(self.im_server)
self.im_server.applyChanges()
def menu_remove_cb(self, feedback):
self.remove_name(feedback.marker_name)
def process_im_feedback(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
ps = PoseStamped()
ps.header = feedback.header
ps.pose = feedback.pose
if feedback.marker_name in self.artificial_objects:
self.set_primitive_pose(feedback.marker_name, ps)
elif feedback.marker_name:
# detected objects
pass
def make_interactive(self, p):
im = self.im_server.get(p.name)
for v in (("x", (1, 0, 0, 1)), ("y", (0, 0, 1, 1)), ("z", (0, 1, 0,
1))):
im.controls.append(rotate_axis_control("rotate_" + v[0], v[1]))
im.controls.append(move_axis_control("move_" + v[0], v[1]))
def remove_name(self, name):
super(IntCollisionEnv, self).remove_name(name)
self.im_server.erase(name)
self.im_server.applyChanges()
def add_primitive(self, p):
super(IntCollisionEnv, self).add_primitive(p)
self.im_server.insert(make_def(p), self.process_im_feedback)
self.a_obj_menu_handler.apply(self.im_server, p.name)
self.im_server.applyChanges()
def reload(self):
self.im_server.clear()
self.create_menus()
super(IntCollisionEnv, self).reload()
def add_detected(self, name, ps, object_type):
super(IntCollisionEnv, self).add_detected(name, ps, object_type)
p = CollisionPrimitive()
p.name = name
p.pose = ps
p.bbox = object_type.bbox
self.im_server.insert(make_def(p, color=(0, 0, 1)),
self.process_im_feedback)
self.im_server.applyChanges()
def clear_detected(self, name):
super(IntCollisionEnv, self).clear_detected(name)
self.im_server.erase(name)
self.im_server.applyChanges()
def check_attached_timer_cb(self, evt):
for name, ps, bbox in self.get_attached():
p = CollisionPrimitive()
p.name = name
p.pose = ps
p.bbox = bbox
self.im_server.insert(make_def(p, color=(1, 0, 0)),
self.process_im_feedback)
self.im_server.applyChanges()
class SkirosInteractiveMarkers:
default_box_size = 0.1
def on_marker_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
print s
print mp
def _make_box(self, msg, size):
marker = Marker()
marker.type = Marker.CUBE
if None in size:
size = [SkirosInteractiveMarkers.default_box_size, SkirosInteractiveMarkers.default_box_size, SkirosInteractiveMarkers.default_box_size]
marker.scale.x = msg.scale * size[0]
marker.scale.y = msg.scale * size[1]
marker.scale.z = msg.scale * size[2]
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 0.5
marker.color.a = 0.5
return marker
def _make_box_control(self, msg, size):
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(self._make_box(msg, size))
msg.controls.append(control)
return control
def initInteractiveServer(self, name):
"""
@brief Start the interactive marker server
"""
self._server = InteractiveMarkerServer(name)
def clear_markers(self):
self._server.clear()
def make_6dof_marker(self, pose, size, frame_id, base_frame_id, interaction_mode):
int_marker = InteractiveMarker()
int_marker.header.frame_id = base_frame_id
int_marker.pose.position.x = pose[0][0]
int_marker.pose.position.y = pose[0][1]
int_marker.pose.position.z = pose[0][2]
int_marker.pose.orientation.x = pose[1][0]
int_marker.pose.orientation.y = pose[1][1]
int_marker.pose.orientation.z = pose[1][2]
int_marker.pose.orientation.w = pose[1][3]
int_marker.scale = 1
int_marker.name = frame_id
int_marker.description = frame_id
# insert a box
self._make_box_control(int_marker, size)
int_marker.controls[0].interaction_mode = interaction_mode
n = norm([1, 1])
control = InteractiveMarkerControl()
control.orientation.w = 1 / n
control.orientation.x = 1 / n
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1 / n
control.orientation.x = 1 / n
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1 / n
control.orientation.x = 0
control.orientation.y = 1 / n
control.orientation.z = 0
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1 / n
control.orientation.x = 0
control.orientation.y = 1 / n
control.orientation.z = 0
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1 / n
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1 / n
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1 / n
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1 / n
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self._server.insert(int_marker, self.on_marker_feedback)
self._server.applyChanges()
