From 32908ae55556f1182e9e6f43302ffafecee9fe1f Mon Sep 17 00:00:00 2001 From: Shushman Choudhury Date: Mon, 1 Feb 2016 12:44:04 -0500 Subject: [PATCH 01/21] Hacked module for Chisel --- src/prpy/perception/chisel.py | 70 +++++++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 src/prpy/perception/chisel.py diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py new file mode 100644 index 0000000..57399a5 --- /dev/null +++ b/src/prpy/perception/chisel.py @@ -0,0 +1,70 @@ +import rospy +import chisel_msgs.srv +import chisel_msgs.msg +import math +import tf +import tf.transformations as transformations +import numpy +import os.path + +''' +Typical way of running + +from prpy.perception.chisel import ChiselModule +from offscreen_render import ros_camera_sim +robot.DetectObjects() +table = env.GetKinBody('table127') +glass = env.GetKinBody('plastic_glass124') +camera = ros_camera_sim.RosCameraSim(env) +camera.start('/head/kinect2/qhd') +camera.add_body(table) +camera.add_body(glass) +mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker') +chisel_det = ChiselModule(mesh_client,'Chisel') + +robot.DetectObjects() +#reset chisel mesh +mesh_client.SendCommand('GetMesh Chisel/full_mesh') +''' + + + +class ChiselModule(): + + def __init__(self,mesh_client,service_namespace=None): + + try: + rospy.init_node('chisel_detector',anonymous=True) + except rospy.exceptions.ROSException: + pass + + if service_namespace is None: + service_namespace = '/Chisel' + + self.mesh_client = mesh_client + self.serv_ns = service_namespace + + def DetectObject(self,robot,**kw_args): + + env = robot.GetEnv() + already_in_env = False + + #Check if kinbody in env + for body in env.GetBodies(): + if body.GetName() == 'Chisel/full_mesh': + already_in_env = True + break + + #Remove kinbody if present + if already_in_env: + chisel_kb = env.GetKinBody('Chisel/full_mesh') + env.RemoveKinBody(chisel_kb) + + #Reset Chisel + srv_nm = self.serv_ns+'/Reset' + rospy.wait_for_service(srv_nm) + detect_chisel_refresh = rospy.ServiceProxy(srv_nm, + chisel_msgs.srv.ResetService) + + #Get Kinbody and load into env + #self.mesh_client.SendCommand('GetMesh Chisel/full_mesh') From 17b4c4f8f67b765f025d3ebe0343c82a10291ea2 Mon Sep 17 00:00:00 2001 From: Shushman Choudhury Date: Fri, 26 Feb 2016 14:14:11 -0500 Subject: [PATCH 02/21] For chisel module --- src/prpy/perception/chisel.py | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index 57399a5..d0d1814 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -19,6 +19,19 @@ camera.start('/head/kinect2/qhd') camera.add_body(table) camera.add_body(glass) + +Nodelet manager screen +rosrun nodelet nodelet manager __name:=nodelet_manager +Depth converter screen +rosrun nodelet nodelet load depth_image_proc/convert_metric nodelet_manager image_raw:=/head/kinect2/qhd/image_depth_rect image:=/head/kinect2/qhd/image_depth_rect_float +In herb2, source herb2_chisel_ws/devel/setup.bash +Depth mask screen +roslaunch depth_mask launch_depth_mask.launch (args are correct) + +Chisel screen +roslaunch chisel_ros launch_kinect_local.launch + +IN PYTHON mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker') chisel_det = ChiselModule(mesh_client,'Chisel') @@ -39,7 +52,7 @@ def __init__(self,mesh_client,service_namespace=None): pass if service_namespace is None: - service_namespace = '/Chisel' + service_namespace = 'Chisel' self.mesh_client = mesh_client self.serv_ns = service_namespace @@ -55,16 +68,16 @@ def DetectObject(self,robot,**kw_args): already_in_env = True break - #Remove kinbody if present + #Remove kinbody if present if already_in_env: chisel_kb = env.GetKinBody('Chisel/full_mesh') env.RemoveKinBody(chisel_kb) - #Reset Chisel + #Reset Chisel srv_nm = self.serv_ns+'/Reset' rospy.wait_for_service(srv_nm) detect_chisel_refresh = rospy.ServiceProxy(srv_nm, chisel_msgs.srv.ResetService) - #Get Kinbody and load into env + ` #Get Kinbody and load into env #self.mesh_client.SendCommand('GetMesh Chisel/full_mesh') From 199d542e2d5821fee69a68a0913efe4a8d8cd6b5 Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Fri, 26 Feb 2016 16:13:33 -0500 Subject: [PATCH 03/21] Updating package to work with single call to DetectObject --- src/prpy/perception/chisel.py | 44 ++++++++++++++++++++++++++--------- 1 file changed, 33 insertions(+), 11 deletions(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index d0d1814..6979444 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -40,11 +40,9 @@ mesh_client.SendCommand('GetMesh Chisel/full_mesh') ''' - - class ChiselModule(): - def __init__(self,mesh_client,service_namespace=None): + def __init__(self, env, mesh_client=None, service_namespace=None): try: rospy.init_node('chisel_detector',anonymous=True) @@ -54,14 +52,36 @@ def __init__(self,mesh_client,service_namespace=None): if service_namespace is None: service_namespace = 'Chisel' + if mesh_client is None: + import openravepy + mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker') + self.mesh_client = mesh_client self.serv_ns = service_namespace - def DetectObject(self,robot,**kw_args): + from offscreen_render import ros_camera_sim + self.camera = ros_camera_sim.RosCameraSim(env) + self.camera.start('/head/kinect2/qhd') + self.camera_bodies = [] + + + self.detect_chisel_refresh=None + + + + def DetectObject(self, robot, ignored_bodies=None, **kw_args): env = robot.GetEnv() already_in_env = False + if ignored_bodies is None: + ignored_bodies = [] + + for b in ignored_bodies: + if b not in self.camera_bodies: + self.camera.add_body(b) + self.camera_bodies.push_back(b) + #Check if kinbody in env for body in env.GetBodies(): if body.GetName() == 'Chisel/full_mesh': @@ -74,10 +94,12 @@ def DetectObject(self,robot,**kw_args): env.RemoveKinBody(chisel_kb) #Reset Chisel - srv_nm = self.serv_ns+'/Reset' - rospy.wait_for_service(srv_nm) - detect_chisel_refresh = rospy.ServiceProxy(srv_nm, - chisel_msgs.srv.ResetService) - - ` #Get Kinbody and load into env - #self.mesh_client.SendCommand('GetMesh Chisel/full_mesh') + if self.detect_chisel_refresh is None: + srv_nm = self.serv_ns+'/Reset' + rospy.wait_for_service(srv_nm) + detect_chisel_refresh = rospy.ServiceProxy(srv_nm, + chisel_msgs.srv.ResetService) + detect_chisel_refresh() + + #Get Kinbody and load into env + self.mesh_client.SendCommand('GetMesh Chisel/full_mesh') From bde5ebfbaa18de00b4af367e3697afa9a06808ac Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Fri, 26 Feb 2016 17:09:32 -0500 Subject: [PATCH 04/21] Working on transforming chisel mesh to herb frame --- src/prpy/perception/chisel.py | 34 ++++++++++++++++++++++++++++++---- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index 6979444..57b581d 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -42,7 +42,8 @@ class ChiselModule(): - def __init__(self, env, mesh_client=None, service_namespace=None): + def __init__(self, env, mesh_client=None, service_namespace=None, + reference_link=None): try: rospy.init_node('chisel_detector',anonymous=True) @@ -64,10 +65,11 @@ def __init__(self, env, mesh_client=None, service_namespace=None): self.camera.start('/head/kinect2/qhd') self.camera_bodies = [] - self.detect_chisel_refresh=None - + self.reference_link=reference_link + import tf + self.listener = tf.TransformListener() def DetectObject(self, robot, ignored_bodies=None, **kw_args): @@ -80,7 +82,7 @@ def DetectObject(self, robot, ignored_bodies=None, **kw_args): for b in ignored_bodies: if b not in self.camera_bodies: self.camera.add_body(b) - self.camera_bodies.push_back(b) + self.camera_bodies.append(b) #Check if kinbody in env for body in env.GetBodies(): @@ -103,3 +105,27 @@ def DetectObject(self, robot, ignored_bodies=None, **kw_args): #Get Kinbody and load into env self.mesh_client.SendCommand('GetMesh Chisel/full_mesh') + + self.listener.waitForTransform( + '/map', + '/herb_base', + rospy.Time(), + rospy.Duration(10)) + frame_trans, frame_rot = self.listener.lookupTransform( + '/map', '/herb_base', + rospy.Time(0)) + from tf.transformations import quaternion_matrix + offset = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) + offset[0,3] = frame_trans[0] + offset[1,3] = frame_trans[1] + offset[2,3] = frame_trans[2] + + chisel_mesh = env.GetKinBody('Chisel/full_mesh') + if chisel_mesh is not None and self.reference_link is not None: + print 'Moving mesh' + t = numpy.dot(offset, chisel_mesh.GetTransform()) + ref_link_pose = self.reference_link.GetTransform() + t = numpy.dot(ref_link_pose, t) + chisel_mesh.SetTransform(t) + + return chisel_mesh From 13227e507f414efbe024333812ee954a9ae67047 Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Fri, 26 Feb 2016 17:10:59 -0500 Subject: [PATCH 05/21] Flipping to correct transform --- src/prpy/perception/chisel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index 57b581d..fff915e 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -112,7 +112,7 @@ def DetectObject(self, robot, ignored_bodies=None, **kw_args): rospy.Time(), rospy.Duration(10)) frame_trans, frame_rot = self.listener.lookupTransform( - '/map', '/herb_base', + '/herb_base', '/map', rospy.Time(0)) from tf.transformations import quaternion_matrix offset = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) From 245764513786181553e10d68e8b2e35924f2d2c3 Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Mon, 29 Feb 2016 20:17:55 -0800 Subject: [PATCH 06/21] Cleaning up chisel class. Adding kinbody_helper class. Currently contains helper function for performing transformations on detected objects. --- src/prpy/perception/chisel.py | 230 ++++++++++++-------------- src/prpy/perception/kinbody_helper.py | 36 ++++ 2 files changed, 139 insertions(+), 127 deletions(-) create mode 100644 src/prpy/perception/kinbody_helper.py diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index fff915e..c53f5f4 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -1,131 +1,107 @@ -import rospy -import chisel_msgs.srv -import chisel_msgs.msg -import math -import tf -import tf.transformations as transformations -import numpy -import os.path +import logging +from base import PerceptionModule, PerceptionMethod -''' -Typical way of running +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) -from prpy.perception.chisel import ChiselModule -from offscreen_render import ros_camera_sim -robot.DetectObjects() -table = env.GetKinBody('table127') -glass = env.GetKinBody('plastic_glass124') -camera = ros_camera_sim.RosCameraSim(env) -camera.start('/head/kinect2/qhd') -camera.add_body(table) -camera.add_body(glass) - -Nodelet manager screen -rosrun nodelet nodelet manager __name:=nodelet_manager -Depth converter screen -rosrun nodelet nodelet load depth_image_proc/convert_metric nodelet_manager image_raw:=/head/kinect2/qhd/image_depth_rect image:=/head/kinect2/qhd/image_depth_rect_float -In herb2, source herb2_chisel_ws/devel/setup.bash -Depth mask screen -roslaunch depth_mask launch_depth_mask.launch (args are correct) - -Chisel screen -roslaunch chisel_ros launch_kinect_local.launch - -IN PYTHON -mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker') -chisel_det = ChiselModule(mesh_client,'Chisel') - -robot.DetectObjects() -#reset chisel mesh -mesh_client.SendCommand('GetMesh Chisel/full_mesh') -''' - -class ChiselModule(): - - def __init__(self, env, mesh_client=None, service_namespace=None, - reference_link=None): - - try: - rospy.init_node('chisel_detector',anonymous=True) - except rospy.exceptions.ROSException: - pass - - if service_namespace is None: - service_namespace = 'Chisel' - - if mesh_client is None: - import openravepy - mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker') - - self.mesh_client = mesh_client - self.serv_ns = service_namespace - - from offscreen_render import ros_camera_sim - self.camera = ros_camera_sim.RosCameraSim(env) - self.camera.start('/head/kinect2/qhd') - self.camera_bodies = [] - - self.detect_chisel_refresh=None - self.reference_link=reference_link - - import tf - self.listener = tf.TransformListener() - - def DetectObject(self, robot, ignored_bodies=None, **kw_args): +class ChiselModule(PerceptionModule): - env = robot.GetEnv() - already_in_env = False - - if ignored_bodies is None: - ignored_bodies = [] - - for b in ignored_bodies: - if b not in self.camera_bodies: - self.camera.add_body(b) - self.camera_bodies.append(b) - - #Check if kinbody in env - for body in env.GetBodies(): - if body.GetName() == 'Chisel/full_mesh': - already_in_env = True - break - - #Remove kinbody if present - if already_in_env: - chisel_kb = env.GetKinBody('Chisel/full_mesh') - env.RemoveKinBody(chisel_kb) - - #Reset Chisel - if self.detect_chisel_refresh is None: - srv_nm = self.serv_ns+'/Reset' - rospy.wait_for_service(srv_nm) - detect_chisel_refresh = rospy.ServiceProxy(srv_nm, - chisel_msgs.srv.ResetService) - detect_chisel_refresh() - - #Get Kinbody and load into env - self.mesh_client.SendCommand('GetMesh Chisel/full_mesh') - - self.listener.waitForTransform( - '/map', - '/herb_base', - rospy.Time(), - rospy.Duration(10)) - frame_trans, frame_rot = self.listener.lookupTransform( - '/herb_base', '/map', - rospy.Time(0)) - from tf.transformations import quaternion_matrix - offset = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) - offset[0,3] = frame_trans[0] - offset[1,3] = frame_trans[1] - offset[2,3] = frame_trans[2] - - chisel_mesh = env.GetKinBody('Chisel/full_mesh') - if chisel_mesh is not None and self.reference_link is not None: - print 'Moving mesh' - t = numpy.dot(offset, chisel_mesh.GetTransform()) - ref_link_pose = self.reference_link.GetTransform() - t = numpy.dot(ref_link_pose, t) - chisel_mesh.SetTransform(t) + def __init(self, env, mesh_client=None, service_namespace=None, + destination_frame=None, reference_link=None): + """ + @param env The OpenRAVE environment + @param mesh_client An instance of the Chisel sensor system (if None, one is created) + @param service_namespace The namespace to use for Chisel service calls (default: Chisel) + @param destination_frame The tf frame the detected chisel kinbody should be transformed to + (default map) + @param reference_link A link on an OpenRAVE kinbody that corresponds to the tf frame + specified by the destination_frame parameter. If not provided it is assumed that the + transform from the destination_frame tf frame to the OpenRAVE environment is the identity + """ + import rospy + + try: + rospy.init_node('chisel_detector',anonymous=True) + except rospy.exceptions.ROSException: + pass + + if service_namespace is None: + service_namespace = 'Chisel' + + if mesh_client is None: + import openravepy + mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker') + self.mesh_client = mesh_client + self.serv_ns = service_namespace + + # Create a ROS camera used to generate a synthetic point cloud + # to mask known kinbodies from the chisel mesh + from offscreen_render import ros_camera_sim + self.camera = ros_camera_sim.RosCameraSim(env) + self.camera.start('/head/kinect2/qhd') + self.camera_bodies = [] + + self.reset_detection_srv = None + if destination_frame is None: + destination_frame = "/map" + self.destination_frame = destination_frame + self.reference_link = reference_link + + @PerceptionMethod + def DetectObject(self, robot, ignored_bodies=None, **kw_args): + """ + @param robot Required by the PerceptionMethod interface + @param ignored_bodies A list of known kinbodies to be masked out + of the chisel mesh + """ + env = robot.GetEnv() + + if ignored_bodies is None: + ignored_bodies = [] + + # Remove any previous bodies in the camera + for b in self.camera_bodies: + self.camera.remove_body(b) + self.camera_bodies = [] + + # Add any ignored bodies to the camera + for b in ignored_bodies: + if b not in self.camera_bodies: + self.camera.add_body(b) + self.camera_bodies.append(b) + + #Check if chisel kinbody in env + already_in_env = False + for body in env.GetBodies(): + if body.GetName() == 'Chisel/full_mesh': + already_in_env = True + break + + #Remove chisel kinbody if present + if already_in_env: + chisel_kb = env.GetKinBody('Chisel/full_mesh') + env.Remove(chisel_kb) + + #Reset Chisel + if self.reset_detection_srv is None: + import rospy, chisel_msgs.srv + srv_nm = self.serv_ns+'/Reset' + rospy.wait_for_service(srv_nm) + self.reset_detection_srv = rospy.ServiceProxy(srv_nm, + chisel_msgs.srv.ResetService) + self.reset_detection_srv() + + #Get Kinbody and load into env + self.mesh_client.SendCommand('GetMesh Chisel/full_mesh') + chisel_mesh = env.GetKinBody('Chisel/full_mesh') + + if chisel_mesh is not None and self.reference_link is not None: + # Apply a transform to the chisel kinbody to put it in the correct + # location in the OpenRAVE environment + from kinbody_helper import transform_to_or + transform_to_or(detection_frame="/map", + destination_frame=self.destination_frame, + reference_link=self.reference_link, + kinbody=chisel_mesh) - return chisel_mesh + return chisel_mesh diff --git a/src/prpy/perception/kinbody_helper.py b/src/prpy/perception/kinbody_helper.py new file mode 100644 index 0000000..337c9aa --- /dev/null +++ b/src/prpy/perception/kinbody_helper.py @@ -0,0 +1,36 @@ + +def transform_to_or(detection_frame, destination_frame, reference_link, + kinbody): + """ + Transform the pose of a kinbody from a pose determined using TF to + the correct relative pose in OpenRAVE. This transformation is performed + by providing a link in OpenRAVE that corresponds directly to a frame in TF. + + @param detection_frame The tf frame the kinbody was originally detected in + @param destination_frame A tf frame that has a direct correspondence with + a link on an OpenRAVE Kinbody + @param reference_link The OpenRAVE link that corresponds to the tf frame + given by the destination_frame parameter + @param kinbody The kinbody to transform + """ + + import tf, rospy + listener = tf.TransformListener() + + listener.waitForTransform( + detection_frame, destination_frame, + rospy.Time(), + rospy.Duration(10)) + + frame_trans, frame_rot = listener.lookupTransform( + destination_frame, detection_frame, + rospy.Time(0)) + + from tf.transformations import quaternion_matrix + detection_in_destination = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) + detection_in_destination[:3,3] = frame_trans + + body_in_destination = numpy.dot(detection_in_destination, kinbody.GetTransform()) + destination_in_or = reference_link.GetTransform() + body_in_or= numpy.dot(destination_in_or, body_in_destination) + kinbody.SetTransform(t) From 3e3c93f2074a01b0fa7f4570501425814719aae6 Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Tue, 1 Mar 2016 16:34:14 -0500 Subject: [PATCH 07/21] Updating vncc to also use the kinbody_helper. --- src/prpy/perception/chisel.py | 6 +++--- src/prpy/perception/kinbody_helper.py | 23 +++++++++++++++-------- src/prpy/perception/vncc.py | 25 +++++++++++++++++++------ 3 files changed, 37 insertions(+), 17 deletions(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index c53f5f4..f2e9645 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -99,9 +99,9 @@ def DetectObject(self, robot, ignored_bodies=None, **kw_args): # Apply a transform to the chisel kinbody to put it in the correct # location in the OpenRAVE environment from kinbody_helper import transform_to_or - transform_to_or(detection_frame="/map", + transform_to_or(kinbody=chisel_mesh, + detection_frame="/map", destination_frame=self.destination_frame, - reference_link=self.reference_link, - kinbody=chisel_mesh) + reference_link=self.reference_link) return chisel_mesh diff --git a/src/prpy/perception/kinbody_helper.py b/src/prpy/perception/kinbody_helper.py index 337c9aa..59cb65b 100644 --- a/src/prpy/perception/kinbody_helper.py +++ b/src/prpy/perception/kinbody_helper.py @@ -1,20 +1,22 @@ -def transform_to_or(detection_frame, destination_frame, reference_link, - kinbody): +def transform_to_or(kinbody, detection_frame, destination_frame, + reference_link=None): """ Transform the pose of a kinbody from a pose determined using TF to the correct relative pose in OpenRAVE. This transformation is performed by providing a link in OpenRAVE that corresponds directly to a frame in TF. - + + @param kinbody The kinbody to transform @param detection_frame The tf frame the kinbody was originally detected in @param destination_frame A tf frame that has a direct correspondence with a link on an OpenRAVE Kinbody @param reference_link The OpenRAVE link that corresponds to the tf frame - given by the destination_frame parameter - @param kinbody The kinbody to transform + given by the destination_frame parameter (if None it is assumed + that the transform between the OpenRAVE world and the destination_frame + tf frame is the identity) """ - import tf, rospy + import numpy, tf, rospy listener = tf.TransformListener() listener.waitForTransform( @@ -31,6 +33,11 @@ def transform_to_or(detection_frame, destination_frame, reference_link, detection_in_destination[:3,3] = frame_trans body_in_destination = numpy.dot(detection_in_destination, kinbody.GetTransform()) - destination_in_or = reference_link.GetTransform() + + if reference_link is not None: + destination_in_or = reference_link.GetTransform() + else: + destination_in_or = numpy.eye(4) + body_in_or= numpy.dot(destination_in_or, body_in_destination) - kinbody.SetTransform(t) + kinbody.SetTransform(body_in_or) diff --git a/src/prpy/perception/vncc.py b/src/prpy/perception/vncc.py index b28edba..3ec8312 100644 --- a/src/prpy/perception/vncc.py +++ b/src/prpy/perception/vncc.py @@ -12,14 +12,17 @@ class VnccModule(PerceptionModule): - def __init__(self, kinbody_path, detection_frame, world_frame, + def __init__(self, kinbody_path, detection_frame, + destination_frame=None, reference_link=None, service_namespace=None): """ This initializes a VNCC detector. @param kinbody_path The path to the folder where kinbodies are stored @param detection_frame The TF frame of the camera - @param world_frame The desired world TF frame + @param destination_frame The tf frame that the kinbody should be transformed to + @param reference_link The OpenRAVE link that corresponds to the tf frame + given by the destination_frame parameter @param service_namespace The namespace for the VNCC service (default: /vncc) """ @@ -39,7 +42,11 @@ def __init__(self, kinbody_path, detection_frame, world_frame, service_namespace='/vncc' self.detection_frame = detection_frame - self.world_frame = world_frame + if destination_frame is None: + destination_frame='/map' + self.destination_frame = destination_frame + self.reference_link = reference_link + self.service_namespace = service_namespace self.kinbody_path = kinbody_path @@ -107,9 +114,8 @@ def _GetDetection(self, obj_name): #Assumes one instance of object result = self._MsgToPose(detect_resp.detections[0]) - if (self.detection_frame is not None and self.world_frame is not None): - result = self._LocalToWorld(result) result[:3,:3] = numpy.eye(3) + return result @PerceptionMethod @@ -144,5 +150,12 @@ def DetectObject(self, robot, obj_name, **kw_args): os.path.join(self.kinbody_path, kinbody_file)) body = env.GetKinBody(obj_name) - body.SetTransform(obj_pose) + + # Apply a transform to the kinbody to put it in the + # desired location in OpenRAVE + from kinbody_helper import transform_to_or + transform_to_or(kinbody=body, + detection_frame=self.detection_frame, + destination_frame=self.destination_frame, + reference_link=self.reference_link) return body From f38ad0ef6438d7ee1f2bbb75f2fc703a927a8548 Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Thu, 17 Mar 2016 13:05:11 -0400 Subject: [PATCH 08/21] Updating simtrack to use kinbody_helper --- src/prpy/perception/simtrack.py | 61 ++++++++++++++------------------- src/prpy/perception/vncc.py | 24 ------------- 2 files changed, 26 insertions(+), 59 deletions(-) diff --git a/src/prpy/perception/simtrack.py b/src/prpy/perception/simtrack.py index c65058a..abb3eb8 100644 --- a/src/prpy/perception/simtrack.py +++ b/src/prpy/perception/simtrack.py @@ -7,14 +7,16 @@ class SimtrackModule(PerceptionModule): - def __init__(self, kinbody_path, detection_frame, world_frame, + def __init__(self, kinbody_path, detection_frame, + destination_frame=None, reference_link=None, service_namespace=None): """ This initializes a simtrack detector. @param kinbody_path The path to the folder where kinbodies are stored @param detection_frame The TF frame of the camera - @param world_frame The desired world TF frame + @param destination_frame The tf frame that the kinbody should be transformed to + @param reference_link The OpenRAVE link that corresponds to the tf destination_frame @param service_namespace The namespace for the simtrack service (default: /simtrack) """ import rospy @@ -34,10 +36,13 @@ def __init__(self, kinbody_path, detection_frame, world_frame, if service_namespace is None: service_namespace='/simtrack' + self.service_namespace = service_namespace self.detection_frame = detection_frame - self.world_frame = world_frame - self.service_namespace = service_namespace + if destination_frame is None: + destination_frame='/map' + self.destination_frame = destination_frame + self.reference_link = reference_link self.kinbody_path = kinbody_path @@ -65,31 +70,6 @@ def _MsgToPose(msg): return pose - def _LocalToWorld(self,pose): - """ - Transform a pose from local frame to world frame - @param pose The 4x4 transformation matrix containing the pose to transform - @return The 4x4 transformation matrix describing the pose in world frame - """ - import rospy - #Get pose w.r.t world frame - self.listener.waitForTransform(self.world_frame,self.detection_frame, - rospy.Time(),rospy.Duration(10)) - t, r = self.listener.lookupTransform(self.world_frame,self.detection_frame, - rospy.Time(0)) - - #Get relative transform between frames - offset_to_world = numpy.matrix(transformations.quaternion_matrix(r)) - offset_to_world[0,3] = t[0] - offset_to_world[1,3] = t[1] - offset_to_world[2,3] = t[2] - - #Compose with pose to get pose in world frame - result = numpy.array(numpy.dot(offset_to_world, pose)) - - return result - - def _GetDetections(self, obj_names): import simtrack_msgs.srv """ @@ -111,8 +91,6 @@ def _GetDetections(self, obj_names): obj_name = detect_resp.detected_models[i]; obj_pose = detect_resp.detected_poses[i]; obj_pose_tf = self._MsgToPose(obj_pose); - if (self.detection_frame is not None and self.world_frame is not None): - obj_pose_tf = self._LocalToWorld(obj_pose_tf) detections.append((obj_name, obj_pose_tf)); return detections @@ -157,7 +135,15 @@ def DetectObject(self, robot, obj_name, **kw_args): os.path.join(self.kinbody_path, kinbody_file)) body = env.GetKinBody(obj_name) - body.SetTransform(obj_pose) + + # Apply a transform to the kinbody to put it in the + # desired location in OpenRAVE + from kinbody_helper import transform_to_or + transform_to_or(kinbody=body, + detection_frame=self.detection_frame, + destination_frame=self.destination_frame, + reference_link=self.reference_link) + return body @PerceptionMethod @@ -166,6 +152,8 @@ def DetectObjects(self, robot, **kw_args): Overriden method for detection_frame """ from prpy.perception.base import PerceptionException + from kinbody_helper import transform_to_or + env = robot.GetEnv() # Detecting empty list will detect all possible objects detections = self._GetDetections([]) @@ -175,7 +163,6 @@ def DetectObjects(self, robot, **kw_args): continue kinbody_name = self.query_to_kinbody_map[obj_name] - if env.GetKinBody(kinbody_name) is None: from prpy.rave import add_object kinbody_file = '%s.kinbody.xml' % kinbody_name @@ -183,8 +170,12 @@ def DetectObjects(self, robot, **kw_args): env, kinbody_name, os.path.join(self.kinbody_path, kinbody_file)) - print kinbody_name + + body = env.GetKinBody(kinbody_name) - body.SetTransform(obj_pose) + transform_to_or(kinbody=body, + detection_frame=self.detection_frame, + destination_frame=self.destination_frame, + reference_link=self.reference_link) diff --git a/src/prpy/perception/vncc.py b/src/prpy/perception/vncc.py index 3ec8312..a911304 100644 --- a/src/prpy/perception/vncc.py +++ b/src/prpy/perception/vncc.py @@ -72,30 +72,6 @@ def _MsgToPose(msg): return pose - def _LocalToWorld(self,pose): - """ - Transform a pose from local frame to world frame - @param pose The 4x4 transformation matrix containing the pose to transform - @return The 4x4 transformation matrix describing the pose in world frame - """ - #Get pose w.r.t world frame - self.listener.waitForTransform(self.world_frame,self.detection_frame, - rospy.Time(),rospy.Duration(10)) - t, r = self.listener.lookupTransform(self.world_frame,self.detection_frame, - rospy.Time(0)) - - #Get relative transform between frames - offset_to_world = numpy.matrix(transformations.quaternion_matrix(r)) - offset_to_world[0,3] = t[0] - offset_to_world[1,3] = t[1] - offset_to_world[2,3] = t[2] - - #Compose with pose to get pose in world frame - result = numpy.array(numpy.dot(offset_to_world, pose)) - - return result - - def _GetDetection(self, obj_name): """ Calls the service to get a detection of a particular object. From abfd92e7143b86da8b3946f6db859686c3c45819 Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Fri, 18 Mar 2016 16:57:21 -0400 Subject: [PATCH 09/21] Working on integrating simtrack object tracking with prpy --- src/prpy/perception/kinbody_helper.py | 15 +++-- src/prpy/perception/simtrack.py | 96 +++++++++++++++++++++++---- 2 files changed, 94 insertions(+), 17 deletions(-) diff --git a/src/prpy/perception/kinbody_helper.py b/src/prpy/perception/kinbody_helper.py index 59cb65b..7284ece 100644 --- a/src/prpy/perception/kinbody_helper.py +++ b/src/prpy/perception/kinbody_helper.py @@ -1,6 +1,6 @@ def transform_to_or(kinbody, detection_frame, destination_frame, - reference_link=None): + reference_link=None, pose=None): """ Transform the pose of a kinbody from a pose determined using TF to the correct relative pose in OpenRAVE. This transformation is performed @@ -14,11 +14,16 @@ def transform_to_or(kinbody, detection_frame, destination_frame, given by the destination_frame parameter (if None it is assumed that the transform between the OpenRAVE world and the destination_frame tf frame is the identity) + @param pose The pose to transform (if None, kinbody.GetTransform() is used) """ import numpy, tf, rospy listener = tf.TransformListener() + if pose is None: + with kinbody.GetEnv(): + pose = kinbody.GetTransform() + listener.waitForTransform( detection_frame, destination_frame, rospy.Time(), @@ -32,12 +37,14 @@ def transform_to_or(kinbody, detection_frame, destination_frame, detection_in_destination = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) detection_in_destination[:3,3] = frame_trans - body_in_destination = numpy.dot(detection_in_destination, kinbody.GetTransform()) + body_in_destination = numpy.dot(detection_in_destination, pose) if reference_link is not None: - destination_in_or = reference_link.GetTransform() + with kinbody.GetEnv(): + destination_in_or = reference_link.GetTransform() else: destination_in_or = numpy.eye(4) body_in_or= numpy.dot(destination_in_or, body_in_destination) - kinbody.SetTransform(body_in_or) + with kinbody.GetEnv(): + kinbody.SetTransform(body_in_or) diff --git a/src/prpy/perception/simtrack.py b/src/prpy/perception/simtrack.py index abb3eb8..34bf7f1 100644 --- a/src/prpy/perception/simtrack.py +++ b/src/prpy/perception/simtrack.py @@ -27,13 +27,7 @@ def __init__(self, kinbody_path, detection_frame, rospy.init_node('simtrack_detector', anonymous=True) except rospy.exceptions.ROSException: pass - - #For getting transforms in world frame - if detection_frame is not None and world_frame is not None: - self.listener = tf.TransformListener() - else: - self.listener = None - + if service_namespace is None: service_namespace='/simtrack' self.service_namespace = service_namespace @@ -51,6 +45,9 @@ def __init__(self, kinbody_path, detection_frame, 'pop_tarts': 'pop_tarts_visual'} self.query_to_kinbody_map = {'fuze_bottle_visual': 'fuze_bottle', 'pop_tarts_visual': 'pop_tarts'} + + # A dictionary of subscribers to topic - one for each body + self.track_sub = {} @staticmethod def _MsgToPose(msg): @@ -71,13 +68,14 @@ def _MsgToPose(msg): return pose def _GetDetections(self, obj_names): - import simtrack_msgs.srv """ Calls the service to get a detection of a particular object. @param obj_name The name of the object to detect @return A 4x4 transformation matrix describing the pose of the object in world frame, None if the object is not detected """ + import simtrack_msgs.srv, rospy + #Call detection service for a particular object detect_simtrack = rospy.ServiceProxy(self.service_namespace+'/detect_objects', simtrack_msgs.srv.DetectObjects) @@ -111,14 +109,16 @@ def DetectObject(self, robot, obj_name, **kw_args): raise PerceptionException('The simtrack module cannot detect object %s', obj_name) query_name = self.kinbody_to_query_map[obj_name] - obj_poses = self._GetDetections(query_name) - if len(obj_poses is 0): + obj_poses = self._GetDetections([query_name]) + if len(obj_poses) == 0: raise PerceptionException('Failed to detect object %s', obj_name) obj_pose = None + print query_name for (name, pose) in obj_poses: - if (name is query_name): + print name + if name == query_name: obj_pose = pose break; @@ -142,7 +142,8 @@ def DetectObject(self, robot, obj_name, **kw_args): transform_to_or(kinbody=body, detection_frame=self.detection_frame, destination_frame=self.destination_frame, - reference_link=self.reference_link) + reference_link=self.reference_link, + pose=obj_pose) return body @@ -176,6 +177,75 @@ def DetectObjects(self, robot, **kw_args): transform_to_or(kinbody=body, detection_frame=self.detection_frame, destination_frame=self.destination_frame, - reference_link=self.reference_link) + reference_link=self.reference_link, + pose=obj_pose) + + def _UpdatePose(self, msg, kwargs): + """ + Update the pose of an object + """ + pose_tf = self._MsgToPose(msg) + + robot = kwargs['robot'] + obj_name = kwargs['obj_name'] + + env = robot.GetEnv() + with env: + obj = env.GetKinBody(obj_name) + if obj is None: + from prpy.rave import add_object + kinbody_file = '%s.kinbody.xml' % obj_name + new_body = add_object( + env, + obj_name, + os.path.join(self.kinbody_path, kinbody_file)) + + with env: + obj = env.GetKinBody(obj_name) + + from kinbody_helper import transform_to_or + transform_to_or(kinbody=obj, + detection_frame=self.detection_frame, + destination_frame=self.destination_frame, + reference_link=self.reference_link, + pose=pose_tf) + @PerceptionMethod + def StartTrackObject(self, robot, obj_name, **kw_args): + """ + Subscribe to the pose array for an object in order to track + @param robot The OpenRAVE robot + @param obj_name The name of the object to track + """ + import rospy + from geometry_msgs.msg import PoseStamped + from prpy.perception.base import PerceptionException + if obj_name not in self.kinbody_to_query_map: + raise PerceptionException('The simtrack module cannot track object %s' & obj_name) + query_name = self.kinbody_to_query_map[obj_name] + pose_topic = self.service_namespace + '/' + query_name + + if obj_name in self.track_sub and self.track_sub[obj_name] is not None: + raise PerceptionException('The object %s is already being tracked' % obj_name) + + print 'Subscribing to topic: ', pose_topic + self.track_sub[obj_name] = rospy.Subscriber(pose_topic, + PoseStamped, + callback=self._UpdatePose, + callback_args={ + 'robot':robot, + 'obj_name':obj_name}, + queue_size=1 + ) + + @PerceptionMethod + def StopTrackObject(self, robot, obj_name, **kw_args): + """ + Unsubscribe from the pose array to end tracking + @param robot The OpenRAVE robot + @param obj_name The name of the object to stop tracking + """ + if obj_name in self.track_sub and self.track_sub[obj_name] is not None: + self.track_sub[obj_name].unregister() + self.track_sub[obj_name] = None From debef02b9e8f342210c4aa85a0dbb9b150729c1b Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Fri, 18 Mar 2016 17:24:05 -0400 Subject: [PATCH 10/21] Adding ability to bypass tf lookup during tracking --- src/prpy/perception/kinbody_helper.py | 36 ++++++++++++--------- src/prpy/perception/simtrack.py | 46 ++++++++++++++++++++++----- 2 files changed, 59 insertions(+), 23 deletions(-) diff --git a/src/prpy/perception/kinbody_helper.py b/src/prpy/perception/kinbody_helper.py index 7284ece..5b434f3 100644 --- a/src/prpy/perception/kinbody_helper.py +++ b/src/prpy/perception/kinbody_helper.py @@ -1,5 +1,6 @@ -def transform_to_or(kinbody, detection_frame, destination_frame, +def transform_to_or(kinbody, detection_frame=None, destination_frame=None, + detection_in_destination=None, reference_link=None, pose=None): """ Transform the pose of a kinbody from a pose determined using TF to @@ -16,26 +17,31 @@ def transform_to_or(kinbody, detection_frame, destination_frame, tf frame is the identity) @param pose The pose to transform (if None, kinbody.GetTransform() is used) """ - - import numpy, tf, rospy - listener = tf.TransformListener() - + import numpy if pose is None: with kinbody.GetEnv(): pose = kinbody.GetTransform() - listener.waitForTransform( - detection_frame, destination_frame, - rospy.Time(), - rospy.Duration(10)) + if detection_in_destination is None: + import tf, rospy + listener = tf.TransformListener() - frame_trans, frame_rot = listener.lookupTransform( - destination_frame, detection_frame, - rospy.Time(0)) + if pose is None: + with kinbody.GetEnv(): + pose = kinbody.GetTransform() + + listener.waitForTransform( + detection_frame, destination_frame, + rospy.Time(), + rospy.Duration(10)) + + frame_trans, frame_rot = listener.lookupTransform( + destination_frame, detection_frame, + rospy.Time(0)) - from tf.transformations import quaternion_matrix - detection_in_destination = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) - detection_in_destination[:3,3] = frame_trans + from tf.transformations import quaternion_matrix + detection_in_destination = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) + detection_in_destination[:3,3] = frame_trans body_in_destination = numpy.dot(detection_in_destination, pose) diff --git a/src/prpy/perception/simtrack.py b/src/prpy/perception/simtrack.py index 34bf7f1..871bd11 100644 --- a/src/prpy/perception/simtrack.py +++ b/src/prpy/perception/simtrack.py @@ -188,6 +188,7 @@ def _UpdatePose(self, msg, kwargs): robot = kwargs['robot'] obj_name = kwargs['obj_name'] + detection_in_destination = kwargs['detection_in_destination'] env = robot.GetEnv() with env: @@ -204,18 +205,28 @@ def _UpdatePose(self, msg, kwargs): obj = env.GetKinBody(obj_name) from kinbody_helper import transform_to_or - transform_to_or(kinbody=obj, - detection_frame=self.detection_frame, - destination_frame=self.destination_frame, - reference_link=self.reference_link, - pose=pose_tf) + if detection_in_destination is not None: + transform_to_or(kinbody=obj, + detection_in_destination=detection_in_destination, + reference_link=self.reference_link, + pose=pose_tf) + else: + transform_to_or(kinbody=obj, + detection_frame=self.detection_frame, + destination_frame=self.destination_frame, + reference_link=self.reference_link, + pose=pose_tf) @PerceptionMethod - def StartTrackObject(self, robot, obj_name, **kw_args): + def StartTrackObject(self, robot, obj_name, cache_transform=True, **kw_args): """ Subscribe to the pose array for an object in order to track @param robot The OpenRAVE robot @param obj_name The name of the object to track + @param cache_transform If true, lookup the transform from detection_frame + to destination_frame once, cache it and use it for the duration of tracking. + This speeds up tracking by removing the need to perform a tf lookup + in the callback. """ import rospy from geometry_msgs.msg import PoseStamped @@ -229,13 +240,32 @@ def StartTrackObject(self, robot, obj_name, **kw_args): if obj_name in self.track_sub and self.track_sub[obj_name] is not None: raise PerceptionException('The object %s is already being tracked' % obj_name) - print 'Subscribing to topic: ', pose_topic + # These speeds up tracking by removing the need to do a tf lookup + detection_in_destination=None + if cache_transform: + import numpy, tf, rospy + listener = tf.TransformListener() + + listener.waitForTransform( + self.detection_frame, self.destination_frame, + rospy.Time(), + rospy.Duration(10)) + + frame_trans, frame_rot = listener.lookupTransform( + self.destination_frame, self.detection_frame, + rospy.Time(0)) + + from tf.transformations import quaternion_matrix + detection_in_destination = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) + detection_in_destination[:3,3] = frame_trans + self.track_sub[obj_name] = rospy.Subscriber(pose_topic, PoseStamped, callback=self._UpdatePose, callback_args={ 'robot':robot, - 'obj_name':obj_name}, + 'obj_name':obj_name, + 'detection_in_destination': detection_in_destination}, queue_size=1 ) From 86a9259fdb61d0c0c1bfca004052e0512fcbf226 Mon Sep 17 00:00:00 2001 From: Shushman Choudhury Date: Tue, 22 Mar 2016 13:56:06 -0400 Subject: [PATCH 11/21] Some serialization fixes --- src/prpy/rave.py | 9 +++++---- src/prpy/serialization.py | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/prpy/rave.py b/src/prpy/rave.py index f39166b..be81376 100644 --- a/src/prpy/rave.py +++ b/src/prpy/rave.py @@ -109,15 +109,17 @@ def add_object(env, object_name, object_xml_path, object_pose = None): return None + + # render a trajectory in the environment -def render_trajectory(env, manip, traj): +def render_trajectory(env, manip, traj,obj_path): ''' Renders a trajectory in the specified OpenRAVE environment @param env The openrave environment object @param traj The trajectory to render ''' - sphere_obj_path = 'objects/misc/smallsphere.kinbody.xml' + sphere_obj_path = obj_path+'/objects/smallsphere.kinbody.xml' numdofs = len(manip.GetArmIndices()) with env: @@ -132,12 +134,11 @@ def render_trajectory(env, manip, traj): # put the arm in that pose manip.SetActive() - manip.parent.SetActiveDOFValues(arm_config) + manip.GetRobot().SetActiveDOFValues(arm_config) # grab the end effector pose ee_pose = manip.GetEndEffectorTransform() - # render a sphere here add_object(env, 'traj_sphere'+str(wid), sphere_obj_path, ee_pose) # Clear out any rendered trajectories diff --git a/src/prpy/serialization.py b/src/prpy/serialization.py index 139fc82..c837f15 100644 --- a/src/prpy/serialization.py +++ b/src/prpy/serialization.py @@ -558,7 +558,7 @@ def deserialize_transform(data): '_name': str_identity, '_type': ( lambda x: x.name, - lambda x: openravepy.KinBody.JointType.names[x].encode() + lambda x: openravepy.KinBody.JointType.names[x] ), '_vanchor': numpy_identity, '_vaxes': ( From 3f54fe9f69732226a8b507983400a32704c0f903 Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Tue, 22 Mar 2016 17:14:23 -0400 Subject: [PATCH 12/21] fixing bugs in chisel refactor. adding ability to transform kinbodies to tf for offscreen render to use --- src/prpy/perception/chisel.py | 83 +++++++++++++++++---------- src/prpy/perception/kinbody_helper.py | 44 ++++++++++++++ 2 files changed, 98 insertions(+), 29 deletions(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index f2e9645..db58db9 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -6,8 +6,8 @@ class ChiselModule(PerceptionModule): - def __init(self, env, mesh_client=None, service_namespace=None, - destination_frame=None, reference_link=None): + def __init__(self, env, mesh_client=None, service_namespace=None, + detection_frame=None, destination_frame=None, reference_link=None): """ @param env The OpenRAVE environment @param mesh_client An instance of the Chisel sensor system (if None, one is created) @@ -31,8 +31,9 @@ def __init(self, env, mesh_client=None, service_namespace=None, if mesh_client is None: import openravepy mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker') - self.mesh_client = mesh_client - self.serv_ns = service_namespace + + self.mesh_client = mesh_client + self.serv_ns = service_namespace # Create a ROS camera used to generate a synthetic point cloud # to mask known kinbodies from the chisel mesh @@ -42,6 +43,12 @@ def __init(self, env, mesh_client=None, service_namespace=None, self.camera_bodies = [] self.reset_detection_srv = None + + if detection_frame is None: + detection_frame='/map' +# detection_frame='/head/kinect2_rgb_optical_frame' + self.detection_frame = detection_frame + if destination_frame is None: destination_frame = "/map" self.destination_frame = destination_frame @@ -59,17 +66,9 @@ def DetectObject(self, robot, ignored_bodies=None, **kw_args): if ignored_bodies is None: ignored_bodies = [] - # Remove any previous bodies in the camera - for b in self.camera_bodies: - self.camera.remove_body(b) - self.camera_bodies = [] + # Dictionary mapping ignored bodies to start transform + orig_bodies = {} - # Add any ignored bodies to the camera - for b in ignored_bodies: - if b not in self.camera_bodies: - self.camera.add_body(b) - self.camera_bodies.append(b) - #Check if chisel kinbody in env already_in_env = False for body in env.GetBodies(): @@ -81,26 +80,52 @@ def DetectObject(self, robot, ignored_bodies=None, **kw_args): if already_in_env: chisel_kb = env.GetKinBody('Chisel/full_mesh') env.Remove(chisel_kb) - - #Reset Chisel - if self.reset_detection_srv is None: - import rospy, chisel_msgs.srv - srv_nm = self.serv_ns+'/Reset' - rospy.wait_for_service(srv_nm) - self.reset_detection_srv = rospy.ServiceProxy(srv_nm, - chisel_msgs.srv.ResetService) - self.reset_detection_srv() - - #Get Kinbody and load into env - self.mesh_client.SendCommand('GetMesh Chisel/full_mesh') - chisel_mesh = env.GetKinBody('Chisel/full_mesh') - + + try: + from kinbody_helper import transform_from_or + + # Add any ignored bodies to the camera + for b in ignored_bodies: + if b not in self.camera_bodies: + with env: + orig_bodies[b] = b.GetTransform() + transform_from_or(kinbody=b, + detection_frame=self.detection_frame, + destination_frame=self.destination_frame, + reference_link=self.reference_link) + self.camera.add_body(b) + self.camera_bodies.append(b) + #Reset Chisel + if self.reset_detection_srv is None: + import rospy, chisel_msgs.srv + srv_nm = self.serv_ns+'/Reset' + rospy.wait_for_service(srv_nm) + self.reset_detection_srv = rospy.ServiceProxy(srv_nm, + chisel_msgs.srv.ResetService) + self.reset_detection_srv() + + #Get Kinbody and load into env + self.mesh_client.SendCommand('GetMesh Chisel/full_mesh') + chisel_mesh = env.GetKinBody('Chisel/full_mesh') + + finally: + + # Remove any previous bodies in the camera + for b in self.camera_bodies: + self.camera.remove_body(b) + self.camera_bodies = [] + + with env: + # Restore all bodies to their original pose + for b, orig_pose in orig_bodies.iteritems(): + b.SetTransform(orig_pose) + if chisel_mesh is not None and self.reference_link is not None: # Apply a transform to the chisel kinbody to put it in the correct # location in the OpenRAVE environment from kinbody_helper import transform_to_or transform_to_or(kinbody=chisel_mesh, - detection_frame="/map", + detection_frame=self.detection_frame, destination_frame=self.destination_frame, reference_link=self.reference_link) diff --git a/src/prpy/perception/kinbody_helper.py b/src/prpy/perception/kinbody_helper.py index 59cb65b..1487e4e 100644 --- a/src/prpy/perception/kinbody_helper.py +++ b/src/prpy/perception/kinbody_helper.py @@ -41,3 +41,47 @@ def transform_to_or(kinbody, detection_frame, destination_frame, body_in_or= numpy.dot(destination_in_or, body_in_destination) kinbody.SetTransform(body_in_or) + +def transform_from_or(kinbody, detection_frame, destination_frame, + reference_link=None): + """ + Transform the pose of a kinbody from a OpenRAVE pose to the correct + relative pose in TF.This transformation is performed + by providing a link in OpenRAVE that corresponds directly to a frame in TF. + + @param kinbody The kinbody to transform + @param detection_frame The tf frame the kinbody was originally detected in + @param destination_frame A tf frame that has a direct correspondence with + a link on an OpenRAVE Kinbody + @param reference_link The OpenRAVE link that corresponds to the tf frame + given by the destination_frame parameter (if None it is assumed + that the transform between the OpenRAVE world and the destination_frame + tf frame is the identity) + """ + + import numpy, tf, rospy + listener = tf.TransformListener() + + listener.waitForTransform( + detection_frame, destination_frame, + rospy.Time(), + rospy.Duration(10)) + + frame_trans, frame_rot = listener.lookupTransform( + detection_frame, destination_frame, + rospy.Time(0)) + + from tf.transformations import quaternion_matrix + destination_in_detection = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) + destination_in_detection[:3,3] = frame_trans + + with kinbody.GetEnv(): + body_in_or = kinbody.GetTransform() + + or_in_destination = numpy.linalg.inv(reference_link.GetTransform()) + body_in_destination = numpy.dot(or_in_destination, body_in_or) + + body_in_detection = numpy.dot(destination_in_detection, body_in_destination) + + with kinbody.GetEnv(): + kinbody.SetTransform(body_in_detection) From 1b4af44caa58e208dbdf5311fa4098eb2f9f2fde Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Sat, 26 Mar 2016 07:04:45 -0700 Subject: [PATCH 13/21] Adding some comments to clarify logic --- src/prpy/perception/chisel.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index db58db9..1e9315c 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -45,12 +45,13 @@ def __init__(self, env, mesh_client=None, service_namespace=None, self.reset_detection_srv = None if detection_frame is None: - detection_frame='/map' -# detection_frame='/head/kinect2_rgb_optical_frame' + # By default, the Chisel kinbody is put in map frame + # not camera frame + detection_frame='/map' self.detection_frame = detection_frame if destination_frame is None: - destination_frame = "/map" + destination_frame = '/map' self.destination_frame = destination_frame self.reference_link = reference_link @@ -85,6 +86,11 @@ def DetectObject(self, robot, ignored_bodies=None, **kw_args): from kinbody_helper import transform_from_or # Add any ignored bodies to the camera + # We need to transform all these bodies from their pose + # in OpenRAVE to the equivalent pose in TF so that + # chisel can perform the appropriate masking + # Then we will transform them all back after the server + # performs the detection for b in ignored_bodies: if b not in self.camera_bodies: with env: @@ -95,6 +101,7 @@ def DetectObject(self, robot, ignored_bodies=None, **kw_args): reference_link=self.reference_link) self.camera.add_body(b) self.camera_bodies.append(b) + #Reset Chisel if self.reset_detection_srv is None: import rospy, chisel_msgs.srv From 076191fcdc3acfd9f8dfa6664333b1f81142e353 Mon Sep 17 00:00:00 2001 From: Shushman Choudhury Date: Fri, 22 Apr 2016 17:01:44 -0400 Subject: [PATCH 14/21] Around 70% of Mike's original comments --- package.xml | 3 + src/prpy/perception/chisel.py | 133 ++++++++++++-------------- src/prpy/perception/kinbody_helper.py | 5 +- 3 files changed, 67 insertions(+), 74 deletions(-) diff --git a/package.xml b/package.xml index 90d6065..ae8986c 100644 --- a/package.xml +++ b/package.xml @@ -26,12 +26,15 @@ manipulation2 openrave openrave_catkin + or_mesh_marker + offscreen_render python-scipy python-termcolor python-trollius python-rospkg python-yaml python-lxml + tf python-nose cbirrt2 diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index 1e9315c..29412b6 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -1,14 +1,20 @@ import logging +import openravepy from base import PerceptionModule, PerceptionMethod logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) + class ChiselModule(PerceptionModule): - def __init__(self, env, mesh_client=None, service_namespace=None, - detection_frame=None, destination_frame=None, reference_link=None): + + def __init__(self, env, service_namespace='Chisel', mesh_name = 'Chisel/full_mesh', + detection_frame='/map', destination_frame='/map', reference_link=None): """ + The perception module implementation for CHISEL - which converts all unmodelled clutter from the point + cloud to an OpenRAVE Kinbody - after masking out desired known objects + @param env The OpenRAVE environment @param mesh_client An instance of the Chisel sensor system (if None, one is created) @param service_namespace The namespace to use for Chisel service calls (default: Chisel) @@ -20,20 +26,13 @@ def __init__(self, env, mesh_client=None, service_namespace=None, """ import rospy - try: - rospy.init_node('chisel_detector',anonymous=True) - except rospy.exceptions.ROSException: - pass - - if service_namespace is None: - service_namespace = 'Chisel' + self.mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker') - if mesh_client is None: - import openravepy - mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker') + if self.mesh_client is None: + raise RuntimeError('Could not create mesh client') - self.mesh_client = mesh_client self.serv_ns = service_namespace + self.mesh_name = mesh_name # Create a ROS camera used to generate a synthetic point cloud # to mask known kinbodies from the chisel mesh @@ -43,21 +42,16 @@ def __init__(self, env, mesh_client=None, service_namespace=None, self.camera_bodies = [] self.reset_detection_srv = None - - if detection_frame is None: - # By default, the Chisel kinbody is put in map frame - # not camera frame - detection_frame='/map' self.detection_frame = detection_frame - - if destination_frame is None: - destination_frame = '/map' self.destination_frame = destination_frame self.reference_link = reference_link @PerceptionMethod def DetectObject(self, robot, ignored_bodies=None, **kw_args): """ + Obtains the KinBody corresponding to the Chisel Mesh after masking out + any existing kinbodies if desired + @param robot Required by the PerceptionMethod interface @param ignored_bodies A list of known kinbodies to be masked out of the chisel mesh @@ -71,58 +65,51 @@ def DetectObject(self, robot, ignored_bodies=None, **kw_args): orig_bodies = {} #Check if chisel kinbody in env - already_in_env = False - for body in env.GetBodies(): - if body.GetName() == 'Chisel/full_mesh': - already_in_env = True - break - - #Remove chisel kinbody if present - if already_in_env: - chisel_kb = env.GetKinBody('Chisel/full_mesh') - env.Remove(chisel_kb) - - try: - from kinbody_helper import transform_from_or - - # Add any ignored bodies to the camera - # We need to transform all these bodies from their pose - # in OpenRAVE to the equivalent pose in TF so that - # chisel can perform the appropriate masking - # Then we will transform them all back after the server - # performs the detection - for b in ignored_bodies: - if b not in self.camera_bodies: - with env: - orig_bodies[b] = b.GetTransform() - transform_from_or(kinbody=b, - detection_frame=self.detection_frame, - destination_frame=self.destination_frame, - reference_link=self.reference_link) - self.camera.add_body(b) - self.camera_bodies.append(b) - - #Reset Chisel - if self.reset_detection_srv is None: - import rospy, chisel_msgs.srv - srv_nm = self.serv_ns+'/Reset' - rospy.wait_for_service(srv_nm) - self.reset_detection_srv = rospy.ServiceProxy(srv_nm, - chisel_msgs.srv.ResetService) - self.reset_detection_srv() - - #Get Kinbody and load into env - self.mesh_client.SendCommand('GetMesh Chisel/full_mesh') - chisel_mesh = env.GetKinBody('Chisel/full_mesh') - - finally: - - # Remove any previous bodies in the camera - for b in self.camera_bodies: - self.camera.remove_body(b) - self.camera_bodies = [] - - with env: + chisel_kb = env.GetKinBody(self.mesh_name) + if chisel_kb is not None: + env.RemoveKinBody(chisel_kb) + + with env: + try: + from kinbody_helper import transform_from_or + + # Add any ignored bodies to the camera + # We need to transform all these bodies from their pose + # in OpenRAVE to the equivalent pose in TF so that + # chisel can perform the appropriate masking + # Then we will transform them all back after the server + # performs the detection + for b in ignored_bodies: + if b not in self.camera_bodies: + with env: + orig_bodies[b] = b.GetTransform() + transform_from_or(kinbody=b, + detection_frame=self.detection_frame, + destination_frame=self.destination_frame, + reference_link=self.reference_link) + self.camera.add_body(b) + self.camera_bodies.append(b) + + #Reset Chisel + if self.reset_detection_srv is None: + import rospy, chisel_msgs.srv + srv_nm = self.serv_ns+'/Reset' + rospy.wait_for_service(srv_nm) + self.reset_detection_srv = rospy.ServiceProxy(srv_nm, + chisel_msgs.srv.ResetService) + self.reset_detection_srv() + + #Get Kinbody and load into env + self.mesh_client.SendCommand('GetMesh '+self.mesh_name) + chisel_mesh = env.GetKinBody(self.mesh_name) + + finally: + + # Remove any previous bodies in the camera + for b in self.camera_bodies: + self.camera.remove_body(b) + self.camera_bodies = [] + # Restore all bodies to their original pose for b, orig_pose in orig_bodies.iteritems(): b.SetTransform(orig_pose) diff --git a/src/prpy/perception/kinbody_helper.py b/src/prpy/perception/kinbody_helper.py index 1487e4e..33a2312 100644 --- a/src/prpy/perception/kinbody_helper.py +++ b/src/prpy/perception/kinbody_helper.py @@ -78,7 +78,10 @@ def transform_from_or(kinbody, detection_frame, destination_frame, with kinbody.GetEnv(): body_in_or = kinbody.GetTransform() - or_in_destination = numpy.linalg.inv(reference_link.GetTransform()) + if reference_link is not None: + or_in_destination = numpy.linalg.inv(reference_link.GetTransform()) + else: + or_in_destination = numpy.eye(4) body_in_destination = numpy.dot(or_in_destination, body_in_or) body_in_detection = numpy.dot(destination_in_detection, body_in_destination) From d5afd97be8ef22e0657cb5d2d3deb3ac1398c1ce Mon Sep 17 00:00:00 2001 From: Shushman Choudhury Date: Wed, 27 Apr 2016 14:11:09 -0400 Subject: [PATCH 15/21] Basic tracking interface --- src/prpy/perception/chisel.py | 6 +- src/prpy/perception/tracking/base.py | 42 ++++++ .../tracking/point_cloud_tracker.py | 51 +++++++ .../perception/tracking/sim_track_tracker.py | 124 ++++++++++++++++++ 4 files changed, 220 insertions(+), 3 deletions(-) create mode 100644 src/prpy/perception/tracking/base.py create mode 100644 src/prpy/perception/tracking/point_cloud_tracker.py create mode 100644 src/prpy/perception/tracking/sim_track_tracker.py diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index f2e9645..00b1814 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -6,7 +6,7 @@ class ChiselModule(PerceptionModule): - def __init(self, env, mesh_client=None, service_namespace=None, + def __init__(self, env, mesh_client=None, service_namespace=None, destination_frame=None, reference_link=None): """ @param env The OpenRAVE environment @@ -31,8 +31,8 @@ def __init(self, env, mesh_client=None, service_namespace=None, if mesh_client is None: import openravepy mesh_client = openravepy.RaveCreateSensorSystem(env, 'mesh_marker') - self.mesh_client = mesh_client - self.serv_ns = service_namespace + self.mesh_client = mesh_client + self.serv_ns = service_namespace # Create a ROS camera used to generate a synthetic point cloud # to mask known kinbodies from the chisel mesh diff --git a/src/prpy/perception/tracking/base.py b/src/prpy/perception/tracking/base.py new file mode 100644 index 0000000..ae3a29f --- /dev/null +++ b/src/prpy/perception/tracking/base.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python + +import functools + +class TrackingException(Exception): + pass + +class TrackingMethod(object): + + def __init__(self, func): + self.func = func + + def __call__(self,instance,robot,*args,**kw_args): + return self.func(instance, robot, *args, **kw_args) + + def __get__(self, instance, instancetype): + wrapper = functools.partial(self.__call__, instance) + functools.update_wrapper(wrapper, self.func) + wrapper.is_tracking_method = True + return wrapper + +class TrackingModule(object): + def has_tracking_method(self, method_name): + """ + Check if this module has the desired TrackingMethod + """ + if hasattr(self, method_name): + method = getattr(self, method_name) + if hasattr(method, 'is_tracking_method'): + return method.is_tracking_method + else: + return False + else: + return False + + def get_tracking_method_names(self): + """ + @return A list of all the TrackingMethod functions + defined for this module + """ + return filter(lambda method_name: self.has_tracking_method(method_name), dir(self)) + diff --git a/src/prpy/perception/tracking/point_cloud_tracker.py b/src/prpy/perception/tracking/point_cloud_tracker.py new file mode 100644 index 0000000..5aaf106 --- /dev/null +++ b/src/prpy/perception/tracking/point_cloud_tracker.py @@ -0,0 +1,51 @@ +from base import TrackingModule, TrackingMethod +import openravepy + +class PointCloudTracker(TrackingModule): + + def __init__(self,env,depth_info='/head/kinect2/qhd/camera_info',pcl_topic='/head/kinect2/qhd/points'): + """ + Initializes an OpenRAVE plugin that tracks the point-cloud + corresponding to some provided KinBody + @param env The OpenRAVE robot + @depth_info The camera information of depth topic + @pcl_topic + """ + + self.env = env + self.depth_info = depth_info + self.pcl_topic = pcl_topic + self.tracked_objects = list() + + #Create tracker module + self.tracker = openravepy.RaveCreateModule(env, "object_tracker") + + #Initialize tracker + tracker_init_message = + self.tracker.SendCommand('Initialize '+self.depth_info+' '+self.pcl_topic) + + @TrackingMethod + def StartTrackObject(self,obj_name,n_iters=300): + """ + Begin tracking the OpenRAVE object - blocks and updated kinbody pose + @param obj_name The name of the kinbody to track + @n_iters The number of tracking iterations it is blocked for + """ + from prpy.perception.base import PerceptionException + if obj_name in self.tracked_objects: + raise TrackingException('The object %s is already being tracked' % obj_name) + + self.tracked_objects.append(obj_name) + self.tracker.SendCommand("Track "+obj_name+' '+`n_iters`) + + @TrackingMethod + def StopTrackObject(self,obj_name): + """ + Stop tracking given object. Currently nothing because PCL tracker stops after iterations + @param obj_name The name of the object to stop tracking + """ + + if obj_name not in self.tracked_objects: + print '{0} is not being tracked!'.format(obj_name) + else: + self.tracked_objects.remove(obj_name) \ No newline at end of file diff --git a/src/prpy/perception/tracking/sim_track_tracker.py b/src/prpy/perception/tracking/sim_track_tracker.py new file mode 100644 index 0000000..4d21a9c --- /dev/null +++ b/src/prpy/perception/tracking/sim_track_tracker.py @@ -0,0 +1,124 @@ +from base import TrackingModule, TrackingMethod + +class SimTracker(TrackingModule): + + def __init__(self,env,detection_frame, destination_frame = '/map', + service_namespace='/simtrack'): + + self.env = env + self.service_namespace = service_namespace + + # A map of known objects that can be queries + self.kinbody_to_query_map = {'fuze_bottle': 'fuze_bottle_visual', + 'pop_tarts': 'pop_tarts_visual'} + self.query_to_kinbody_map = {'fuze_bottle_visual': 'fuze_bottle', + 'pop_tarts_visual': 'pop_tarts'} + + # A dictionary of subscribers to topic - one for each body + self.track_sub = {} + + self.detection_frame = detection_frame + self.destination_frame = destination_frame + + + @TrackingMethod + def StartTrackObject(self, robot, obj_name, cache_transform=True): + """ + Subscribe to the pose array for an object in order to track + @param robot The OpenRAVE robot + @param obj_name The name of the object to track + @param cache_transform If true, lookup the transform from detection_frame + to destination_frame once, cache it and use it for the duration of tracking. + This speeds up tracking by removing the need to perform a tf lookup + in the callback. + """ + import rospy + from geometry_msgs.msg import PoseStamped + from prpy.perception.base import PerceptionException + + if obj_name not in self.kinbody_to_query_map: + raise TrackingException('The simtrack tracker cannot track object %s' & obj_name) + + query_name = self.kinbody_to_query_map[obj_name] + pose_topic = self.service_namespace + '/' + query_name + + if obj_name in self.track_sub and self.track_sub[obj_name] is not None: + raise TrackingException('The object %s is already being tracked' % obj_name) + + + detection_in_destination=None + if cache_transform: + import numpy, tf, rospy + listener = tf.TransformListener() + + listener.waitForTransform( + self.detection_frame, self.destination_frame, + rospy.Time(), + rospy.Duration(10)) + + frame_trans, frame_rot = listener.lookupTransform( + self.destination_frame, self.detection_frame, + rospy.Time(0)) + + from tf.transformations import quaternion_matrix + detection_in_destination = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) + detection_in_destination[:3,3] = frame_trans + + self.track_sub[obj_name] = rospy.Subscriber(pose_topic, + PoseStamped, + callback=self._UpdatePose, + callback_args={ + 'robot':robot, + 'obj_name':obj_name, + 'detection_in_destination': detection_in_destination}, + queue_size=1 + ) + + + @TrackingMethod + def StopTrackObject(self, robot, obj_name, **kw_args): + """ + Unsubscribe from the pose array to end tracking + @param robot The OpenRAVE robot + @param obj_name The name of the object to stop tracking + """ + if obj_name in self.track_sub and self.track_sub[obj_name] is not None: + self.track_sub[obj_name].unregister() + self.track_sub[obj_name] = None + + def _UpdatePose(self, msg, kwargs): + """ + Update the pose of an object + """ + pose_tf = self._MsgToPose(msg) + + robot = kwargs['robot'] + obj_name = kwargs['obj_name'] + detection_in_destination = kwargs['detection_in_destination'] + + env = robot.GetEnv() + with env: + obj = env.GetKinBody(obj_name) + if obj is None: + from prpy.rave import add_object + kinbody_file = '%s.kinbody.xml' % obj_name + new_body = add_object( + env, + obj_name, + os.path.join(self.kinbody_path, kinbody_file)) + + with env: + obj = env.GetKinBody(obj_name) + + from kinbody_helper import transform_to_or + if detection_in_destination is not None: + transform_to_or(kinbody=obj, + detection_in_destination=detection_in_destination, + reference_link=self.reference_link, + pose=pose_tf) + else: + transform_to_or(kinbody=obj, + detection_frame=self.detection_frame, + destination_frame=self.destination_frame, + reference_link=self.reference_link, + pose=pose_tf) \ No newline at end of file From c62e2d31d874b25c234881659514f1618c9ce2ac Mon Sep 17 00:00:00 2001 From: Shushman Choudhury Date: Fri, 6 May 2016 17:16:05 -0400 Subject: [PATCH 16/21] Addressed most of Mike's other comments --- package.xml | 1 + src/prpy/perception/chisel.py | 30 +++++++++---------- src/prpy/perception/kinbody_helper.py | 42 ++++++++++++++------------- 3 files changed, 38 insertions(+), 35 deletions(-) diff --git a/package.xml b/package.xml index ae8986c..c420548 100644 --- a/package.xml +++ b/package.xml @@ -27,6 +27,7 @@ openrave openrave_catkin or_mesh_marker + chisel_msgs offscreen_render python-scipy python-termcolor diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index 29412b6..9516b85 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -12,14 +12,14 @@ class ChiselModule(PerceptionModule): def __init__(self, env, service_namespace='Chisel', mesh_name = 'Chisel/full_mesh', detection_frame='/map', destination_frame='/map', reference_link=None): """ - The perception module implementation for CHISEL - which converts all unmodelled clutter from the point - cloud to an OpenRAVE Kinbody - after masking out desired known objects + The perception module implementation for CHISEL. It converts all unmodelled clutter from the point + cloud to an OpenRAVE Kinbody - after masking out specified known objects already in the Environment. @param env The OpenRAVE environment - @param mesh_client An instance of the Chisel sensor system (if None, one is created) @param service_namespace The namespace to use for Chisel service calls (default: Chisel) - @param destination_frame The tf frame the detected chisel kinbody should be transformed to - (default map) + @param mesh_name The name of the topic on which the Chisel marker is published. + @param detection_frame The frame in which the Chisel mesh is detected. (default: map) + @param destination_frame The tf frame the detected chisel kinbody should be transformed to (default: map) @param reference_link A link on an OpenRAVE kinbody that corresponds to the tf frame specified by the destination_frame parameter. If not provided it is assumed that the transform from the destination_frame tf frame to the OpenRAVE environment is the identity @@ -47,13 +47,14 @@ def __init__(self, env, service_namespace='Chisel', mesh_name = 'Chisel/full_mes self.reference_link = reference_link @PerceptionMethod - def DetectObject(self, robot, ignored_bodies=None, **kw_args): + def DetectObject(self, robot, ignored_bodies=None, timeout=5, **kw_args): """ - Obtains the KinBody corresponding to the Chisel Mesh after masking out - any existing kinbodies if desired + Obtain the KinBody corresponding to the Chisel Mesh after masking out + any existing kinbodies if desired (as specified in ignored_bodies). @param robot Required by the PerceptionMethod interface @param ignored_bodies A list of known kinbodies to be masked out + @param timeout The timeout for which to wait for the Chisel Mesh of the chisel mesh """ env = robot.GetEnv() @@ -91,13 +92,12 @@ def DetectObject(self, robot, ignored_bodies=None, **kw_args): self.camera_bodies.append(b) #Reset Chisel - if self.reset_detection_srv is None: - import rospy, chisel_msgs.srv - srv_nm = self.serv_ns+'/Reset' - rospy.wait_for_service(srv_nm) - self.reset_detection_srv = rospy.ServiceProxy(srv_nm, - chisel_msgs.srv.ResetService) - self.reset_detection_srv() + import rospy, chisel_msgs.srv + srv_nm = self.serv_ns+'/Reset' + rospy.wait_for_service(srv_nm,timeout) + reset_detection_srv = rospy.ServiceProxy(srv_nm, + chisel_msgs.srv.ResetService) + reset_detection_srv() #Get Kinbody and load into env self.mesh_client.SendCommand('GetMesh '+self.mesh_name) diff --git a/src/prpy/perception/kinbody_helper.py b/src/prpy/perception/kinbody_helper.py index 33a2312..3af147f 100644 --- a/src/prpy/perception/kinbody_helper.py +++ b/src/prpy/perception/kinbody_helper.py @@ -16,7 +16,9 @@ def transform_to_or(kinbody, detection_frame, destination_frame, tf frame is the identity) """ - import numpy, tf, rospy + import numpy + import tf + import rospy listener = tf.TransformListener() listener.waitForTransform( @@ -32,15 +34,16 @@ def transform_to_or(kinbody, detection_frame, destination_frame, detection_in_destination = numpy.array(numpy.matrix(quaternion_matrix(frame_rot))) detection_in_destination[:3,3] = frame_trans - body_in_destination = numpy.dot(detection_in_destination, kinbody.GetTransform()) + with env: + body_in_destination = numpy.dot(detection_in_destination, kinbody.GetTransform()) - if reference_link is not None: - destination_in_or = reference_link.GetTransform() - else: - destination_in_or = numpy.eye(4) + if reference_link is not None: + destination_in_or = reference_link.GetTransform() + else: + destination_in_or = numpy.eye(4) - body_in_or= numpy.dot(destination_in_or, body_in_destination) - kinbody.SetTransform(body_in_or) + body_in_or= numpy.dot(destination_in_or, body_in_destination) + kinbody.SetTransform(body_in_or) def transform_from_or(kinbody, detection_frame, destination_frame, reference_link=None): @@ -59,7 +62,9 @@ def transform_from_or(kinbody, detection_frame, destination_frame, tf frame is the identity) """ - import numpy, tf, rospy + import numpy + import tf + import rospy listener = tf.TransformListener() listener.waitForTransform( @@ -77,14 +82,11 @@ def transform_from_or(kinbody, detection_frame, destination_frame, with kinbody.GetEnv(): body_in_or = kinbody.GetTransform() - - if reference_link is not None: - or_in_destination = numpy.linalg.inv(reference_link.GetTransform()) - else: - or_in_destination = numpy.eye(4) - body_in_destination = numpy.dot(or_in_destination, body_in_or) - - body_in_detection = numpy.dot(destination_in_detection, body_in_destination) - - with kinbody.GetEnv(): - kinbody.SetTransform(body_in_detection) + if reference_link is not None: + or_in_destination = numpy.linalg.inv(reference_link.GetTransform()) + else: + or_in_destination = numpy.eye(4) + body_in_destination = numpy.dot(or_in_destination, body_in_or) + + body_in_detection = numpy.dot(destination_in_detection, body_in_destination) + kinbody.SetTransform(body_in_detection) \ No newline at end of file From 91935199c57eba6f81f9ffb8398f00b10dee8ede Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Thu, 2 Jun 2016 11:43:22 -0400 Subject: [PATCH 17/21] Small changes to get simtrack working correctly and ignoring failed poses --- src/prpy/perception/kinbody_helper.py | 6 +----- src/prpy/perception/simtrack.py | 6 +++++- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/prpy/perception/kinbody_helper.py b/src/prpy/perception/kinbody_helper.py index 3b15613..f6cb775 100644 --- a/src/prpy/perception/kinbody_helper.py +++ b/src/prpy/perception/kinbody_helper.py @@ -26,10 +26,6 @@ def transform_to_or(kinbody, detection_frame=None, destination_frame=None, import tf, rospy listener = tf.TransformListener() - if pose is None: - with kinbody.GetEnv(): - pose = kinbody.GetTransform() - listener.waitForTransform( detection_frame, destination_frame, rospy.Time(), @@ -44,7 +40,7 @@ def transform_to_or(kinbody, detection_frame=None, destination_frame=None, detection_in_destination[:3,3] = frame_trans with kinbody.GetEnv(): - body_in_destination = numpy.dot(detection_in_destination, kinbody.GetTransform()) + body_in_destination = numpy.dot(detection_in_destination, pose) if reference_link is not None: destination_in_or = reference_link.GetTransform() diff --git a/src/prpy/perception/simtrack.py b/src/prpy/perception/simtrack.py index 871bd11..58e3205 100644 --- a/src/prpy/perception/simtrack.py +++ b/src/prpy/perception/simtrack.py @@ -117,13 +117,16 @@ def DetectObject(self, robot, obj_name, **kw_args): print query_name for (name, pose) in obj_poses: - print name + print name if name == query_name: obj_pose = pose break; if (obj_pose is None): raise PerceptionException('Failed to detect object %s', obj_name) + + if numpy.array_equal(obj_pose, numpy.eye(4)): + raise PerceptionException('Object %s detected but failed to compute pose', obj_name) env = robot.GetEnv() if env.GetKinBody(obj_name) is None: @@ -134,6 +137,7 @@ def DetectObject(self, robot, obj_name, **kw_args): obj_name, os.path.join(self.kinbody_path, kinbody_file)) + print 'Object pose: ', obj_pose body = env.GetKinBody(obj_name) # Apply a transform to the kinbody to put it in the From 68449f21024a453b12c33758fa58cc6ff9761add Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Thu, 2 Jun 2016 12:21:56 -0400 Subject: [PATCH 18/21] Fixing merge error in kinbody_detectory. Changing default chisel detection frame --- src/prpy/perception/chisel.py | 2 +- src/prpy/perception/kinbody_helper.py | 12 ------------ 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index cac87bb..98ce5b1 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -9,7 +9,7 @@ class ChiselModule(PerceptionModule): def __init__(self, env, service_namespace='Chisel', mesh_name = 'Chisel/full_mesh', - detection_frame='/map', destination_frame='/map', reference_link=None): + detection_frame='/herb_base', destination_frame='/map', reference_link=None): """ The perception module implementation for CHISEL. It converts all unmodelled clutter from the point cloud to an OpenRAVE Kinbody - after masking out specified known objects already in the Environment. diff --git a/src/prpy/perception/kinbody_helper.py b/src/prpy/perception/kinbody_helper.py index f6cb775..61fe238 100644 --- a/src/prpy/perception/kinbody_helper.py +++ b/src/prpy/perception/kinbody_helper.py @@ -95,15 +95,3 @@ def transform_from_or(kinbody, detection_frame, destination_frame, body_in_detection = numpy.dot(destination_in_detection, body_in_destination) kinbody.SetTransform(body_in_detection) - - body_in_destination = numpy.dot(detection_in_destination, pose) - - if reference_link is not None: - with kinbody.GetEnv(): - destination_in_or = reference_link.GetTransform() - else: - destination_in_or = numpy.eye(4) - - body_in_or= numpy.dot(destination_in_or, body_in_destination) - with kinbody.GetEnv(): - kinbody.SetTransform(body_in_or) From 5aec63bc2083ded308debf7c01fd09893b472578 Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Thu, 2 Jun 2016 13:05:56 -0400 Subject: [PATCH 19/21] Modifying transform logic to put objects in map frame for offscreen_render --- src/prpy/perception/chisel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index 98ce5b1..443cf9a 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -84,7 +84,7 @@ def DetectObject(self, robot, ignored_bodies=None, timeout=5, **kw_args): with env: orig_bodies[b] = b.GetTransform() transform_from_or(kinbody=b, - detection_frame=self.detection_frame, + detection_frame='/map', destination_frame=self.destination_frame, reference_link=self.reference_link) self.camera.add_body(b) From bbbf4f5ff77a7bb867ecf8e4b954a041e53c4fcf Mon Sep 17 00:00:00 2001 From: Jennifer King Date: Mon, 13 Jun 2016 16:03:28 -0400 Subject: [PATCH 20/21] adding logic to add clones of the original body to the camera in order to account for detection noise during masking --- src/prpy/perception/chisel.py | 59 ++++++++++++++++++--------- src/prpy/perception/kinbody_helper.py | 8 +++- 2 files changed, 46 insertions(+), 21 deletions(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index 443cf9a..8ca12ce 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -61,9 +61,6 @@ def DetectObject(self, robot, ignored_bodies=None, timeout=5, **kw_args): if ignored_bodies is None: ignored_bodies = [] - # Dictionary mapping ignored bodies to start transform - orig_bodies = {} - #Check if chisel kinbody in env chisel_kb = env.GetKinBody(self.mesh_name) if chisel_kb is not None: @@ -72,7 +69,7 @@ def DetectObject(self, robot, ignored_bodies=None, timeout=5, **kw_args): with env: try: from kinbody_helper import transform_from_or - + import openravepy # Add any ignored bodies to the camera # We need to transform all these bodies from their pose # in OpenRAVE to the equivalent pose in TF so that @@ -81,14 +78,22 @@ def DetectObject(self, robot, ignored_bodies=None, timeout=5, **kw_args): # performs the detection for b in ignored_bodies: if b not in self.camera_bodies: - with env: - orig_bodies[b] = b.GetTransform() - transform_from_or(kinbody=b, - detection_frame='/map', - destination_frame=self.destination_frame, - reference_link=self.reference_link) - self.camera.add_body(b) - self.camera_bodies.append(b) + poses = self.SamplePoses(b.GetTransform()) + orig_name = b.GetName() + + for idx, pose in enumerate(poses): + bcloned = openravepy.RaveCreateKinBody(env, b.GetXMLId()) + bcloned.Clone(b, 0) + + bcloned.SetName('%s_%d' % (orig_name, idx)) + env.Add(bcloned) + transform_from_or(kinbody=bcloned, + pose=pose, + detection_frame='/map', + destination_frame=self.destination_frame, + reference_link=self.reference_link) + self.camera.add_body(bcloned) + self.camera_bodies.append(bcloned) #Reset Chisel import rospy, chisel_msgs.srv @@ -103,16 +108,12 @@ def DetectObject(self, robot, ignored_bodies=None, timeout=5, **kw_args): chisel_mesh = env.GetKinBody(self.mesh_name) finally: - + # Remove any previous bodies in the camera for b in self.camera_bodies: - self.camera.remove_body(b) - self.camera_bodies = [] + self.camera.remove_body_now(b) + env.Remove(b) - # Restore all bodies to their original pose - for b, orig_pose in orig_bodies.iteritems(): - b.SetTransform(orig_pose) - if chisel_mesh is not None and self.reference_link is not None: # Apply a transform to the chisel kinbody to put it in the correct # location in the OpenRAVE environment @@ -123,3 +124,23 @@ def DetectObject(self, robot, ignored_bodies=None, timeout=5, **kw_args): reference_link=self.reference_link) return chisel_mesh + + def SamplePoses(self, orig_transform, radius=0.03): + """ + Sample poses around the sphere of the given radius + Sampled poses maintain orientation but have different translations + @param orig_transform The original pose + @param radius The radius of the sphere to sample from + """ + import copy, numpy + retvals = [] + for i in [-1., 0., 1.]: + for j in [-1., 0., 1.]: + for k in [-1., 0., 1.]: + v = numpy.array([i, j, k]) + if numpy.linalg.norm(v) > 0: + v = radius * v / numpy.linalg.norm(v) + new_transform = copy.deepcopy(orig_transform) + new_transform[:3,3] = orig_transform[:3,3] + v + retvals.append(new_transform) + return retvals diff --git a/src/prpy/perception/kinbody_helper.py b/src/prpy/perception/kinbody_helper.py index 61fe238..36731c6 100644 --- a/src/prpy/perception/kinbody_helper.py +++ b/src/prpy/perception/kinbody_helper.py @@ -51,7 +51,7 @@ def transform_to_or(kinbody, detection_frame=None, destination_frame=None, kinbody.SetTransform(body_in_or) def transform_from_or(kinbody, detection_frame, destination_frame, - reference_link=None): + reference_link=None, pose=None): """ Transform the pose of a kinbody from a OpenRAVE pose to the correct relative pose in TF.This transformation is performed @@ -72,6 +72,10 @@ def transform_from_or(kinbody, detection_frame, destination_frame, import rospy listener = tf.TransformListener() + if pose is None: + with kinbody.GetEnv(): + pose = kinbody.GetTransform() + listener.waitForTransform( detection_frame, destination_frame, rospy.Time(), @@ -86,7 +90,7 @@ def transform_from_or(kinbody, detection_frame, destination_frame, destination_in_detection[:3,3] = frame_trans with kinbody.GetEnv(): - body_in_or = kinbody.GetTransform() + body_in_or = pose if reference_link is not None: or_in_destination = numpy.linalg.inv(reference_link.GetTransform()) else: From cac5a9629c9f4b0147c442526b0f4278965a6ab0 Mon Sep 17 00:00:00 2001 From: Shushman Choudhury Date: Tue, 14 Jun 2016 16:06:53 -0400 Subject: [PATCH 21/21] Using 0.05 for pose offset --- src/prpy/perception/chisel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/prpy/perception/chisel.py b/src/prpy/perception/chisel.py index 8ca12ce..09070ba 100644 --- a/src/prpy/perception/chisel.py +++ b/src/prpy/perception/chisel.py @@ -125,7 +125,7 @@ def DetectObject(self, robot, ignored_bodies=None, timeout=5, **kw_args): return chisel_mesh - def SamplePoses(self, orig_transform, radius=0.03): + def SamplePoses(self, orig_transform, radius=0.05): """ Sample poses around the sphere of the given radius Sampled poses maintain orientation but have different translations