update name.

CMakeLists.txt

@@ -9,9 +9,10 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs 
   genmsg 
   message_generation 
-  serial
   actionlib
   moveit_msgs
+  image_transport
+  cv_bridge
 )
 
 add_message_files(FILES
@@ -35,16 +36,17 @@ catkin_package(
 )
 
 ## Build talker and listener
-include_directories(include ${catkin_INCLUDE_DIRS}) 
+include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 
 
 catkin_install_python(PROGRAMS 
-  scripts/display.py 
-  scripts/control_slider.py 
+  scripts/follow_display.py 
+  scripts/slider_control.py 
   scripts/control_marker.py
   scripts/mycobot_ros.py
   scripts/listen_real.py
   scripts/teleop_keyboard.py
   scripts/client.py
+  scripts/camera.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
@@ -52,5 +54,10 @@ install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   PATTERN "setup_assistant.launch" EXCLUDE)
 install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
+find_package(OpenCV REQUIRED)
+add_executable(opencv_camera src/opencv_camera)
+target_link_libraries(opencv_camera ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+add_executable(camera_display src/camera_display)
+target_link_libraries(camera_display ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
 
 

README.md

@@ -155,6 +155,8 @@ python scripts/test.py
                 w(x+)
   
       a(y-)     s(x-)     d(y+)
+
+      z(z-) x(z+)
   
   u(rx+)   i(ry+)   o(rz+)
   j(rx-)   k(ry-)   l(rz-)
@@ -172,6 +174,20 @@ python scripts/test.py
   currently:      speed 50        change size 10 
   ```
 
+## MoveIT
+
+### Execute plan with actual robot.
+
+```
+roslaunch mycobot_ros demo.launch
+```
+
+Open a new terminal and run:
+
+```
+rosrun mycobot_ros sync_signal.py
+```
+
 <!-- If you use the above command, then you may need to manually add some model components. If you don't want to be so troublesome, you can use the following command to load a saved **myCobot** model.
 
 ```bash

launch/mycobot_slider.launch

@@ -1,4 +1,7 @@
 <launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+
 	<arg name="model" default="$(find mycobot_ros)/urdf/mycobot_urdf.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_ros)/config/mycobot.rviz" />
 	<arg name="gui" default="true" />
@@ -12,7 +15,10 @@
 		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
 	</node>
 	<!-- Open control script -->
-	<node name="control_slider" pkg="mycobot_ros" type="control_slider.py"/> 
+	<!-- <node name="control_slider" pkg="mycobot_ros" type="control_slider.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
 	<!-- Show in Rviz -->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 </launch>

scripts/camera.py

@@ -0,0 +1,106 @@
+#!/usr/bin/env python
+import rospy
+import cv2 as cv
+import numpy as np
+from geometry_msgs.msg import Twist
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image
+from visualization_msgs.msg import Marker
+import tf_conversions
+from myCobotROS.srv import (
+    GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus)
+
+
+class image_converter:
+    def __init__(self):
+        self.mark_pub = rospy.Publisher("/bebop/marker", Marker, queue_size=1)
+        self.bridge = CvBridge()
+        self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
+        self.aruo_params = cv.aruco.DetectorParameters_create()
+        calibrationParams = cv.FileStorage('calibrationFileName.xml', cv.FILE_STORAGE_READ)
+        self.dist_coeffs = calibrationParams.getNode('distCoeffs').mat()
+        self.camera_matrix = None
+        # rospy.wait_for_service('get_joint_coords')
+        # rospy.wait_for_service('set_joint_coords')
+
+        try:
+            self.get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords)
+            self.set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords)
+        except:
+            print('Error: cannot connect service...')
+            exit(1)
+        self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
+
+
+    def callback(self, data):
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+            # sucess, cv_image = self.cap.read()
+        except CvBridgeError as e:
+            print(e)
+        size = cv_image.shape
+        focal_length = size[1]
+        center = [size[1]/2, size[0]/2]
+        if self.camera_matrix is None:
+            self.camera_matrix = np.array([
+                [focal_length,0,center[0]],
+                [0, focal_length, center[1]],
+                [0,0,1],
+            ], dtype=np.float32)
+        gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
+        corners, ids, rejectImaPoint = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
+        if len(corners) > 0:
+            if ids is not None:
+                # print('corners:', corners, 'ids:', ids)
+                rvec, tvec, _ = cv.aruco.estimatePoseSingleMarkers(corners, 0.05, self.camera_matrix, self.dist_coeffs)
+                (rvec - tvec).any()
+
+                print('rvec:', rvec, 'tvec:', tvec)
+
+                for i in range(rvec.shape[0]):
+                    cv.aruco.drawDetectedMarkers(cv_image, corners)
+                    cv.aruco.drawAxis(cv_image, self.camera_matrix, self.dist_coeffs, rvec[i, :, :], tvec[i, :, :], 0.03)
+
+                res = self.get_coords()
+                if res.x == res.y == 0.0:
+                    return
+                record_coords = [
+                    res.x, res.y, res.z, res.rx, res.ry, res.rz, 60, 1
+                ]
+                print(record_coords)
+
+        # [x, y, z, -172, 3, -46.8]
+        cv.imshow("Image", cv_image)
+
+
+        # marker = Marker()
+        # marker.header.frame_id = '/joint1'
+        # marker.header.stamp = rospy.Time.now()
+        # marker.type = marker.SPHERE
+        # marker.action = marker.ADD
+        # marker.scale.x = 0.04
+        # marker.scale.y = 0.04
+        # marker.scale.z = 0.04
+
+        # marker.pose.position.x = center_x / 100
+        # marker.pose.position.y = center_y / 100
+        # marker.pose.position.z = 0
+
+        # marker.color.a = 1.0
+        # marker.color.g = 1.0
+
+        cv.waitKey(3)
+        try:
+            # self.mark_pub.publish(marker)
+            pass
+        except CvBridgeError as e:
+            print e
+if __name__ == '__main__':
+    try:
+        rospy.init_node("cv_bridge_test")
+        rospy.loginfo("Starting cv_bridge_test node")
+        image_converter()
+        rospy.spin()
+    except KeyboardInterrupt:
+        print "Shutting down cv_bridge_test node."
+        cv.destroyAllWindows()

scripts/cilibrate_camera.py

@@ -0,0 +1,31 @@
+import numpy as np
+import cv2
+import cv2.aruco as aruco
+import glob
+
+def calibrateKd(im_fpath, aruco_len=60.0):
+    (w, h) = (6, 4)
+    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01)
+    objp = np.zeros((w*h, 3), np.float32)
+    objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)
+    objp *= aruco_len
+    objpoints, imgpoints = [], []
+    images = glob.glob(f'{im_fpath}/*')
+    for fname in images:
+        img = cv2.imread(fname)
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+        ret, corners = cv2.findChessboardCorners(gray, (w, h), None)
+        if ret == True:
+            objpoints.append(objp)
+            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
+            imgpoints.append(corners2)
+            img = cv2.drawChessboardCorners(img, (6, 4), corners2, ret)
+
+    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shap[::-1], None, None)
+    tot_error = 0
+    for i in range(len(objpoints)):
+        imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
+        error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2)/len(imgpoints2)
+        tot_error += error
+    print(tot_error)
+    return mtx, dist

