Stable

config/object_finder.yaml

@@ -16,9 +16,9 @@ thresh2: 0
 thresh_max: 2000
 
 # Color detection
-blur: 12
+blur: 20
 blur_max: 50
-radius: 22
+radius: 72
 radius_max: 128
 open: 4
 open_max: 15

launch/object_finder_test.launch

@@ -3,6 +3,7 @@
   <arg name="method"   default="color"/>
   <arg name="topic"   default="object_finder_test"/>
   <arg name="folder"   default="$(find baxter_demos)/tests/assets/block_imgs/"/>
+  <param name="/object_tracker/config_folder"   value="$(find baxter_demos)/config/"/>
 
   <node pkg="baxter_demos" name="object_finder" type="object_finder.py"
   args="--limb $(arg limb) --method $(arg method) --topic $(arg topic)" required="true">

launch/object_tracker.launch

@@ -2,25 +2,44 @@
 
   <arg name="limb"     default="right"/>
   <arg name="method"   default="color"/>
-  <arg name="folder"   default="$(find baxter_demos)/assets/"/>
 
-  <param name="robot_description" textfile="$(find baxter_description)/urdf/baxter.urdf"/>
+  <!-- Start the Joint Trajectory Action Server -->
 
-  <param name="object_tracker/config_folder" value="$(find baxter_demos)/config/"/>
+  <!--node name="rsdk_joint_trajectory_action_server" pkg="baxter_interface"
+  type="joint_trajectory_action_server.py" args="limb $(arg limb)"
+  required="true"/-->
 
-  <node pkg="baxter_demos" name="object_finder" type="object_finder.py"
-  args="--limb $(arg limb) --method $(arg method)" required="true">
-  </node>
-
 
-  <node pkg="baxter_demos" name="estimate_depth" type="estimate_depth.py"
-  args="--limb $(arg limb)" />
+  <!--Parameters for the baxter_demos/object_tracker application based on Baxter's workspace-->
 
-  <node pkg="baxter_demos" name="servo_to_object" type="servo_to_object.py"
-args="--limb $(arg limb) --folder $(arg folder)" required="true">
+<node pkg="baxter_demos" name="visual_servo" type="visual_servo.py"
+args="--limb $(arg limb)" required="true">
+    <!--speed determines how fast Baxter's arm moves in visual_servo-->
+    <param name="servo_speed" value="0.0095"/>
 
-</node>
+    <!--min_pose_z is the minimum allowed height of Baxter's end effector. Figure this out by checking what the z position coordinate of /robot/limb/<side>/endpoint_state is when Baxter's gripper is close to its work surface-->
+    <param name="min_pose_z" value="-0.05"/>
+
+    <!--min_ir_depth is the minimum allowed IR depth. Figure this out by placing Baxter's gripper around an object, close enough for it to get a firm grasp on that object, and check the range field of /robot/range/<side>_hand_range/state -->
+    <param name="min_ir_depth" value="0.05" />
 
+    <!--the dimensions of Baxter's hand camera-->
+    <param name="camera_x" value="640"/>
+    <param name="camera_y" value="400"/>
+
+    <param name="goal_ratio_x" value="0.5"/> 
+    <param name="goal_ratio_y" value="0.15"/> 
 
+    <!-- UNUSED: these are the ratios where the edges of Baxter's grippers appear on its right camera, relative to the dimensions of the camera. So, if the width of the camera is 640, you can get the pixel value position of the innermost edge of the left gripper by gripper_min*640-->
+    <param name="gripper_min" value="0.289" />
+    <param name="gripper_max" value="0.75" />
+</node>
+
+  <node pkg="baxter_demos" name="object_finder" type="object_finder.py"
+  args="--limb $(arg limb) --method $(arg method)" required="true"/>
+
+  <!--node pkg="baxter_demos" name="gripper" type="gripper.py"
+  args="limb $(arg limb)" required="true"/ -->
+
 
 </launch>

package.xml

