TurtleBot3 zigzag coverage algorithm problem
This is the first time for me to touch ROS, TurtleBot3 and Python, I need to do coverage task with TurtleBot3 in Gazebo, but I meet some trouble, hope someone can help me!! I do not know whether I make some mistakes with my code, but it always in "not know" state, please help me check it and teach me how to fix and finish it, thank you! Following is my code:
#! /usr/bin/env python
# import ros stuff
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from tf import transformations
import math
regions_ = {
'right': 0,
'fright': 0,
'front': 0,
'fleft': 0,
'left': 0,
}
state_ = 0
state_dict_ = {
0: 'find the wall',
1: 'make wall right',
2: 'go straight',
3: 'turn left',
4: 'turn right',
5: 'X',
6: 'Y',
}
begin_ = True,
wall_right_ = False,
def clbk_laser(msg):
global regions_
regions_ = {
'right': min(min(msg.ranges[180:299]), 1.0),
'fright': min(min(msg.ranges[300:339]), 1.0),
'front': min(min(min(msg.ranges[0:19]), min(msg.ranges[340:359])),1.0),
'fleft': min(min(msg.ranges[20:59]), 1.0),
'left': min(min(msg.ranges[60:179]), 1.0),
}
take_action()
def change_state(state):
global state_, state_dict_
if state is not state_:
print "Wall follower - [%s] - %s" % (state, state_dict_[state])
state_ = state
def take_action():
global regions_
regions = regions_
global begin_
begin = begin_
global wall_right_
wall_right = wall_right_
msg = Twist()
linear_x = 0
angular_z = 0
state_description = ''
d = 0.6
if begin == True and wall_right == False:
if regions['front'] > d and regions['fleft'] > d and regions['fright'] > d:
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'
change_state(1)
#elif regions['front'] > d and regions['fleft'] > d and regions['fright'] < d:
#state_description = 'case 3 - fright'
#change_state(3)
#wall_right = 1
#begin = 0
else:
state_description = 'unknown case'
rospy.loginfo(regions)
elif begin == False and wall_right == True:
if regions['front'] > d and regions['fleft'] > d and regions['fright'] > d:
state_description = 'case 1 - nothing'
change_state(2)
elif regions['front'] < d and regions['fleft'] > d and regions['fright'] > d:
state_description = 'case 2 - front'
change_state(3)
elif regions['front'] > d and regions['fleft'] > d and regions['fright'] < d:
state_description = 'case 3 - fright'
change_state(2)
elif regions['front'] > d and regions['fleft'] < d and regions['fright'] > d:
state_description = 'case 4 - fleft'
change_state(5)
elif regions['front'] < d and regions['fleft'] > d and regions['fright'] < d:
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'
change_state(5)
elif regions['front'] < d and regions['fleft'] < d and regions['fright'] < d:
state_description = 'case 7 - front and fleft and fright'
change_state(3)
elif regions['front'] > d and regions['fleft'] < d and regions['fright'] < d ...