TurtleBot3 zigzag coverage algorithm problem

asked 2019-06-27 04:24:03 -0500

VIvian gravatar image

updated 2019-06-27 05:30:25 -0500

gvdhoorn gravatar image

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 ...
(more)
edit retag flag offensive close merge delete