Merge branch 'revert-e2495c80' into 'master'

Revert "final"

See merge request !1

catkin_ws/src/asclinic_pkg/src/nodes/my_main_control.py

@@ -90,7 +90,7 @@ class myMainControl:
         self.isUp = msg.isUp
 
 
-        # print("[my_main_control] Start Robot",str(msg))
+        print("[my_main_control] Start Robot",str(msg))
 
         counter = 0
 
@@ -103,25 +103,22 @@ class myMainControl:
                 current_theta.data = pos[3]
                 self.set_current_theta.publish(current_theta)
 
-                # print('[my_main_control] Publish theta and pos:',pos)
+                print('[my_main_control] Publish theta and pos:',pos)
 
                 # self.path_discription = mp.getPath(self.g,(pos[1],pos[2]),(target_x_pos,target_y_pos),3500,5000,50)
-                if (self.target_id == 5):
-                    self.path_discription = [[first_theta,first_dist], [0,0],[self.target_theta,0]]
-                else:
-                    self.path_discription = [[first_theta,first_dist], [self.target_theta,0]]
+                self.path_discription = [[first_theta,first_dist], [self.target_theta,0]]
                 rospy.loginfo("[my_main_control] Start")
 
                 # Start Odometry
-                # odo_msg = UInt32()
-                # odo_msg.data = 1
-                # self.odometry_timer_control.publish(odo_msg)
+                odo_msg = UInt32()
+                odo_msg.data = 1
+                self.odometry_timer_control.publish(odo_msg)
 
 
                 self.mainController()
                 break
             else:
-                # print('pos not found')
+                print('pos not found')
                 motor_msg = MotorDriveControl()
                 motor_msg.dir = 5
                 motor_msg.speed = 0
@@ -143,7 +140,7 @@ class myMainControl:
         start_line = self.path_start_line
         start_col = self.path_start_col
 
-        # print('test line',start_line, start_col)
+        print('test line',start_line, start_col)
 
         if(start_col):
             # 发布直行指令
@@ -159,25 +156,10 @@ class myMainControl:
             # 发布转向指令
 
             pos = my_pos.get_current_pos()
-            
             if(pos):
                 current_theta =  Float32()
                 current_theta.data = pos[3]
                 self.set_current_theta.publish(current_theta)
-                print('转向:',current_theta,'目标：',int(self.path_discription[start_line][start_col]))
-            else:
-                motor_msg = MotorDriveControl()
-                motor_msg.dir = 5
-                motor_msg.speed = 0
-                motor_msg.lspeed = -20
-                motor_msg.rspeed = 20
-                self.motor_drive_control.publish(motor_msg)
-                rospy.sleep(1.5)
-                motor_msg.dir = 5
-                motor_msg.speed = 0
-                motor_msg.lspeed = 0
-                motor_msg.rspeed = 0
-                self.motor_drive_control.publish(motor_msg)
 
             path_instruction = PathInstruction()
             path_instruction.isRelative = 0
@@ -204,10 +186,10 @@ class myMainControl:
             self.path_start_line = start_line
             self.mainController()
         else:
-            # print('else')
+            print('else')
             self.path_start_line = 0
             self.path_start_col = 0
-            # print('finish pos 1')
+            print('finish pos 1')
         
 
             target_msg = StartRobot()
@@ -216,9 +198,9 @@ class myMainControl:
             target_msg.isUp = self.isUp
 
             #stop Odometry
-            # odo_msg = UInt32()
-            # odo_msg.data = 0
-            # self.odometry_timer_control.publish(odo_msg)
+            odo_msg = UInt32()
+            odo_msg.data = 0
+            self.odometry_timer_control.publish(odo_msg)
 
             self.reach_to_object.publish(target_msg)
             

catkin_ws/src/asclinic_pkg/src/nodes/my_motor_control.cpp

@@ -90,8 +90,8 @@ void templateSubscriberCallback(const LeftAndRightInt & msg)
 		// ROS_INFO_STREAM("Robot Theta"<< current_theta <<"prev"<<prev_theta<<"current"<<theta);
 		// ROS_INFO_STREAM("Robot Target theta "<< (target_theta - 1) << "->" <<(target_theta + 1));
 
