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 study:

• Easy methods to publish AND subscribe subjects in the identical ROS2 Node
• Easy methods to keep away from obstacles
• Easy methods to implement your individual algorithm in ROS2 and C++

1 – Setup surroundings – Launch simulation

Earlier than anything, ensure you have the rosject from the earlier submit, you may copy it from right here.

Launch the simulation in a single webshell and in a special tab, checkout the subjects we have now obtainable. You will need to get one thing just like the picture under:

2 – Create the node

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

#embody "geometry_msgs/msg/twist.hpp"    // Twist
#embody "rclcpp/rclcpp.hpp"              // ROS Core Libraries
#embody "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);
  }

personal:
  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 primary(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared());
  rclcpp::shutdown();
  return 0;
}

Within the primary perform we have now:

• Initialize node rclcpp::init
• Preserve it operating 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 based mostly 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, think about solely the readings in entrance of the robotic, and even test whether it is higher to show left or proper, that is the place that you must work on! Keep 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 depends upon each geometry_msgs and sensor_msgs, that we have now added within the two earlier posts of this sequence. Be sure you have them originally 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 package deal:
colcon construct --symlink-install --packages-select my_package

4 – Run the node

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

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

We want 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(
        package deal="my_package",
        executable="obstacle_avoidance",
        output="display",
        remappings=[
            ('laser_scan', '/dolly/laser_scan'),
            ('cmd_vel', '/dolly/cmd_vel'),
        ]
    )

    return LaunchDescription([obstacle_avoidance])

Recompile the package deal, 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 & further hyperlinks:

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