test senario

catkin_ws/src/asclinic_pkg/src/FSM.py

@@ -26,8 +26,10 @@ class FSM:
         self.state = 0
         self.x = -1
         self.y = -3
-        self.phi = 0
-        self.poi = [[1,-1], [4, -2], [6,-3], [7,1], [6,3], [3,3], [-1,1], [-1, -2]]
+        #self.phi = 0
+        self.phi = math.pi/2##########################
+        #self.poi = [[1,-1], [4, -2], [6,-3], [7,1], [6,3], [3,3], [-1,1], [-1, -2]]
+        self.poi = [[-1,1],[1,1],[1, -1]] ############
         self.poi_index = 0
         self.outer_control = rospy.Publisher("/outer_control_signal", pathpoint, queue_size=1, latch=False)
         self.camera_control = rospy.Publisher("/camera_control_signal", UInt32, queue_size=1, latch=False)

catkin_ws/src/asclinic_pkg/src/Kalman.cpp

@@ -12,7 +12,8 @@
 using Eigen::MatrixXd;
 
 // Robot's position and orientation variables
-float x = -1, y = -3, phi = 0.0;
+//float x = -1, y = -3, phi = 0.0;/////////////////////////////////////////////
+float x = -1, y = -3, phi = M_PI/2;
 int left_p = 1, right_p = 1;
 ros::Time time_previous, time_now; // Timing variables
 ros::Duration elapsed_time; // Duration to calculate elapsed time