@@ -19,6 +19,7 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>sensor_msgs</build_depend>
@@ -28,7 +29,10 @@
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>baxter_core_msgs</build_depend>
   <build_depend>baxter_interface</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>tf</build_depend>
 
+  <run_depend>roscpp</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>actionlib</run_depend>
@@ -39,6 +43,8 @@
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>baxter_core_msgs</run_depend>
   <run_depend>baxter_interface</run_depend>
+  <run_depend>pcl_conversions</run_depend>
+  <run_depend>tf</run_depend>
 
 </package>
 

scripts/estimate_depth.py

@@ -107,7 +107,7 @@ def solve_goal_pose(self, centroid):
         center = (centroid[0] - self.camera_x/2, centroid[1] - self.camera_y/2) 
         vec = numpy.array( self.camera_model.projectPixelTo3dRay(center) )
         # Scale it by the IR reading
-        d_cam = ( self.ir_reading - self.min_ir_depth - self.object_height ) * vec
+        d_cam = ( self.ir_reading - self.min_ir_depth - self.object_height/2.0 ) * vec
         d_cam = numpy.concatenate((d_cam, numpy.ones(1)))
         #print "Camera vector:", d_cam
 

scripts/object_finder.py

@@ -40,11 +40,6 @@
 processed_win = params[processed_win]
 edge_win = params[edge_win]
 
-"""def getHoughArgs():
-    names = ["rho", "theta", "threshold", "minLineLength", "maxLineGap"]
-    node_full_name = node_name+"/"
-    return [rospy.get_param(node_full_name+name) for name in names]"""
-
 class CameraSubscriber:
     """Subscribe to the hand camera image and convert the message to an OpenCV-
     friendly format"""
@@ -126,8 +121,7 @@ def __init__(self, method, point):
         self.processed = None
         self.canny = None
 
-    def publish(self, limb):
-        topic = "object_tracker/"+limb+"/centroid"
+    def publish(self, topic):
 
         self.handler_pub = rospy.Publisher(topic, BlobInfoArray)
         self.pub_rate = rospy.Rate(params['rate'])
@@ -179,7 +173,7 @@ def callback(self, data):
 
         # Find the contour associated with self.point
         contour_img = self.processed.copy()
-        contours, hierarchy = cv2.findContours(contour_img, cv2.RETR_LIST,
+        contours, hierarchy = cv2.findContours(contour_img, cv2.RETR_EXTERNAL,
                                                cv2.CHAIN_APPROX_SIMPLE)
         if contours is None or len(contours) == 0:
             rospy.loginfo( "no contours found" )
@@ -188,6 +182,7 @@ def callback(self, data):
 
         cv2.drawContours(contour_img, contours, -1, (255, 255, 255))
         contour_img = cv2.cvtColor(contour_img, cv2.COLOR_GRAY2BGR)
+        self.canny = self.edgeDetect(contour_img)
         self.centroids = []
         self.axes = []
 
@@ -202,23 +197,23 @@ def callback(self, data):
                               int(moments['m01']/moments['m00']), 0 )
             self.centroids.append( centroid )
 
-            self.canny = self.edgeDetect(contour_img)
             axis = self.getObjectAxes(self.canny, contour)
             self.axes.append( axis )
 
             # Draw the detected object
             if axis is not None:
                 cv2.line(self.cur_img, tuple(axis[0:2]),
                                        tuple(axis[3:5]), (0, 255, 0), 2)
-            """    self.prev_axis = self.axis
-            elif self.prev_axis is not None:
-                cv2.line(self.cur_img, tuple(prev_axis[0:2]),
-                                       tuple(prev_axis[3:5]), (0, 255, 0), 2)"""
-
 
             cv2.circle(img=self.cur_img, center=centroid[0:2], radius=2,
                        color=(0, 255, 0), thickness=-1)
-
+        """for contour in contours:
+            has_centroid = False
+            for centroid in centroid:
+                if cv2.pointPolygonTest(contour, centroid) > 0:        
+                    if has_centroid:"""
+                        # Break and remove this contour and its centroid from play
+
         self.prev_img = self.img
 
     def updatePoint(self, event, x, y, flags, param):
