Robotics StackExchange | Archived questions

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

Answers