Although this code uses ros2 my concern is about C++ code quality, because I still struggle when it comes to making good design decisions with regards to single responsibility, dependency injection, and callbacks, etc. I tend to second-guess decisions due to lack of experience.
I am also always 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 already tested this code on the Jetson Nano and it is fully functional.
The wheel_encoder_sensor.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 wheel_encoder_publisher.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
