add comments

.vscode/c_cpp_properties.json

@@ -0,0 +1,42 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "",
+        "limitSymbolsToIncludedHeaders": true
+      },
+      "includePath": [
+        "/home/maggie/rosprojects/tool/devel/include/**",
+        "/home/maggie/rosprojects/catkin_ws/devel/include/**",
+        "/opt/ros/kinetic/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/amcl/include/**",
+        "/home/maggie/rosprojects/catkin_ws/src/ar_track_alvar/ar_track_alvar/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/base_local_planner/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/carrot_planner/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/clear_costmap_recovery/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/costmap_2d/include/**",
+        "/home/maggie/rosprojects/tool/src/ddynamic_reconfigure/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/dwa_local_planner/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/global_planner/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/huanyu_robot_start/include/**",
+        "/home/maggie/rosprojects/catkin_ws/src/learning_tf2/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/map_server/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/move_base/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/move_slow_and_clear/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/nav_core/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/navfn/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/opencv_move/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/publisher/include/**",
+        "/home/maggie/rosprojects/tool/src/realsense-ros/realsense2_camera/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/robot_pose_ekf/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/rotate_recovery/include/**",
+        "/home/maggie/rosprojects/catkin_ws/src/universal_robot/ur_kinematics/include/**",
+        "/home/maggie/rosprojects/catkin_ws/src/universal_robot/ur_modern_driver/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/usb_cam/include/**",
+        "/home/maggie/rosprojects/robot_ws/robot_ws/src/navigation-melodic/voxel_grid/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS"
+    }
+  ]
+}

.vscode/settings.json

@@ -0,0 +1,7 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/maggie/rosprojects/tool/devel/lib/python2.7/dist-packages",
+        "/home/maggie/rosprojects/catkin_ws/devel/lib/python2.7/dist-packages",
+        "/opt/ros/kinetic/lib/python2.7/dist-packages"
+    ]
+}

CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

huanyu_robot_start/script/show_mark.py

@@ -1,6 +1,9 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
+# this node is designed to achieve two functions:
+# 1.record the points for navigation and publish them correspondingly
+# 2.pulihs initial points when targe is grabbed
 
 from visualization_msgs.msg import Marker
 from visualization_msgs.msg import MarkerArray
@@ -13,18 +16,20 @@
 #from actionlib_msgs.msg import *  
 
 
-def blue_callback(blue):
+def blue_callback(blue): # check if the object is detected or not
     global find_blue
     if blue.x == 1:
         find_blue = 1
 
 
-def finish_callback(finish):
+def finish_callback(finish): # check if grab is finished or not
     global go_back
     if finish.x == 1:
         go_back = 1
     move = MoveBaseActionResult()
-    move.status.status = 3  # 目标已由操作服务器成功完成（终端状态）
+    move.status.status = 3  
+    # publish status = 3 to activate the status callback function
+    # so that the robot can return
     move.header.stamp = rospy.Time.now()
     goal_status_pub.publish(move)
 
@@ -36,16 +41,19 @@ def status_callback(msg):
     rospy.loginfo(str(find_blue))
     rospy.loginfo(str(go_back))
 
-    if msg.status.status == 3:  # 目标已由操作服务器成功完成（终端状态）
+    if msg.status.status == 3:  
+        # status=3 indicates robot has reached current goal
 
         try_again = 1
 
         if find_blue == 1:
+            # check if the robot has found the target
+            # if yes, the node stop publish new points and wait to return
 
-            if go_back == 0:
+            if go_back == 0: # sleep until the object is grabbed
                 rospy.sleep(1)
 
-            if go_back == 1:
+            if go_back == 1: # publsih the inital point 0 to move_base for navigating back to origin
                 index = count
                 pose = PoseStamped()
                 pose.header.frame_id = 'map'
@@ -54,16 +62,17 @@ def status_callback(msg):
                 pose.pose.position.y = markerArray.markers[0].pose.position.y
                 pose.pose.orientation.w = 1
                 goal_pub.publish(pose)
-                # add_more_point = 1  # 完成状态
+                # add_more_point = 1 
                 # index += 1
                 find_blue = 2
 
         elif find_blue == 0:
-
-            if add_more_point == 0:  # 到达目标
+            # check if the robot has found the target
+            # if no, the node will publish new points when one goal has reached
+            if add_more_point == 0:  # check if there is new nav goal
                 print
                 'Goal reached'
-            if index < count:  # 目标点未完成  count目标点计数，index完成目标点计数
+            if index < count:  # if not all input nav goal is finished
                 pose = PoseStamped()
                 pose.header.frame_id = 'map'
                 pose.header.stamp = rospy.Time.now()
