update

CMakeLists.txt

@@ -5,15 +5,16 @@ cmake_minimum_required(VERSION 2.8.3)
 project(ur5_notebook)
 
 # Find the catkin build system, and any other packages on which we depend.
-find_package(catkin REQUIRED COMPONENTS gazebo_msgs roscpp geometry_msgs turtlesim std_srvs std_msgs message_generation message_runtime)
+find_package(catkin REQUIRED COMPONENTS gazebo_msgs roscpp geometry_msgs turtlesim std_srvs std_msgs message_generation)
 # Declare our catkin package.
 
 #add_message files
 add_message_files(FILES blocks_poses.msg)
+add_message_files(FILES Tracker.msg)
 
 generate_messages(DEPENDENCIES std_msgs)
 
-catkin_package()
+catkin_package(CATKIN_DEPENDS message_runtime)
 
 # Specify locations of header files.
 include_directories(include ${catkin_INCLUDE_DIRS})

msg/Tracker.msg

@@ -0,0 +1,12 @@
+# message type to describe the tracking information of the blocks
+# to be published as a topic
+
+float64 x  # x coordinate in the world
+float64 y  # y coordinate in the world
+float64 z  # z coordinate in the world
+float64 error_x
+float64 error_y
+float64 error_z
+bool flag1
+bool flag2
+bool flag3

ur5_mp.py

@@ -25,9 +25,10 @@
 import rospy, sys, numpy as np
 import moveit_commander
 from copy import deepcopy
-from geometry_msgs.msg import Twist
+import geometry_msgs.msg
 import moveit_msgs.msg
 from sensor_msgs.msg import Image
+from ur5_notebook.msg import Tracker
 
 
 from std_msgs.msg import Header
@@ -39,11 +40,10 @@
 class ur5_mp:
     def __init__(self):
         rospy.init_node("ur5_mp", anonymous=False)
-        self.cxy_sub = rospy.Subscriber('cxy', Twist, self.tracking_callback, queue_size=1)
+        self.cxy_sub = rospy.Subscriber('cxy', Tracker, self.tracking_callback, queue_size=1)
 
         self.track_flag = False
         self.default_pose_flag = True
-        self.area_flag = 0.0
         self.cx = 400.0
         self.cy = 400.0
         self.points=[]
@@ -153,11 +153,11 @@ def cleanup(self):
         moveit_commander.os._exit(0)
 
     def tracking_callback(self, msg):
-        self.track_flag = bool(msg.linear.z)
-        self.cx = msg.linear.x
-        self.cy = msg.linear.y
-        self.error_x = msg.angular.x
-        self.error_y = msg.angular.y
+        self.track_flag = msg.flag1
+        self.cx = msg.x
+        self.cy = msg.y
+        self.error_x = msg.error_x
+        self.error_y = msg.error_y
 
         if (self.track_flag and -0.4 < self.waypoints[0].position.x and self.waypoints[0].position.x < 0.6):
             self.execute()

ur5_vision.py

@@ -25,7 +25,8 @@
 import rospy, sys, numpy as np
 import moveit_commander
 from copy import deepcopy
-from geometry_msgs.msg import Twist
+import geometry_msgs.msg
+from ur5_notebook.msg import Tracker
 import moveit_msgs.msg
 import cv2, cv_bridge
 from sensor_msgs.msg import Image
@@ -34,18 +35,17 @@
 from std_msgs.msg import Header
 from trajectory_msgs.msg import JointTrajectory
 from trajectory_msgs.msg import JointTrajectoryPoint
-twist = Twist()
+tracker = Tracker()
 class ur5_vision:
     def __init__(self):
         rospy.init_node("ur5_vision", anonymous=False)
-        self.track_flag = 0.0
-        self.default_pose_flag = 1.0
-        self.area_flag = 0.0
+        self.track_flag = False
+        self.default_pose_flag = True
         self.cx = 400.0
         self.cy = 400.0
         self.bridge = cv_bridge.CvBridge()
         self.image_sub = rospy.Subscriber('/ur5/usbcam/image_raw', Image, self.image_callback)
-        self.cxy_pub = rospy.Publisher('cxy', Twist, queue_size=1)
+        self.cxy_pub = rospy.Publisher('cxy', Tracker, queue_size=1)
 
 
     def image_callback(self,msg):
@@ -79,16 +79,16 @@ def image_callback(self,msg):
                 area = cv2.contourArea(c)
 
                 if area > 7500:
-                    self.track_flag = 1.0
+                    self.track_flag = True
                     self.cx = cx
                     self.cy = cy
                     self.error_x = self.cx - w/2
                     self.error_y = self.cy - h/2
-                    twist.linear.x = cx
-                    twist.linear.y = cy
-                    twist.linear.z = self.track_flag
-                    twist.angular.x = self.error_x
-                    twist.angular.y = self.error_y
+                    tracker.x = cx
+                    tracker.y = cy
+                    tracker.flag1 = self.track_flag
+                    tracker.error_x = self.error_x
+                    tracker.error_y = self.error_y
                     #(_,_,w_b,h_b)=cv2.boundingRect(c)
                     #print w_b,h_b
                     # BEGIN circle
@@ -98,11 +98,11 @@ def image_callback(self,msg):
                     #BGIN CONTROL
                     break
                 else:
-                    self.track_flag = 0.0
-                    twist.linear.z = self.track_flag
+                    self.track_flag = False
+                    tracking_info.flag1 = self.track_flag
 
 
-        self.cxy_pub.publish(twist)
+        self.cxy_pub.publish(tracker)
         cv2.namedWindow("window", 1)
         cv2.imshow("window", image )
         cv2.waitKey(1)