Exploring ROS2 with a wheeled robotic – #4 – Impediment avoidance

By Marco Arruda

On this submit you’ll discover ways to program a robotic to keep away from obstacles utilizing ROS2 and C++. As much as the tip of the submit, the Dolly robotic strikes autonomously in a scene with many obstacles, simulated utilizing Gazebo 11.

You’ll be taught:

• The way to publish AND subscribe subjects in the identical ROS2 Node
• The way to keep away from obstacles
• The way to implement your personal algorithm in ROS2 and C++

1 – Setup atmosphere – Launch simulation

Earlier than anything, be sure to have the rosject from the earlier submit, you’ll be able to copy it from right here.

Launch the simulation in a single webshell and in a unique tab, checkout the subjects we’ve accessible. You have to get one thing much like the picture under:

2 – Create the node

With the intention to have our impediment avoidance algorithm, let’s create a brand new executable within the file ~/ros2_ws/src/my_package/obstacle_avoidance.cpp:

#embrace "geometry_msgs/msg/twist.hpp"    // Twist
#embrace "rclcpp/rclcpp.hpp"              // ROS Core Libraries
#embrace "sensor_msgs/msg/laser_scan.hpp" // Laser Scan

utilizing std::placeholders::_1;

class ObstacleAvoidance : public rclcpp::Node {
public:
  ObstacleAvoidance() : Node("ObstacleAvoidance") {

    auto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());
    subscription_ = this->create_subscription(
        "laser_scan", default_qos,
        std::bind(&ObstacleAvoidance::topic_callback, this, _1));
    publisher_ =
        this->create_publisher("cmd_vel", 10);
  }

non-public:
  void topic_callback(const sensor_msgs::msg::LaserScan::SharedPtr _msg) {
    // 200 readings, from proper to left, from -57 to 57 degress
    // calculate new velocity cmd
    float min = 10;
    for (int i = 0; i < 200; i++) { float present = _msg->ranges[i];
      if (present < min) { min = present; } } 
    auto message = this->calculateVelMsg(min);
    publisher_->publish(message);
  }
  geometry_msgs::msg::Twist calculateVelMsg(float distance) {
    auto msg = geometry_msgs::msg::Twist();
    // logic
    RCLCPP_INFO(this->get_logger(), "Distance is: '%f'", distance);
    if (distance < 1) {
      // flip round
      msg.linear.x = 0;
      msg.angular.z = 0.3;
    } else {
      // go straight forward
      msg.linear.x = 0.3;
      msg.angular.z = 0;
    }
    return msg;
  }
  rclcpp::Writer::SharedPtr publisher_;
  rclcpp::Subscription::SharedPtr subscription_;
};

int important(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared());
  rclcpp::shutdown();
  return 0;
}

Within the important perform we’ve:

• Initialize node rclcpp::init
• Hold it working rclcpp::spin

Inside the category constructor:

• Subcribe to the laser scan messages: subscription_
• Publish to the robotic diff driver: publisher_

The impediment avoidance intelligence goes inside the strategy calculateVelMsg. That is the place choices are made primarily based on the laser readings. Discover that’s relies upon purely on the minimal distance learn from the message.

If you wish to customise it, for instance, contemplate solely the readings in entrance of the robotic, and even examine whether it is higher to show left or proper, that is the place you might want to work on! Bear in mind to regulate the parameters, as a result of the way in which it’s, solely the minimal worth involves this methodology.

3 – Compile the node

This executable is dependent upon each geometry_msgs and sensor_msgs, that we’ve added within the two earlier posts of this sequence. Be sure you have them at first of the ~/ros2_ws/src/my_package/CMakeLists.txt file:

# discover dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)

And at last, add the executable and set up it:

# impediment avoidance
add_executable(obstacle_avoidance src/obstacle_avoidance.cpp)
ament_target_dependencies(obstacle_avoidance rclcpp sensor_msgs geometry_msgs)

...

set up(TARGETS
  reading_laser
  moving_robot
  obstacle_avoidance
  DESTINATION lib/${PROJECT_NAME}/
)

Compile the bundle:
colcon construct --symlink-install --packages-select my_package

4 – Run the node

With the intention to run, use the next command:
ros2 run my_package obstacle_avoidance

It is not going to work for this robotic! Why is that? We’re subscribing and publishing to generic subjects: cmd_vel and laser_scan.

We’d like a launch file to remap these subjects, let’s create one at ~/ros2_ws/src/my_package/launch/obstacle_avoidance.launch.py:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    obstacle_avoidance = Node(
        bundle="my_package",
        executable="obstacle_avoidance",
        output="display",
        remappings=[
            ('laser_scan', '/dolly/laser_scan'),
            ('cmd_vel', '/dolly/cmd_vel'),
        ]
    )

    return LaunchDescription([obstacle_avoidance])

Recompile the bundle, supply the workspace as soon as extra and launch it:
colcon construct --symlink-install --packages-select my_package
supply ~/ros2_ws/set up/setup.bash
ros2 launch my_package obstacle_avoidance.launch.py

Associated programs & additional hyperlinks:

The submit Exploring ROS2 with a wheeled robotic – #4 – Impediment Avoidance appeared first on The Assemble.