@@ -73,7 +82,7 @@ def status_callback(msg):
                 goal_pub.publish(pose)
                 index += 1
 
-            elif index == count:  # 目标点完成
+            elif index == count:  # if all input nav goal is finished
                 print
                 'finish all point'
                 #index = 0;
@@ -87,6 +96,9 @@ def status_callback(msg):
                 add_more_point = 1
 
         elif find_blue == 2:
+            # find_blue 2 indicates the robot has grabbed and is asked to return
+            # when this line is executed, it means the robot has return to origin
+            # then it publish to put down the target
 
             grab = Point()
             grab.x = 2
@@ -95,7 +107,7 @@ def status_callback(msg):
             rospy.signal_shutdown("task finish")
 
 
-    # else:  # 未到达目标点
+    # else:  # nav goal not reached, for error and debugging
 
     #     print
     #     'Goal cannot reached has some error :', msg.status.status, ' try again!!!!'
@@ -119,9 +131,11 @@ def status_callback(msg):
     #         index += 1
 
 
-def click_callback(msg):  # 点击调用回调函数
+def click_callback(msg): 
+    #record the input nav goals and publish it in rviz for visualization
     global index, add_more_point, count, index
-    marker = Marker()  # marker 赋值
+
+    marker = Marker() 
     marker.header.frame_id = 'map'
     marker.type = marker.TEXT_VIEW_FACING
     marker.action = marker.ADD
@@ -139,7 +153,7 @@ def click_callback(msg):  # 点击调用回调函数
     marker.text = str(count)
     markerArray.markers.append(marker)
     id = 0
-    for m in markerArray.markers:  # 遍历，记录多少点
+    for m in markerArray.markers:  # record the total points
         m.id = id
         id += 1
 
@@ -153,13 +167,13 @@ def click_callback(msg):  # 点击调用回调函数
         pose.pose.orientation.w = 1
         goal_pub.publish(pose)
         index += 1
-    if add_more_point and count > 0:  # 有新目标出现
-        add_more_point = 0  # 新目标状态标志
+    if add_more_point and count > 0:  # new goal
+        add_more_point = 0  # status of new goal
         move = MoveBaseActionResult()
-        move.status.status = 3  # 目标已由操作服务器成功完成（终端状态）
+        move.status.status = 3  # robot has reached current goal
         move.header.stamp = rospy.Time.now()
         goal_status_pub.publish(move)
-    count += 1  # 增加目标点count
+    count += 1  # add nav goal point
     print
     'add a path goal point'
 
@@ -174,19 +188,30 @@ def Show_mark():
     find_blue = 0
     go_back = 0
     rospy.init_node('path_point')
-
-    mark_pub = rospy.Publisher('/path_point', MarkerArray, queue_size=100)
+     
+    # record input nav goals
     click_sub = rospy.Subscriber('/clicked_point', PointStamped, click_callback)
+    # store them in a marker array and publish to Rviz for visluization
+    mark_pub = rospy.Publisher('/path_point', MarkerArray, queue_size=100)
 
+    # publish goal
     goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1)
-
+
+    # status is an indication of nav goal finsihed, status = 3 means it is finished and the robot can
+    # publish another goal
     goal_status_sub = rospy.Subscriber('/move_base/result', MoveBaseActionResult, status_callback)
-    goal_status_pub = rospy.Publisher('/move_base/result', MoveBaseActionResult, queue_size=1)
 
+    #sometimes we need to self-publish to status to use the callback fucntion in status_callback
+    goal_status_pub = rospy.Publisher('/move_base/result', MoveBaseActionResult, queue_size=1)
+
+    #get to know if target is detected
     blue_sub = rospy.Subscriber('/find_blue', Point, blue_callback, queue_size=1)
+    # check if target is grabbed
     grab_finish_sub = rospy.Subscriber('/grab_finish', Point, finish_callback, queue_size=1)
+    # publish to /grab to put down the object in the end of the whole task
     grab_pub = rospy.Publisher('/grab', Point, queue_size=1)
-    #cancel any active goals
+
+    # cancel any active goals
     # move_base_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
     # move_base_client.cancel_goal()    
 

object_detect/src/bar_control.py

@@ -9,15 +9,17 @@
 def nothing(x):
     pass
 
+#trackbar for setting the hsv color boundaries
 cv2.namedWindow("Tracking")
