Exploring ROS2 using wheeled Robot – #3 – Moving the robot

On Nov 30, 2021

By Marco Arruda

In this post you’ll learn how to publish to a ROS2 topic using ROS2 C++. Up to the end of the video, we are moving the robot Dolly robot, simulated using Gazebo 11.

You’ll learn:

How to create a node with ROS2 and C++
How to public to a topic with ROS2 and C++

1 – Setup environment – Launch simulation

Before anything else, make sure you have the rosject from the previous post, you can copy it from here.

Launch the simulation in one webshell and in a different tab, checkout the topics we have available. You must get something similar to the image below:

2 – Create a topic publisher

Create a new file to container the publisher node: moving_robot.cpp and paste the following content:

#include <chrono>
#include <functional>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"

using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class MovingRobot : public rclcpp::Node {
public:
  MovingRobot() : Node("moving_robot"), count_(0) {
    publisher_ =
        this->create_publisher("/dolly/cmd_vel", 10);
    timer_ = this->create_wall_timer(
        500ms, std::bind(&MovingRobot::timer_callback, this));
  }

private:
  void timer_callback() {
    auto message = geometry_msgs::msg::Twist();
    message.linear.x = 0.5;
    message.angular.z = 0.3;
    RCLCPP_INFO(this->get_logger(), "Publishing: '%f.2' and %f.2",
                message.linear.x, message.angular.z);
    publisher_->publish(message);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher::SharedPtr publisher_;
  size_t count_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared());
  rclcpp::shutdown();
  return 0;
}QoS (Quality of Service)

Similar to the subscriber it is created a class that inherits Node. A publisher_ is setup and also a callback, although this time is not a callback that receives messages, but a timer_callback called in a frequency defined by the timer_ variable. This callback is used to publish messages to the robot.

The create_publisher method needs two arguments:

topic name
QoS (Quality of Service) – This is the policy of data saved in the queue. You can make use of different middlewares or even use some provided by default. We are just setting up a queue of 10. By default, it keeps the last 10 messages sent to the topic.

The message published must be created using the class imported:

message = geometry_msgs::msg::Twist();

We ensure the callback methods on the subscribers side will always recognize the message. This is the way it has to be published by using the publisher method publish.

3 – Compile and run the node

In order to compile we need to adjust some things in the ~/ros2_ws/src/my_package/CMakeLists.txt. So add the following to the file:

Add the geometry_msgs dependency
Append the executable moving_robot
Add install instruction for moving_robot

find_package(geometry_msgs REQUIRED)
...
# moving robot
add_executable(moving_robot src/moving_robot.cpp)
ament_target_dependencies(moving_robot rclcpp geometry_msgs)
...
install(TARGETS
  moving_robot
  reading_laser
  DESTINATION lib/${PROJECT_NAME}/
)

We can run the node like below:

source ~/ros2_ws/install/setup.bash ros2 run my_package