ROS in 5 days (Python) - ROSject Part 2

Course Support ROS Basics In 5 Days (Python)
1

Hi there,

Absolutely loving the platform so far! Though, with the next rosject features added into the ROS basics in 5 courses i find myself stuck sometimes, and i really have a hard time getting anywhere, even with the given hints. I’m a Python novice, so bear with me.

Specifically, im stuck trying to make the turtlebot find the wall, move close to it, and then turn to be readu for the follow_wall program from part1. I’m able to get the turtlebot to find the nearest object and move towards it, but i think my code structure is really bad. Once i have the main code down for the movement, i’ll fix the the service calls last. This is what i have so far:

#!/usr/bin/env python

import rospy

from sensor_msgs.msg import LaserScan

from geometry_msgs.msg import Twist

def next_step(msg):

    print("NEXT STEP HERE")

    rospy.sleep(10)

def move_forward(msg):

    step2_front_scan = msg.ranges[0]

    print("Front scan: ", step2_front_scan)

    print("--------------------------------------------")

    rospy.loginfo("Moving towards wall!")

    while step2_front_scan > 0.3:

        print("Front_scan: ", step2_front_scan)

        move.linear.x = 0.05        

        pub.publish(move)

    move.linear.x = 0.0

    pub.publish(move)

    rospy.loginfo("Wall is now within target!")

    next_step(msg)

def callback(msg):

    front_scan = msg.ranges[0]

    min_scan = min(msg.ranges)

    a_front_scan = round(front_scan,1)

    a_min_scan = round(min_scan,1)

    print("Front scan: ", a_front_scan)

    print("Minimal scan: ", a_min_scan)

    print("--------------------------------------------")

    if a_front_scan <= a_min_scan:

        rospy.loginfo("Front is now facing wall!")

        move.angular.z = 0.0

        pub.publish(move)

        move_forward(msg)

    else:

        move.angular.z = 0.1

    

    pub.publish(move)

move = Twist() 

laser_get = LaserScan()

rospy.init_node('obstacle_avoidance_node')

pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

sub = rospy.Subscriber("/scan", LaserScan, callback)

rospy.spin()
2

Hi,

Could you state exactly the issue in the code? That way we can guide you where t go next.
Seeing this, check that index 0 corresponds in the laser to the reading you want .

3

Hi,

Yes, sure. Sorry for not being too concrete.

So overall, i think my code structure is wrong. I’m a little confused as to the best way to structure such a program. I feel this rosject was a big jump from the teaching material.
I’m able to have it scan, but because it seems impossible to hit the exact same as minimum value i rounded off the laser data to 1 decimals. It works ok, but it feels like a bad workaround. I call the function to move forward once the robot has stoppet, but it seems the LaserScan data from the msg variable does not update. Any guidance is appreciated.

Also:
Is it best practice to divide the program into several small functions?
such as the following? From what i can see others doing online, this seems like a good approach. Though i’m not sure about the best way to “jump” between functions, and still have the sensor data available from the initial laser callback.

def rotate_until_right():
    do_something

def move_forward():
     do_something

def find_closests_wall():
     do_something

def callback_laser(msg):
     do_something

def main():
    rospy.init_node('reading_laser')   

    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1    

    sub = rospy.Subscriber('/scan', LaserScan, calback_laser)
           
    while not rospy.is_shutdown():
        msg = Twist()

        pub_.publish(msg)
                
        rate.sleep()

if __name__ = 'main':
        main()
4

from the msg.ranges[0] i get the scan from the front of the turtlebot in the gazebo simulation.

5

So when you run your program, what does the simulation do? It shouldn’t be a structure problem if you are getting all of the data and the turtlebot starts moving. How did you determine that msg.ranges[0] is the scan from the front of the turtlebot in the gazebo simulation?

6

I’m very sorry, I just now realize that msg.ranges[0] is not the front. I cant say where i went wrong - i now realize that msg.ranges[180] is the front.

I’m able to get the robot turning, and finding the closest wall, but i’m unsure about how to keep the robot receiving the laser scan data when i jump to my move_forward() function. Currently, it doesnt update my msg.ranges[180] data, and just keeps driving forward.

def move_forward(msg):

    step2_front_scan = msg.ranges[0]

    print("Front scan: ", step2_front_scan)

    print("--------------------------------------------")

    rospy.loginfo("Moving towards wall!")

    while step2_front_scan > 0.3:

        print("Front_scan: ", step2_front_scan)

        move.linear.x = 0.05        

        pub.publish(move)

    move.linear.x = 0.0

    pub.publish(move)

    rospy.loginfo("Wall is now within target!")

def callback(msg):

    front_scan = msg.ranges[180]

    min_scan = min(msg.ranges)

    a_front_scan = round(front_scan,1)

    a_min_scan = round(min_scan,1)

    print("Front scan: ", a_front_scan)

    print("Minimal scan: ", a_min_scan)

    print("--------------------------------------------")

    if a_front_scan <= a_min_scan:

        rospy.loginfo("Front is now facing wall!")

        move.angular.z = 0.0

        pub.publish(move) # publish the move object

        move_forward(msg)

    else:

        move.angular.z = -0.2

    

    pub.publish(move) # publish the move object

move = Twist() # Creates a Twist message type object

laser_get = LaserScan()

rospy.init_node('obstacle_avoidance_node') # Initializes a node

pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)  # Publisher object which will publish "Twist" type messages

sub = rospy.Subscriber("/scan", LaserScan, callback)  # Subscriber object which will listen "LaserScan" type messages

rospy.spin() # Loops infinitely until someone stops the program execution
7

Hi,

The best way would be to use a global variable or place the callback inside a class. I would say that placing the callback inside a class is by far the best option. That way, you can assign the newest laser data to a variable of the class like this:

def callback(msg):

    self.front_scan = msg.ranges[180]
1 Like
8

thanks Miguel. I’ll keep working on it. I’m many hours into this rosject now :sweat_smile:

9

I ended up spending way too long on this, but i learned a lot trying to figure it out. Never actually ended up using the service call, which i guess was the point. I found an older example for a wall following robot from The Construct webpage, and modified it enough to work with this case. I didn’t end up using a class, partly because i’m not particularly good at setting them up currently (i believe this is a later part of the ROS basics course in 5 days), and also the example was not using it.

Here’s the code for anyone finding this thread in the future. It’s probably not perfect, but it works as long as the wall is the nearest object. Any comments on making it better is highly appreciated.

#!/usr/bin/env python

# -*- coding: utf-8 -*-

import rospy

from sensor_msgs.msg import LaserScan

from geometry_msgs.msg import Twist

pub_ = None

# defines the regions of the laser, empty values.

regions_ = {

    'right': 0,

    'fright': 0,

    'front': 0,

    'fleft': 0,

    'left': 0,

}

# starting state

state_ = 0

# state dictionary

state_dict_ = {

    0: 'find the wall',

    1: 'go to the wall',

    2: 'turn left in front of the wall',

    3: 'follow the wall'

}

# function that changes the state and prints the current state number and name.

# also, stores the new state as the old state

def change_state(state):

    global state_, state_dict_

    print ('State: [%s] - %s' % (state, state_dict_[state]))

    state_ = state      # new becomes old

# Scan actions

def take_action():

    global regions_, min_scan_, state_

    regions = regions_

    min_scan = round(min_scan_,2)

    # print("min: ", min_scan)     # for debug

    # print("front: ", round(regions['front'],2))    # for debug

    msg = Twist()

    linear_x = 0

    angular_z = 0

    state_description = ''

    dist = 0.3

    # the many cases

    if round(regions['front'],2) > min_scan and state_ != 1 and state_ != 2 and state_ !=3:

        state_description = 'drejer front mod min_scan'

        change_state(0)

    

    elif round(regions['front'],2) <= min_scan and state_ != 1 and state_ != 2 and state_ != 3:

        state_description = 'kører mod væg'

        change_state(1)

    

    elif regions['front'] < dist and regions['fleft'] > dist and regions['fright'] > dist:  

        state_description = 'case 2 - front'

        change_state(2)

    elif regions['front'] < dist and regions['fleft'] > dist and regions['fright'] < dist:

        state_description = 'case 5 - front and fright'

        change_state(2)

    elif regions['front'] < dist and regions['fleft'] < dist and regions['fright'] > dist:

        state_description = 'case 6 - front and fleft'

        change_state(2)

    elif regions['front'] < dist and regions['fleft'] < dist and regions['fright'] < dist:

        state_description = 'case 7 - front and fleft and fright'

        change_state(2)

    elif regions['front'] > dist and regions['fleft'] > dist and regions['fright'] < dist:

        state_description = 'case 3 - fright'

        change_state(3)

    else:

        state_description = 'unknown case'

# Action functions

def find_wall():

    msg = Twist()

    msg.angular.z = -0.3    # turns right

    return msg

def go_to_wall():

    msg = Twist()

    msg.linear.x = 0.1

    return msg

def turn_wall():

    msg = Twist()

    msg.linear.x = 0.005

    msg.angular.z = 0.3      # turns left

    return msg

def follow_wall():

    global regions_

    msg = Twist()

    msg.linear.x = 0.1

    return msg

# callback

def callback_laser(msg):

    global regions_, min_scan_

    min_scan_ = min(msg.ranges) # the smallest scan value currently available

    # divides the 180 degrees of the front of the lidar into 5 sections, in increments of 36 degrees

    regions_ = {

        'right':  min(min(msg.ranges[90:126]), 10),

        'fright': min(min(msg.ranges[127:162]), 10),

        'front':  min(min(msg.ranges[163:198]), 10),

        'fleft':  min(min(msg.ranges[199:234]), 10),

        'left':   min(min(msg.ranges[235:270]), 10),

    }

    take_action()

# the main function

def main():

    global pub

    rospy.init_node('reading_laser')

    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

    sub = rospy.Subscriber('/scan', LaserScan, callback_laser)

    rate = rospy.Rate(50)

    

    while not rospy.is_shutdown():

        msg = Twist()

        if state_ == 0:

            msg = find_wall()

        elif state_ == 1:

            msg = go_to_wall()

        elif state_ == 2:

            msg = turn_wall()

        elif state_ == 3:

            msg = follow_wall()

        else:

            rospy.logerr('Unknown state!')

        pub.publish(msg)

        rate.sleep()

if __name__ == '__main__':

    main()
1 Like