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:
state_description = 'case 8 - fleft and fright'
change_state(3)
else:
state_description = 'unknown case'
rospy.loginfo(regions)
elif begin == False and wall_right == False:
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(4)
elif regions['front'] > d and regions['fleft'] > d and regions['fright'] < d:
state_description = 'case 3 - fright'
change_state(6)
elif regions['front'] > d and regions['fleft'] < d and regions['fright'] > d:
state_description = 'case 4 - fleft'
change_state(2)
elif regions['front'] < d and regions['fleft'] > d and regions['fright'] < d:
state_description = 'case 5 - front and fright'
change_state(6)
elif regions['front'] < d and regions['fleft'] < d and regions['fright'] > d:
state_description = 'case 6 - front and fleft'
change_state(4)
elif regions['front'] < d and regions['fleft'] < d and regions['fright'] < d:
state_description = 'case 7 - front and fleft and fright'
change_state(4)
elif regions['front'] > d and regions['fleft'] < d and regions['fright'] < d:
state_description = 'case 8 - fleft and fright'
change_state(4)
else:
state_description = 'unknown case'
rospy.loginfo(regions)
else:
rospy.loginfo("not know")
def find_wall(): # keep the wall on the right side
msg = Twist()
msg.linear.x = 0.2
msg.angular.z = -0.3
return msg
def make_wall_right(): # keep the wall on the right side
msg = Twist()
msg.angular.z = 0.3
begin = False
wall_right = True
return msg
def go_straight():
global regions_
msg = Twist()
msg.linear.x = 0.5
return msg
def turn_left():
linear_speed = 0.2
goal_distance = 0.4
linear_duration = goal_distance / linear_speed
angular_speed = 1.0
goal_angle = pi/2
angular_duration = goal_angle / angular_speed
for i in range(2):
msg = Twist()
msg.linear.x = linear_speed
ticks = int(linear_duration * 20)
for t in range(ticks):
rate.sleep()
msg = Twist()
rospy.sleep(1)
msg.angular.z = angular_speed
ticks = int(goal_angle * 20)
for t in range(ticks):
rate.sleep()
msg = Twist()
rospy.sleep(1)
wall_right = False
return msg
def turn_right():
linear_speed = 0.2
goal_distance = 0.4
linear_duration = goal_distance / linear_speed
angular_speed = -1.0
goal_angle = pi/2
angular_duration = goal_angle / angular_speed
for i in range(2):
msg = Twist()
msg.linear.x = linear_speed
ticks = int(linear_duration * 20)
for t in range(ticks):
rate.sleep()
msg = Twist()
rospy.sleep(1)
msg.angular.z = angular_speed
ticks = int(goal_angle * 20)
for t in range(ticks):
rate.sleep()
msg = Twist()
rospy.sleep(1)
wall_right = True
return msg
#def X():
# global regions_
#msg = Twist()
#msg.linear.x = 0.5
#return msg
#def Y(): #need odom
#global regions_
#msg = Twist()
#msg.linear.x = 0.5
# return msg
#def go_straight():
#global regions_
#msg = Twist()
#msg.linear.x = 0.5
#return msg
def main():
global pub_
rospy.init_node('reading_laser')
pub_ = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
sub = rospy.Subscriber('/scan', LaserScan, clbk_laser)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
msg = Twist()
if state_ == 0:
msg = find_wall()
elif state_ == 1:
msg = make_wall_right()
elif state_ == 2:
msg = go_straight()
elif state_ == 3:
msg = turn_left()
elif state_ == 4:
msg = turn_right()
elif state_ == 5:
msg = X()
elif state_ == 6:
msg = Y()
pass
else:
rospy.logerr('Unknown state!')
pub_.publish(msg)
rate.sleep()
if __name__ == '__main__':
main()
Asked by VIvian on 2019-06-27 04:24:03 UTC
Comments