-cv2.createTrackbar("LH", "Tracking", 58, 255, nothing)  # onchange?
+cv2.createTrackbar("LH", "Tracking", 58, 255, nothing) 
 cv2.createTrackbar("LS", "Tracking", 62, 255, nothing)
 cv2.createTrackbar("LV", "Tracking", 68, 255, nothing)
 cv2.createTrackbar("UH", "Tracking", 89, 255, nothing)
 cv2.createTrackbar("US", "Tracking", 213, 255, nothing)
 cv2.createTrackbar("UV", "Tracking", 239, 255, nothing)
 
-capture = cv2.VideoCapture(1)
+# please chage the index if cam is wrong, it would be 0 or 1 or two cam
+capture = cv2.VideoCapture(0)
 
 width = capture.get(cv2.CAP_PROP_FRAME_WIDTH)
 height = capture.get(cv2.CAP_PROP_FRAME_HEIGHT)

object_detect/src/go_to_center.py

@@ -8,22 +8,29 @@
 from geometry_msgs.msg import Twist
 from move_base_msgs.msg import *
 
+# this node mainly tracking the target by giving vel commands to base
+# it aslo publish to pick up target
+
 # --- Define our Class
 class motion:
 
     def __init__(self):
-        # --- Publisher to velocity info to Nano
+
         # self.image_pub = rospy.Publisher("image_topic", Image, queue_size=1)
-        # --- Subscriber to the center position and image height ana width
+
+        # --- Publisher to velocity info to Nano
         self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
         self.msg = Twist()
         self.grab_pub = rospy.Publisher('grab', Point, queue_size=1)
         self.grab = Point()
         #self.image_sub = rospy.Subscriber('image_rows_and_cols', Point, callback1)
+        # --- Subscriber to the center position and image height ana width
         self.image_sub = rospy.Subscriber('image_rows_and_cols', Point, self.callback1, queue_size=1,buff_size=5242880)
         self.center_sub = rospy.Subscriber('center_location', Point, self.callback2, queue_size=1, buff_size=5242880)
+
+        #linked with navigation, make sure that the robot motion is not drive by this node until object is detected
         self.goal_status_sub = rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.status_callback, queue_size=1, buff_size=5242880)
-        self.grab_finish_sub = rospy.Publisher('grab_finish', Point, self.finish_callback, queue_size=1)
+        self.grab_finish_sub = rospy.Subscriber('grab_finish', Point, self.finish_callback, queue_size=1)
 
     def callback1(self, info):
 
@@ -32,7 +39,7 @@ def callback1(self, info):
         cols = info.y
         rows = info.x
 
-    def status_callback(self, msg):
+    def status_callback(self, msg)
 
         global status_number
         status_number = msg.status.status
@@ -41,7 +48,9 @@ def finish_callback(self, finish):
 
         global finish_number
 
-        if finsih.x == 1:
+        if finsih.x == 1: #if the object is successfully grabbed, we will shutdown this node
+        # we will also set vel to 0 to make sure to avoid unwanted motion
+
             finsih_number = 1
 
             self.msg.linear.x = 0
@@ -56,7 +65,8 @@ def finish_callback(self, finish):
             rospy.loginfo("shut down go to center")
             rospy.signal_shutdown("shut down go to center")
 
-    def callback2(self, center):  # --- Callback2 function
+    def callback2(self, center):  
+        # callback function transfer center position to speed command and publsih to move the base
 
         global rows, cols, status_number, finish_number
 
@@ -77,7 +87,8 @@ def callback2(self, center):  # --- Callback2 function
             rospy.loginfo("shut down go to center")
             rospy.signal_shutdown("shut down go to center")
 
-        if status_number == 3:
+        if status_number == 3:    # if a nav goal is reached or if target is detected:
+        if True:
 
             x = center.x #rows
             y = center.y #cols
@@ -91,6 +102,9 @@ def callback2(self, center):  # --- Callback2 function
             turn = 0.0
             run = 0.01
 
+        # according to the center location, we divide the screen to 9 partitions
+        # and each corresponding to a particular base motion
+
             if  diff_y >= 10 and diff_x >= 10:             #up left
                 turn = 0.01
                 run = -0.01
@@ -117,6 +131,7 @@ def callback2(self, center):  # --- Callback2 function
 
             rate = rospy.Rate(10)
            # rospy.loginfo(self.msg)
+           # publish the velocity
 
             self.msg.linear.x = run
             self.msg.linear.y = float(0.0)
@@ -130,6 +145,8 @@ def callback2(self, center):  # --- Callback2 function
             rate.sleep()
 
             if run == 0 and turn == 0:
+                # this means the target is in goal zone now
+                # we then publish to /grab to pick up the object
                 self.grab.x = 1
                 self.grab.y = 0
                 self.grab_pub.publish(self.grab)