scripts/follow_display.py

@@ -1,7 +1,6 @@
 #!/usr/bin/env python2
 # license removed for brevity
 import time
-import subprocess
 
 import rospy
 from sensor_msgs.msg import JointState
@@ -12,9 +11,28 @@ from pymycobot.mycobot import MyCobot
 
 
 def talker():
+    rospy.init_node('display', anonymous=True)
+
+    print('Try connect real mycobot...')
+    port = rospy.get_param('~port', '/dev/ttyUSB0')
+    baud = rospy.get_param('~baud', 115200)
+    print('port: {}, baud: {}\n'.format(port, baud))
+    try:
+        mycobot = MyCobot(port, baud)
+    except Exception as e:
+        print(e)
+        print('''\
+            \rTry connect mycobot failed!
+            \rPlease check wether connected with mycobot.
+            \rPlease chckt wether the port or baud is right.
+        ''')
+        exit(1)
+    mycobot.release_all_servos()
+    time.sleep(.1)
+    print('Rlease all servos over.\n')
+
     pub = rospy.Publisher('joint_states', JointState, queue_size=10)
     pub_marker = rospy.Publisher('visualization_marker', Marker, queue_size=10)
-    rospy.init_node('display', anonymous=True)
     rate = rospy.Rate(30)  # 30hz
 
     # pub joint state
@@ -36,6 +54,7 @@ def talker():
     marker_.header.frame_id = '/joint1'
     marker_.ns = 'my_namespace'
 
+    print('publishing ...')
     while not rospy.is_shutdown():
         joint_state_send.header.stamp = rospy.Time.now()
 
@@ -44,7 +63,7 @@ def talker():
         for index, value in enumerate(angles):
             data_list.append(value)
 
