Reactive Move

The goal is to create a straight node that permits the robot to move straight until obstacles are detected then turn left or right to avoid the obstacle.

Node Structure

The straight node is our first real complex node. It will process scan messages to characterize the local environment and regularly send coherent move instructions.

That for, we want to set up a node with \(2\) callback function. A sensing callback and a control callback. In facts, it is not recommended to send control messages only went sensing messages are received. This way, the robot will be capable of dealing failures as interruption in the sensing messages.

So, the straight script will be structured like this:

#!/usr/bin/python3
import rclpy
from  rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import sys


# Ros Node process:
def main():
    # Initialize ROS and a ROS node
    rclpy.init(args=sys.argv)
    node= Node( 'basic_move' )

    # Initialize our control:
    control= StraightCtrl()
    control.initializeRosNode( node )

    # infinite Loop:
    rclpy.spin( node )

    # clean end
    node.destroy_node()
    rclpy.shutdown()

# Ros Node Class:
class StraightCtrl :
    def initializeRosNode(self, rosNode ):
        # Get logger from the node:
        self._logger= rosNode.get_logger()

        # Initialize publisher:
        self._pubVelocity= rosNode.create_publisher(
            Twist, '/multi/cmd_nav', 10
        )

        # Initialize scan callback:
        self._subToScan= rosNode.create_subscription(
            LaserScan, '/scan',
            self.scan_callback, 10
        )

        # Initialize control callback:
        self._timForCtrl= rosNode.create_timer(
            0.05, self.control_callback
        )

    def scan_callback(self, scanMsg ):
        self._logger.info( '> get scan' )

    def control_callback(self):
        self._logger.info( '< define control' )

# Go:
if __name__ == '__main__' :
    main()

Include your straight node into your package, and test it:

ros2 run tutorial_pkg straight

Scan Callback

Implement the scan_callback method in a way that it fills \(2\) boolean class variables: self.obstacle_left and self.obstacle_right.

The self._obstacle_left should be True if an obstacle is detected front-left of the robot and Flase else. The self._obstacle_right should be True if an obstacle is detected front-right of the robot and Flase else.

We consider obstacles only if there are at \(1\) meter or less to the robot.

Control Callback

Implement the control_callback method in a way that it sent velocity message coherent with the presence of obstacles on the left and/or right side of the robot.

The move straight until an obstacle is found in a range of 1 meter in front of it (self.obstacle_left or self.obstacle_right is True).

The robot rotate with a positive or negative value depending on which of the obstacle variable is true.

For security reason, the control should not send velocity messages while scan messages were not received and should stop if scan message stop.

Smart Control

At this point, the control is defined on \(4\) states, with on/off velocity control.

The states are :

state	action
no scan	empty
no obstacle	go straight
left obstacle	turn right
right obstacle	turn left
both obstacle	turn left

As a side effect, the robot behavior is not really efficient. First we can increase the state definition to handle more possibilities.

For instance, if the robot faces a both obstacle situation while it is turning right (i.e. avoiding a left obstacle ) it would be more interesting to continue to turn on the same direction. For that the robot state should remenber the last performed action.

Another example would be to differentiate close and distant obstacles to apply different control velocities (move and turn in a same time while possible), and generate fluid trajectories.

Finally, the desired speed can be reached with several steps, to force a soft acceleration.