@@ -422,6 +417,8 @@ def main():
                         required=False, help='which detection method to use')
     parser.add_argument('-t', '--topic', required=False,
                         help='which image topic to listen on')
+    parser.add_argument('-p', '--pub_topic', required=False,
+                        help='which topic to publish to')
 
     args = parser.parse_args(rospy.myargv()[1:])
     if args.limb is None:
@@ -432,6 +429,8 @@ def main():
         args.method = 'color'
     if args.topic is None:
         args.topic = "/cameras/"+limb+"_hand_camera/image"
+    if args.pub_topic is None:
+        args.pub_topic = "object_tracker/"+limb+"/centroid"
 
     print args
     print("Initializing node... ")
@@ -440,35 +439,34 @@ def main():
     rospy.on_shutdown(cleanup)
 
     baxter_cams = ["/cameras/right_hand_camera/image",
-                    "/cameras/left_hand_camera/image",
-                    "/cameras/head_camera/image"]
+                   "/cameras/left_hand_camera/image",
+                   "/cameras/head_camera/image"]
     if args.topic in baxter_cams:
         print("Getting robot state... ")
         rs = baxter_interface.RobotEnable(CHECK_VERSION)
         print("Enabling robot... ")
         rs.enable()
 
     cv2.namedWindow(raw_win)
+    cv2.namedWindow(edge_win)
     cam = CameraSubscriber()
     cam.subscribe(args.topic)
 
-    rospy.loginfo( "Click on the object you would like to track, then press\
-                    any key to continue." )
-    ml = common.MouseListener()
-
     if "object_finder_test" in args.topic:
         # Hardcoded position
         point = (322, 141)
-
     else:
+        rospy.loginfo( "Click on the object you would like to track, then press\
+                        any key to continue." )
+        ml = common.MouseListener()
+
         cv2.setMouseCallback(raw_win, ml.onMouse)
         while not ml.done:
             if cam.cur_img is not None:
                 cv2.imshow(raw_win, cam.cur_img)
 
             cv2.waitKey(cam.cv_wait)
         point = (ml.x_clicked, ml.y_clicked)
-    detectMethod = None
 
     cam.unsubscribe()
 
@@ -479,8 +477,7 @@ def main():
     print "Starting image processor"
     imgproc = ObjectFinder(args.method, point)
     imgproc.subscribe(args.topic)
-    imgproc.publish(limb)
-    cv2.namedWindow(edge_win)
+    imgproc.publish(args.pub_topic)
     cv2.setMouseCallback(raw_win, imgproc.updatePoint)
 
     while not rospy.is_shutdown():

scripts/servo_to_object.py

@@ -55,6 +55,10 @@ def main():
     required.add_argument(
         '-f', '--folder', help='path to assets/ folder containing help images'
     )
+    required.add_argument('-t', '--topic', required=False,
+                          help='which topic to subscribe to')
+
+
     args = parser.parse_args(rospy.myargv()[1:])
     print args
     limb = args.limb
@@ -74,6 +78,8 @@ def main():
 
     if args.folder is None:
         args.folder = "/home/jackie/ros_ws/src/baxter_demos/assets/"
+    if args.topic is None:
+        args.topic = "object_tracker/"+args.limb+"/centroid"
     # First, get start and end configurations from the user
     filenames = ["getpoint1.png", "getpoint2.png", "executing_grasp.png"]
     filenames = [args.folder+filename for filename in filenames]
@@ -131,7 +137,7 @@ def main():
 
     # Subscribe to object_finder and start visual servoing/grasping
     vc = VisualCommand(iksvc, limb)
-    vc.subscribe()
+    vc.subscribe(args.topic)
 
     while (not vc.done) and (not rospy.is_shutdown()):
         rate.sleep()

scripts/super_stacker.py