-        rospy.loginfo('{}'.format(data_list))
+        # rospy.loginfo('{}'.format(data_list))
         joint_state_send.position = data_list
 
         pub.publish(joint_state_send)
@@ -77,9 +96,6 @@ def talker():
 
 
 if __name__ == '__main__':
-    port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
-                                   shell=True).decode()
-    mycobot = MyCobot(port)
     try:
         talker()
     except rospy.ROSInterruptException:

scripts/slider_control.py

@@ -9,11 +9,13 @@ from sensor_msgs.msg import JointState
 from pymycobot.mycobot import MyCobot
 
 
+mc = None
+
+
 def callback(data):
     #rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
     # print(data.position)
     data_list = []
-    print(data.position)
     for index, value in enumerate(data.position):
         data_list.append(value)
 
@@ -22,19 +24,19 @@ def callback(data):
 
 
 def listener():
+    global mc
     rospy.init_node('control_slider', anonymous=True)
 
     rospy.Subscriber("joint_states", JointState, callback)
+    port = rospy.get_param('~port', '/dev/ttyUSB0')
+    baud = rospy.get_param('~baud', 115200)
+    print(port, baud)
+    mc = MyCobot(port, baud)
 
     # spin() simply keeps python from exiting until this node is stopped
+    print('sping ...')
     rospy.spin()
 
 
 if __name__ == '__main__':
-    print('sart')
-    port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
-                                   shell=True).decode()
-    print(port)
-    mc = MyCobot(port)
-    print(mc)
     listener()

src/camera_display.cpp

@@ -0,0 +1,30 @@
+#include <ros/ros.h>  
+#include <image_transport/image_transport.h>  
+#include <opencv2/highgui/highgui.hpp>  
+#include <cv_bridge/cv_bridge.h>  
+
+void imageCallback(const sensor_msgs::ImageConstPtr& msg)  
+{  
+	try  
+	{  
+		cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image); 
+		cv::waitKey(30);  
+	}  
+	catch (cv_bridge::Exception& e)  
+	{  
+		ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());  
+	}  
+}  
+
+int main(int argc, char **argv)  
+{  
+	ros::init(argc, argv, "image_listener");  
+	ros::NodeHandle nh;  
+	cv::namedWindow("view");  
+	cv::startWindowThread();  
+	image_transport::ImageTransport it(nh);  
+	image_transport::Subscriber sub = it.subscribe("camera/image", 1,imageCallback);  
+	ros::spin();  
+	cv::destroyWindow("view");  
+
+}  

src/opencv_camera.cpp

@@ -0,0 +1,60 @@
+#include <ros/ros.h>  
+#include <image_transport/image_transport.h>  
+#include <opencv2/highgui/highgui.hpp>  
+#include <cv_bridge/cv_bridge.h>  
+#include <sstream> // for converting the command line parameter to integer  
+
+int main(int argc, char** argv)  
+{  
+    // Check if video source has been passed as a parameter  
+    if(argv[1] == NULL)   
+    {  
+        ROS_INFO("argv[1]=NULL\n");  
+        return 1;  
+    }  
+
+    ros::init(argc, argv, "image_publisher"); // Initialize node
+    ros::NodeHandle nh;  
+    image_transport::ImageTransport it(nh);  
+    image_transport::Publisher pub = it.advertise("camera/image", 1);  // Publish topic
+
+    ros::Rate loop_rate(200); // refresh Hz.
+
+    // Convert the passed as command line parameter index for the video device to an integer  
+    std::istringstream video_sourceCmd(argv[1]);  
+    int video_source;  
+    // Check if it is indeed a number  
+    if(!(video_sourceCmd >> video_source))   
+    {  
+        ROS_INFO("video_sourceCmd is %d\n",video_source);  
+        return 1;  
+    }  
+
+    cv::VideoCapture cap(video_source);  
+    // Check if video device can be opened with the given index  
+    if(!cap.isOpened())   
+    {  
+        ROS_INFO("can not opencv video device\n");  
+        return 1;  
+    }  
+    cv::Mat frame;  
+    sensor_msgs::ImagePtr msg;  
+
+    while (nh.ok()) 
+    {  
+        cap >> frame;  
+        // cv::imshow("veiwer", frame);
+        // Check if grabbed frame is actually full with some content  
+        if(!frame.empty()) 
+        {  
+            msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();  
+            pub.publish(msg);  
+            //cv::Wait(1);  
+    	}  
+        ros::spinOnce();  
+        loop_rate.sleep();  
+        // if(cv::waitKey(2) >= 0)
+            // break;
+    }
+    
+}