Commit ddf702a0 authored by Mathieu Reymond's avatar Mathieu Reymond
Browse files

opencv sync

parent f5aa843d
......@@ -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')
#!/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()
......@@ -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()
......@@ -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()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment