opencv sync

src/features-detection/colorDetection.py

@@ -75,9 +75,10 @@ def segment (path):
     fname = path.split('/')[-1]
     cv2.imwrite('output/' + fname, img);
     print (path)
-    
-    #cv2.imshow("Image", img)
-    #cv2.waitKey(0)
+
+    # cv2.imshow("Image", img)
+    cv2.imwrite('camera_image_no_background.jpeg', img)
+    # cv2.waitKey(0)
 
     # construct the argument parse and parse the arguments
     ap = argparse.ArgumentParser()
@@ -98,33 +99,29 @@ def segment (path):
     colorValues = []
 
     # loop over the boundaries
-    for (lower, upper) in boundaries:
+    for i, (lower, upper) in enumerate(boundaries):
         # create NumPy arrays from the boundaries
-	lower = np.array(lower, dtype = "uint8")
-	upper = np.array(upper, dtype = "uint8")
+        lower = np.array(lower, dtype = "uint8")
+        upper = np.array(upper, dtype = "uint8")
 
-	# find the colors within the specified boundaries and apply
-	# the mask
-	mask = cv2.inRange(image, lower, upper)
+        # find the colors within the specified boundaries and apply
+        # the mask
+        mask = cv2.inRange(image, lower, upper)
         #print(cl.label(image, mask))
 
         # save the values
         colorValues.append(np.count_nonzero(mask))
 
         #print(np.count_nonzero(mask))
-	output = cv2.bitwise_and(image, image, mask = mask)
-        
-	# show the images
-	#cv2.imshow("images", np.hstack([image, output]))
-	#cv2.waitKey(0)
-
-    print(getColor(colorValues, ['RED', 'BLUE', 'YELLOW(ish)', 'GREY'], max(colorValues)))
-
-    
-
-segment('bal1.jpg')
+        output = cv2.bitwise_and(image, image, mask = mask)
 
+        # show the images
+        # cv2.imshow("images", np.hstack([image, output]))
+        cv2.imwrite('camera_image_color_{}.jpeg'.format(i), np.hstack([image, output]))
+        # cv2.waitKey(0)
 
+    print(getColor(colorValues, ['RED', 'BLUE', 'YELLOW(ish)', 'GREY'], max(colorValues)))
 
 
 
+segment('camera_image.jpeg')

src/test/baxter_kinematics.py

+#!/usr/bin/python
+
+# Copyright (c) 2013-2014, Rethink Robotics
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+#    this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in the
+#    documentation and/or other materials provided with the distribution.
+# 3. Neither the name of the Rethink Robotics nor the names of its
+#    contributors may be used to endorse or promote products derived from
+#    this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+
+from baxter_pykdl import baxter_kinematics
+
+
+def main():
+    rospy.init_node('baxter_kinematics')
+    print '*** Baxter PyKDL Kinematics ***\n'
+    kin = baxter_kinematics('right')
+
+    print '\n*** Baxter Description ***\n'
+    kin.print_robot_description()
+    print '\n*** Baxter KDL Chain ***\n'
+    kin.print_kdl_chain()
+    # FK Position
+    print '\n*** Baxter Position FK ***\n'
+    print kin.forward_position_kinematics()
+    # FK Velocity
+    # print '\n*** Baxter Velocity FK ***\n'
+    # kin.forward_velocity_kinematics()
+    # IK
+    print '\n*** Baxter Position IK ***\n'
+    pos = [0.582583, -0.180819, 0.216003]
+    rot = [0.03085, 0.9945, 0.0561, 0.0829]
+    print kin.inverse_kinematics(pos)  # position, don't care orientation
+    print '\n*** Baxter Pose IK ***\n'
+    print kin.inverse_kinematics(pos, rot)  # position & orientation
+    # Jacobian
+    print '\n*** Baxter Jacobian ***\n'
+    print kin.jacobian()
+    # Jacobian Transpose
+    print '\n*** Baxter Jacobian Tranpose***\n'
+    print kin.jacobian_transpose()
+    # Jacobian Pseudo-Inverse (Moore-Penrose)
+    print '\n*** Baxter Jacobian Pseudo-Inverse (Moore-Penrose)***\n'
+    print kin.jacobian_pseudo_inverse()
+    # Joint space mass matrix
+    print '\n*** Baxter Joint Inertia ***\n'
+    print kin.inertia()
+    # Cartesian space mass matrix
+    print '\n*** Baxter Cartesian Inertia ***\n'
+    print kin.cart_inertia()
+
+if __name__ == "__main__":
+    main()

src/test/test-trees.py

@@ -290,6 +290,14 @@ def get_test_data():
     return [test_data_0]
 
 
+def train(tree, td):
+    tree.fit(td[0], td[1])
+
+def get_direction(tree, item):
+    item = pd.DataFrame(item)
+    predicted = tree.predict(item)
+    return 'LEFT' if predicted else 'RIGHT'
+
 def run():
     """Main method to run the tests."""
     for i_t, tree in enumerate(get_trees()):
@@ -306,4 +314,4 @@ def run():
                                                                     predicted))
 
 
-run()
+# run()

src/test/test.py

@@ -4,6 +4,8 @@ import os
 import rospy
 import rospkg
 import baxter_interface
+import cv2
+from cv_bridge import CvBridge, CvBridgeError
 from gazebo_msgs.srv import (
     SpawnModel,
     DeleteModel,
@@ -29,6 +31,8 @@ from sensor_msgs.msg import (
     Image,
 )
 
+bridge = CvBridge()
+
 # Infer kinematics from pose
 def ik_request(limb, pose):
 
@@ -67,7 +71,15 @@ def move(limb, pose):
     limb.move_to_joint_positions(joints)
 
 def print_msg(msg):
-    pass
+    print("Received an image!")
+    try:
+        # Convert your ROS Image message to OpenCV2
+        cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
+    except Exception, e:
+        print(e)
+    else:
+        # Save your OpenCV2 image as a jpeg
+        cv2.imwrite('camera_image.jpeg', cv2_img)
     # print(msg)
 
 def take_snapshot():
@@ -120,4 +132,4 @@ if __name__ == '__main__':
     #                          z=0.0,
     #                          w=0.0)
     # move('left', Pose(position=Point(x=0.8, y=0.0, z=0.5), orientation=overhead_orientation))
-    # take_snapshot()
+    take_snapshot()