Add follow left wall

src/test.py

@@ -19,11 +19,13 @@ regions_ = {
 }
 
 state_ = 0
+state_dir_ = -1
 state_dict_ = {
     0: 'find the wall',
-    1: 'turn left',
+    1: 'turn',
     2: 'follow the wall',
 }
+state_description_ = ''
 
 def clbk_laser(msg):
     global regions_
@@ -38,45 +40,45 @@ def clbk_laser(msg):
     take_action()
 
 def change_state(state):
-    global state_, state_dict_
+    global state_, state_dict_, state_description_
     if state is not state_:
-        print('State of Bot - [%s] - %s' % (state, state_dict_[state]))
+        print('State of Bot - [%s] - %s - %s' % (state, state_dict_[state], state_description_))
         state_ = state
 
 def take_action():
-    global regions_
+    global regions_, state_description_, state_dir_
     regions = regions_
     msg = Twist()
     linear_x = 0
     angular_z = 0
     
-    state_description = ''
-    
     d = 1.5
     
     if regions['front'] > d and regions['fleft'] > d and regions['fright'] > d:
-        state_description = 'case 1 - nothing'
+        state_description_ = 'case 1 - nothing'
         change_state(0)
     elif regions['front'] < d and regions['fleft'] > d and regions['fright'] > d:
-        state_description = 'case 2 - front'
+        state_description_ = 'case 2 - front'
         change_state(1)
     elif regions['front'] > d and regions['fleft'] > d and regions['fright'] < d:
-        state_description = 'case 3 - fright'
+        state_description_ = 'case 3 - fright'
+        state_dir_ = -1
         change_state(2)
     elif regions['front'] > d and regions['fleft'] < d and regions['fright'] > d:
-        state_description = 'case 4 - fleft'
-        change_state(0)
+        state_description_ = 'case 4 - fleft'
+        state_dir_ = 1
+        change_state(2)
     elif regions['front'] < d and regions['fleft'] > d and regions['fright'] < d:
-        state_description = 'case 5 - front and fright'
+        state_description_ = 'case 5 - front and fright'
         change_state(1)
     elif regions['front'] < d and regions['fleft'] < d and regions['fright'] > d:
-        state_description = 'case 6 - front and fleft'
+        state_description_ = 'case 6 - front and fleft'
         change_state(1)
     elif regions['front'] < d and regions['fleft'] < d and regions['fright'] < d:
-        state_description = 'case 7 - front and fleft and fright'
+        state_description_ = 'case 7 - front and fleft and fright'
         change_state(1)
     elif regions['front'] > d and regions['fleft'] < d and regions['fright'] < d:
-        state_description = 'case 8 - fleft and fright'
+        state_description_ = 'case 8 - fleft and fright'
         change_state(0)
     else:
         state_description = 'unknown case'
@@ -85,12 +87,12 @@ def take_action():
 def find_wall():
     msg = Twist()
     msg.linear.x = 0.2
-    msg.angular.z = -0.3
+    msg.angular.z = state_dir_*0.3
     return msg
 
-def turn_left():
+def turn():
     msg = Twist()
-    msg.angular.z = 0.3
+    msg.angular.z = state_dir_*-0.3
     return msg
 
 def follow_the_wall():
@@ -105,7 +107,7 @@ def main():
     
     pub_ = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
     
-    sub = rospy.Subscriber('/front/scan', LaserScan, clbk_laser)
+    sub = rospy.Subscriber('/scan', LaserScan, clbk_laser)
     
     rate = rospy.Rate(20)
     while not rospy.is_shutdown():
@@ -113,7 +115,7 @@ def main():
         if state_ == 0:
             msg = find_wall()
         elif state_ == 1:
-            msg = turn_left()
+            msg = turn()
         elif state_ == 2:
             msg = follow_the_wall()
             pass