Robotics StackExchange | Archived questions

Clean Code Review: An NVIDIA Jetson Nano GPIO Wheel Encoder Message Publisher using ROS2

Although I am not entirely new to ROS nor C++ I still struggle when it comes to making good design decisions with regards to single responsibility, dependency injection, and callbacks. Also worried about which objects to use in callbacks in terms of race conditions (which in this case is not really applicable but it's something I worry about getting wrong).

The below code example is very simple. There's a wheel encoder connected to the GPIO pin on an NVIDIA Jetson Nano board, and a ROS2 Node that publishes the encoder. I've tested this code on the Jetson Nano and it is functional.

I already tested this code on the Jetson Nano and it is fully functional.

The wheelencodersensor.h file

#pragma once

#include <iostream>
#include <memory>
#include <functional>
#include <JetsonGPIO.h>

namespace xiaoduckie {
namespace sensors {

/*!
 * @brief This class manages the WheelEncoder sensor connected to the GPIO
 * pins in the NVIDIA Jetson Nano Board
 * @ingroup Sensors
*/
class WheelEncoderSensor {

  /*!
    * @brief ENUM that represents the direction of motion for the robot.
    * This value is updated when the controller makes a decision about
    * direction.
    * The value is used to update the global tick count on the encoder.
    * @ingroup Sensors
    */
  enum Direction {
    FORWARD = 1,
    BACKWARD = -1
  };

  /*!
    * @brief This class is used by the Jetson GPIO c++ library as a callback
    * for the event detect on a GPIO pin.
    * We cannot use a regular lambda bcause this Jetson GPIO library requires
    * the callbacks must be equaility-comparable.
    * More details: https://github.com/pjueon/JetsonGPIO/issues/55#issuecomment-1059888835
    * @ingroup Sensors
    */
  class EncoderCallback
  {
  public:
    /*!
      * @brief Default constructor.
      * @param name Reference to the name assigned to this callback.
      * @param ticks Pointer referring to total ticks in the WheelEncoderSensor.
      * @param callback The external callback passed to WheelEncoderSensor.
      * @param direction Pointer to the DIRECTION enum in WheelEncoderSensor.
      */
    EncoderCallback(
      const std::string& name,
      std::shared_ptr<int> ticks,
      std::function<void(int)> callback,
      std::shared_ptr<WheelEncoderSensor::Direction> direction)
      : name(name),
      callback_(std::move(callback)),
      ticks_(ticks),
      direction_(direction)
    {};

    /*!
      * @brief Default copy constructor
      */
    EncoderCallback(const EncoderCallback&) = default; // Copy-constructible

    /*!
      * @brief Callable executed by the GPIO library upon event detection.
      * Used to update the global tick count and execute the callback provided
      * to the Encoder through its constructor.
      * @param channel Reference to the GPIO pin passed in by the library.
      */
    void operator()(const std::string& channel) // Callable with one string type argument
    {
      std::cout << "A callback named " << name;
      std::cout << " called from channel " << channel << std::endl;
      *ticks_ += *direction_;
      callback_(*ticks_);
    }

    /*!
      * @brief equality operator used by GPIO library to remove callbacks.
      * @param other Reference to the object being compared.
      */
    bool operator==(const EncoderCallback& other) const // Equality-comparable
    {
        return name == other.name;
    }

    /*!
      * @brief in-equality operator used by GPIO library to remove callbacks.
      * @param other Reference to the object being compared.
      */
    bool operator!=(const EncoderCallback& other) const 
    {
        return !(*this == other);
    }

  private:
    std::string name;
    std::function<void(int)> callback_;
    std::shared_ptr<int> ticks_ = std::make_shared<int>(0);
    std::shared_ptr<WheelEncoderSensor::Direction> direction_ = std::make_shared<WheelEncoderSensor::Direction>();
  };

  public:
    /*!
      * @brief Default constructor.
      */
    WheelEncoderSensor() = default;

    /*!
      * @brief The Init function sets the GPIO used by this encoder,
      * accepts an additional callback to use when the encoder is
      * updated.
      * It initializes the internal callback mechanism and passes it
      * the additional/external one.
      * @param gpio The GPIO pin to which the encoder is connected.
      * @param callback The external callback.
      */
    bool Init(int gpio, std::function<void(int)> callback) {
      gpio_pin_ = gpio;
      *direction_ = Direction::FORWARD;

      EncoderCallback callback_(
        "EncoderCallback",
        ticks_,
        std::move(callback),
        direction_
      );

      GPIO::setmode(GPIO::BCM);
      GPIO::setup(gpio_pin_, GPIO::IN);
      GPIO::add_event_detect(gpio_pin_, GPIO::RISING, callback_);

      initialized_ = true;
      return true;
    };
  private:
    int gpio_pin_;
    bool initialized_ = false;
    std::shared_ptr<int> ticks_ = std::make_shared<int>(0);
    std::shared_ptr<WheelEncoderSensor::Direction> direction_ = std::make_shared<WheelEncoderSensor::Direction>();
};


} // namespace sensors
} // namespace xiaoduckie

The wheelencoderpublisher.h file

#pragma once

#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include <iostream>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

#include "wheel_encoder/wheel_encoder_sensor.h"

namespace xiaoduckie {
namespace sensors {

/*!
 * @brief This is the ROS2 Node that publishes the encoder messages.
 * It is constructed with an std::vector of WheelEncodersNode::WheelEncoderConfig
 * objects and creates an instance of a WheelEncoderSensor for each, then assigns
 * to each its own callback using a lambda function.  The lambda function publishes
 * the encoder value.
 * @ingroup Sensors
 */
class WheelEncodersNode : public rclcpp::Node {

  typedef rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr PublisherPtr;

  public:
    /*!
      * @brief This Struct holds the config for each WheelEncoderSensor
      * @ingroup Sensors
      */
    struct WheelEncoderConfig {
        int gpio_pin_;
        int resolution_;
        int publish_frequency_;
        int queue_limit;
        std::string orientation_;
        std::string topic_name_;
    };

    /*!
      * @brief Default constructor.
      * @param encoder_configs const Reference to the std::vector of WheelEncoderConfig.
      */
    WheelEncodersNode(const std::vector<WheelEncoderConfig>& encoder_configs)
    : Node("WheelEncodersNode"), encoder_configs_(encoder_configs)
    {
      int index = 0;
      for (auto const &config : encoder_configs_) {
        encoder_total_ticks_.push_back(0);

        PublisherPtr p = this->create_publisher<std_msgs::msg::Int32>(
          config.topic_name_,
          config.publish_frequency_);

        std::shared_ptr<WheelEncoderSensor> encoder = std::make_shared<WheelEncoderSensor>();

        encoder.get()->Init(
          config.gpio_pin_,
          [p, index, this] (int encoder_ticks) {

            auto message = std_msgs::msg::Int32();
            message.data = encoder_ticks;

            this->encoder_total_ticks_[index] = encoder_ticks;
            p->publish(message);
          }
        );

        encoders_.push_back(encoder);
        ++index;
      }
    };

  private:
    std::vector<int> encoder_total_ticks_;
    std::vector<WheelEncoderConfig> encoder_configs_;
    std::vector<std::shared_ptr<WheelEncoderSensor>> encoders_;
    std::vector<PublisherPtr> publishers_;
};

} // namespace sensors
} // namespace xiaoduckie

Asked by sameh4 on 2022-12-22 13:05:36 UTC

Comments

Answers