Some Code for Working in C++

Basic Example of a Node that Executes a Task over a Sequence of Steps

This code provides a specific way to architect code that executes a task consisting of a sequence of steps (in this case driving a square). This can be improved in many ways, so let me know what you think could be done to make it cleaner.

This code is also in the Github repo on class_activities_and_resources.

Source: drive_square.cpp

/*
 * A reimplementation of the draw_square with e stop Python code
 */

#include <cstdio>

#include <atomic>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/bool.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <math.h>

/// The Node that encapsulates our main functionality 
class DrawSquare : public rclcpp::Node {
  public:
    DrawSquare()
  : Node("draw_square_cplusplus"), e_stop(false)
  {
    publisher = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
    estop_subscription = this->create_subscription<std_msgs::msg::Bool>(
      "estop",
      10,
      std::bind(
        &DrawSquare::handle_estop,
        this,
        std::placeholders::_1));
  }

  /**
   * The callback function to handle incoming messages on the estop topic 
   * 
   * @param msg the message that contains the possible e stop directive
   */
  void handle_estop(const std_msgs::msg::Bool::SharedPtr msg) {
    if (msg->data) {
      e_stop = true;
      drive(0.0, 0.0);
    }
  }

  /// This is the main logic for piloting the square
  void run_loop() {
    // TODO: properly support sim time
    // the first message is often dropped
    drive(0.0, 0.0);
    std::this_thread::sleep_for(std::chrono::seconds(1));
    for (int i = 0; i < 4; i++) {
      if (!e_stop) {
        std::cout << "driving forward" << std::endl;
        drive_forward(0.5);
      }
      if (!e_stop) {
        std::cout << "turning left" << std::endl;
        turn_left();
      }
    }
    std::cout << "done with run loop" << std::endl;
  }

  /// Turn left 90 degrees using a time-based strategy
  void turn_left() {
    float angular_vel = 0.3;
    if (!e_stop) {
      drive(0.0, angular_vel);
      std::this_thread::sleep_for(std::chrono::duration<float>(M_PI / angular_vel / 2.0));
      drive(0.0, 0.0);
    }
  }

  /**
   * Drive forward the specified (positive) distance
   * 
   * @param distance the distance in meters to drive (must be positive)
   */
  void drive_forward(float distance) {
    float forward_vel = 0.1;
    if (!e_stop) {
      drive(forward_vel, 0.0);
      std::this_thread::sleep_for(std::chrono::duration<float>(distance / forward_vel));
      drive(0.0, 0.0);
    }
  }

  /**
   * Set the robot's linear and angular velocity 
   * 
   * @param forward the linear velocity in meters per second 
   * @param angular the angular velocity in radians per second
   */
  void drive(float forward, float angular)
  {
    geometry_msgs::msg::Twist msg;
    msg.linear.x = forward;
    msg.angular.z = angular;
    publisher->publish(msg);
  }

  private:
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_subscription;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher;
    /// we declare e_stop atomic to enforce thread safety
    std::atomic<bool> e_stop;
};


int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<DrawSquare>();
  std::shared_future<void> script = std::async(&DrawSquare::run_loop, node);
  // spin_until_future_complete allows us to handle normal callbacks while also executing
  // an additional function on a separate thread
  rclcpp::spin_until_future_complete(node, script);
  rclcpp::shutdown();
  return 0;
}