-		if(  current_theta >= (target_theta - 5) &&
-			 current_theta <= (target_theta + 5)
+		if(  current_theta >= (target_theta - 1) &&
+			 current_theta <= (target_theta + 1)
 		  ){
 			// 到达目标角度
 			setMortorStop();

catkin_ws/src/asclinic_pkg/src/nodes/my_reach_load.py

@@ -32,31 +32,26 @@ class myReachLoad:
         # rospy.Subscriber("/my_global_namespace"+"/set_instruction_done", UInt32, self.myInstructionDoneCallback)
         rospy.sleep(2.0)
 
-        # start_msg = StartRobot()
-        # start_msg.first_dist = 0
-        # start_msg.first_theta = -1
-        # start_msg.target_theta = -90
-        # start_msg.target_id = 10
-        # start_msg.isUp = 1
-        # self.start_robot.publish(start_msg)
-
-        self.target_theta = -90
-        self.target_id = 10
-        self.isUp = 1
-        self.run()
+        start_msg = StartRobot()
+        start_msg.first_dist = 300
+        start_msg.first_theta = 0
+        start_msg.target_theta = -90
+        start_msg.target_id = 10
+        start_msg.isUp = 1
+        self.start_robot.publish(start_msg)
 
         
 
     def tofDistaceCallBack(self,msg):
-        self.distance = msg.data
+        distance = msg.data
         dis_th = 300
 
         if(self.isUp):
-            dis_th = 420
+            dis_th = 380
 
-        print('robot stopped:',self.distance, 'thres:',dis_th,'isUp:',self.isUp)
+        # print('robot stopped:',distance, 'thres:',dis_th,'isUp:',self.isUp)
 
-        if(self.distance < dis_th and self.isRunning):
+        if(distance < dis_th and self.isRunning):
             motor_msg = MotorDriveControl()
             motor_msg.dir = 5
             motor_msg.speed = 0
@@ -69,7 +64,7 @@ class myReachLoad:
        
 
     def reachToObjectCallBack(self,msg):
-        # print('get the instruction')
+        print('get the instruction')
         self.target_theta = msg.target_theta
         self.target_id = msg.target_id
         self.isUp = msg.isUp
@@ -84,21 +79,21 @@ class myReachLoad:
 
         self.isRunning = True
 
-        counter = 0
+        
 
         while(1):
 
             if( not self.isRunning):
-                # print('enter not true')
+                print('enter not true')
                 break
 
             pos,theta = my_pos.get_t_vec(id)
             x = pos[0]
             motor_msg = MotorDriveControl()
 
-           
+            counter = 0
 
-            # print('relative pos:',pos,'theta',theta)
+            print('relative pos:',pos,'theta',theta)
 
             if(x != -100):
                 counter = 0
@@ -155,22 +150,17 @@ class myReachLoad:
                     motor_msg.rspeed = -15
                     self.motor_drive_control.publish(motor_msg)
                 # print(pos, x, theta)
-
-                if(self.distance<1200):
-                    rospy.sleep(1.5)
-                else:
-                    rospy.sleep(3.0)
+                rospy.sleep(3.0)
                 motor_msg.dir = 5
                 motor_msg.speed = 0
                 motor_msg.lspeed = 0
                 motor_msg.rspeed = 0
                 self.motor_drive_control.publish(motor_msg)
             else:
-                # print(counter)
                 
                 counter = counter + 1
 
-                if(counter > 2):
+                if(counter > 5):
                     motor_msg.dir = 5
                     motor_msg.speed = 0
                     motor_msg.lspeed = -20
@@ -186,7 +176,7 @@ class myReachLoad:
                     counter = 0
         
         
-        # print('reached to the object',self.isRunning)
+        print('reached to the object',self.isRunning)
         if(self.isUp):
             msg = UInt16()
             msg.data = 1
@@ -194,19 +184,6 @@ class myReachLoad:
             rospy.sleep(1.5)
             msg.data = 0
             self.template_publisher.publish(msg)
-
-            motor_msg.dir = 5
-            motor_msg.speed = 0
-            motor_msg.lspeed = -20
-            motor_msg.rspeed = 20
-            self.motor_drive_control.publish(motor_msg)
-            rospy.sleep(3.0)
-            motor_msg.dir = 5
-            motor_msg.speed = 0
-            motor_msg.lspeed = 0
-            motor_msg.rspeed = 0
-            self.motor_drive_control.publish(motor_msg)
-
         else:
             msg = UInt16()
             msg.data = 2
@@ -215,19 +192,17 @@ class myReachLoad:
             msg.data = 0
             self.template_publisher.publish(msg)
 
-            motor_msg.dir = 5
-            motor_msg.speed = 0
-            motor_msg.lspeed = -20
-            motor_msg.rspeed = 20
-            self.motor_drive_control.publish(motor_msg)
-            rospy.sleep(4.5)
-            motor_msg.dir = 5
-            motor_msg.speed = 0
-            motor_msg.lspeed = 0
-            motor_msg.rspeed = 0
-            self.motor_drive_control.publish(motor_msg)
-
-        
+        motor_msg.dir = 5
+        motor_msg.speed = 0
+        motor_msg.lspeed = -20
+        motor_msg.rspeed = 20
+        self.motor_drive_control.publish(motor_msg)
+        rospy.sleep(3.0)
+        motor_msg.dir = 5
+        motor_msg.speed = 0
+        motor_msg.lspeed = 0
+        motor_msg.rspeed = 0
+        self.motor_drive_control.publish(motor_msg)
 
         if(self.ins_num == 0):
             rospy.sleep(1.0)