diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6eb716e..27f2481 100755
--- a/CMakeLists.txt
+++ b/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})
diff --git a/README.md b/README.md
index cdbc2c9..eebcdc0 100644
--- a/README.md
+++ b/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
+```
+
+
@@ -12,7 +15,10 @@
-
+
diff --git a/scripts/camera.py b/scripts/camera.py
new file mode 100755
index 0000000..d5a638f
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/scripts/cilibrate_camera.py b/scripts/cilibrate_camera.py
new file mode 100644
index 0000000..16b3e7d
--- /dev/null
+++ b/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
diff --git a/scripts/display.py b/scripts/follow_display.py
similarity index 76%
rename from scripts/display.py
rename to scripts/follow_display.py
index e18c893..238dcea 100755
--- a/scripts/display.py
+++ b/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:
diff --git a/scripts/control_slider.py b/scripts/slider_control.py
similarity index 77%
rename from scripts/control_slider.py
rename to scripts/slider_control.py
index f565676..3da56fa 100755
--- a/scripts/control_slider.py
+++ b/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()
diff --git a/src/camera_display.cpp b/src/camera_display.cpp
new file mode 100644
index 0000000..8df928d
--- /dev/null
+++ b/src/camera_display.cpp
@@ -0,0 +1,30 @@
+#include
+#include
+#include
+#include
+
+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");
+
+}
\ No newline at end of file
diff --git a/src/opencv_camera.cpp b/src/opencv_camera.cpp
new file mode 100644
index 0000000..9ace009
--- /dev/null
+++ b/src/opencv_camera.cpp
@@ -0,0 +1,60 @@
+#include
+#include
+#include
+#include
+#include // 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;
+ }
+
+}
\ No newline at end of file