@@ -3,6 +3,7 @@
 import rospy
 import baxter_interface
 from baxter_interface import CHECK_VERSION
+import yaml
 import os
 import argparse
 import numpy
@@ -13,28 +14,37 @@
 
 from geometry_msgs.msg import Pose, PoseArray
 
-object_height = 0.06
+config_folder = rospy.get_param('object_tracker/config_folder')
+
+with open(config_folder+'servo_to_object.yaml', 'r') as f:
+    params = yaml.load(f)
 
 class DepthCaller:
-    def __init__(self, limb, iksvc):
+    def __init__(self, limb):
         self.done = False
-        self.iksvc = iksvc
         self.limb = limb
-
+        self.object_height = params['object_height']
         self.depth_handler = rospy.Subscriber("object_tracker/"+limb+"/goal_poses", PoseArray, self.depth_callback)
 
     def depth_callback(self, data):
         print "Estimating depth"
         self.object_poses = []
         for pose in data.poses:
-            p = [pose.position.x, pose.position.y, pose.position.z]+[pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
+            p = [pose.position.x, pose.position.y, pose.position.z + self.object_height]+\
+                [pose.orientation.x, pose.orientation.y, pose.orientation.z,\
+                pose.orientation.w]
             self.object_poses.append(p)
         #print p
 
         self.done = True
         print "unregistering"
         self.depth_handler.unregister()
 
+def incrementPoseZ(pose, inc):
+    pos = numpy.array(pose[0:3])
+    pos += numpy.array((0, 0, inc))
+    pose = numpy.concatenate( (pos, pose[3:7]) )
+    return pose.tolist()
 
 def main():
     arg_fmt = argparse.RawDescriptionHelpFormatter
@@ -66,36 +76,31 @@ def main():
     limbInterface = baxter_interface.Limb(limb)
 
     iksvc, ns = ik_command.connect_service(limb)
-    rate = rospy.Rate(100)
+    rate = rospy.Rate(params['rate'])
 
     # Get goal poses of each object in the scene
-    dc = DepthCaller(limb, iksvc)
+    dc = DepthCaller(limb)
 
     # Subscribe to estimate_depth
     # Move to pose published by estimate_depth
     while (not dc.done) and (not rospy.is_shutdown()):
         rate.sleep()
         #pass
+
 
     print "Start visual servoing to first object"
 
     # Subscribe to object_finder and start visual servoing/grasping
     vc = VisualCommand(iksvc, limb)
     vc.subscribe()
 
-    def incrementPoseZ(pose, inc):
-        pos = numpy.array(pose[0:3])
-        pos += numpy.array((0, 0, inc))
-        pose = numpy.concatenate( (pos, pose[3:7]) )
-        return pose.tolist()
-
     # The stack pose is the pose of the smallest object with a z-offset
     # accounting for the height of the object (this assumption makes
     # it really important that the segmentation is accurate)
-    stack_pose = [0.594676466827,     -0.296644499519,     -0.0322744943164,     0.971805911045,     -0.22637170004,     0.065946440385,     0.000437813100735]
-    #stack_pose = dc.object_poses[-1]
-    #stack_pose = incrementPoseZ(stack_pose, object_height)
-    #dc.object_poses.pop(len(dc.object_poses)-1)
+    #stack_pose = [0.594676466827,     -0.296644499519,     -0.0322744943164,     0.971805911045,     -0.22637170004,     0.065946440385,     0.000437813100735]
+    stack_pose = dc.object_poses[-1]
+    stack_pose = incrementPoseZ(stack_pose, dc.object_height)
+    dc.object_poses.pop(len(dc.object_poses)-1)
 
     for pose in dc.object_poses[:len(dc.object_poses)]:
 
@@ -110,7 +115,7 @@ def incrementPoseZ(pose, inc):
         # Let go
         gripper_if.open(block=True)
 
-        stack_pose = incrementPoseZ(pose, object_height)
+        stack_pose = incrementPoseZ(pose, dc.object_height)
 
 
 if __name__ == "__main__":