HIgh-level task: create simulation of flying drone with ROS2 Humble.

Current task: I am trying to change speed of motors via high level flight controller through combination of topic-subscriber communication and direct function calls (node-topic graph looks like this):

Only parts that mark with red is important.

Algorytm: publish command "cmd_vel" that mast be consumed by /flight_controller velocity_subscriber -> velocity_subscriber bind to control_motors function that mast directly change speed of motors

flight_controller.hpp

#ifndef FLIGHT_CONTROLLER_HPP_
#define FLIGHT_CONTROLLER_HPP_

#include <rclcpp/rclcpp.hpp>
#include "motor_controller.hpp"
#include <geometry_msgs/msg/Twist.hpp>

class FlightController : public rclcpp::Node
{
public:
    FlightController();

    void update();
    void control_motors(const geometry_msgs::msg::Twist::SharedPtr cmd_vel);

private:
    Motor motor_fl_;
    Motor motor_fr_;
    Motor motor_bl_;
    Motor motor_br_;

    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr velocity_subscription_;
};

#endif  // FLIGHT_CONTROLLER_HPP_

flight_controller.cpp

    #include "hrssd/flight_controller.hpp"
    
    FlightController::FlightController()
    : Node("flight_controller"),
      motor_fl_("fl", "motors"),
      motor_fr_("fr", "motors"),
      motor_bl_("bl", "motors"),
      motor_br_("br", "motors")
    {
        velocity_subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "cmd_vel", 10,
            std::bind(&FlightController::control_motors, this, std::placeholders::_1)); // <- HERE command that cousing problems
    }
    
    void FlightController::update()
    {
        // Implementation for updating flight status or performing periodic checks
    }
    
    void FlightController::control_motors(const geometry_msgs::msg::Twist::SharedPtr cmd_vel)
    {
        RCLCPP_INFO(this->get_logger(), "Flight Controller hhh");
        // Here you would convert the Twist message into individual motor speeds.
        // This is a simple proportional placeholder for demonstration purposes.
    
        double linear_x = cmd_vel->linear.x;
        double angular_z = cmd_vel->angular.z;
    
        // Assuming that a positive angular_z represents a clockwise rotation
        // and linear_x controls the forward/backward speed.
        double speed_fl = linear_x - angular_z;
        double speed_fr = linear_x + angular_z;
        double speed_bl = linear_x - angular_z;
        double speed_br = linear_x + angular_z;
    
        motor_fl_.set_speed(speed_fl);
        motor_fr_.set_speed(speed_fr);
        motor_bl_.set_speed(speed_bl);
        motor_br_.set_speed(speed_br);
    }

motor_controller.hpp

#ifndef MOTOR_HPP_
#define MOTOR_HPP_

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/Float64.hpp>

class Motor : public rclcpp::Node
{
public:
    Motor(const std::string & name, const std::string & namespace_); // namespace_ currently not used

    void set_speed(double speed);
    double get_speed() const;

private:
    void speed_callback(const std_msgs::msg::Float64::SharedPtr msg);

    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr speed_publisher_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr speed_subscription_;
    double current_speed_;
};

#endif  // MOTOR_HPP_

motor_controller.cpp

#include "hrssd/motor_controller.hpp"

Motor::Motor(const std::string & name, const std::string & namespace_)
: Node(name), current_speed_(0.0)
{
    // Initialize the publisher for motor speed
    std::string topic_name = name;

    speed_publisher_ = this->create_publisher<std_msgs::msg::Float64>(topic_name, 10);
}

void Motor::set_speed(double speed)
{
    // Update the current speed
    current_speed_ = speed;
    RCLCPP_INFO(this->get_logger(), "set_speed_motor: %f", speed);
    // Publish the new speed
    auto message = std_msgs::msg::Float64();
    message.data = current_speed_;
    speed_publisher_->publish(message);
}

double Motor::get_speed() const
{
    // Return the current speed
    return current_speed_;
}

void Motor::speed_callback(const std_msgs::msg::Float64::SharedPtr msg)
{
    RCLCPP_INFO(this->get_logger(), "speed_callback_motor: %f", msg->data);
    // Update the motor's speed based on the received command
    set_speed(msg->data);
}

i am publishing following command, expecting that control_motors function will be called but dont receive any feedback:

ros2 topic pub --rate 1 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 100.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

The things that i am sure of:

1. message is published, i checked it via command in terminal:

   input: ros2 topic echo /cmd_vel

   output: linear: x: 0.0 y: 0.0 z: 100.0 angular: x: 0.0 y: 0.0 z: 0.0

2. any type of topic-subscriber communication inside my package doesnt work, despite that they are initialized: i tried to publish messages to motor directly (bypass flight_controller) its the same story, but outside of the package ros topics working fine, that make me think that "std::bind" don't work properly.

If you have any clue how to solve/make better/make same task easier, please reach out, thank you!

There is a lot to unpack here, so i'll try to keep it short.

When a node receives a message on a topic, it does not automatically invoke the callback function. You have to create an executor, which will invoke the callbacks of subscriptions, timers, service servers, action servers, etc. on incoming messages and events.

See the Executor documentation for more info on executors.

Since both the FlightController and the Motor classes inherit from rclcpp::Node, all instantiations of FlightController and Motor will be different nodes. All of these nodes need to be spun by an executor, for the callback functions to be invoked. Spinning a flight controller object, does not automatically spin the motor nodes.

The motor objects are completely encapsulated within your FlightController class, which makes it impossible to spin these motors from outside the FlightController.

There are several solutions for this problem

1. Seperating object creation from object use

If you separate the creation of your motor objects from the use of your motor objects, you won't have any problem spinning the nodes. For example if you modify the FlightController to accept pointers to motor objects in it's constructor, like this.

class FlightController : public rclcpp::Node
{
public:
    FlightController(std::shared_ptr<Motor> motor_fl,
                     std::shared_ptr<Motor> motor_fr,
                     std::shared_ptr<Motor> motor_bl,
                     std::shared_ptr<Motor> motor_br)
        : motor_fl_{motor_fl}
        , motor_fr_{motor_fr}
        , motor_bl_{motor_bl}
        , motor_br_{motor_br}
    {
        // do other constructor stuff here.
    }

private:
    std::shared_ptr<Motor> motor_fl_;
    std::shared_ptr<Motor> motor_fr_;
    std::shared_ptr<Motor> motor_bl_;
    std::shared_ptr<Motor> motor_br_;
};

Then you can add all nodes to a single executor and spin the executor.

int main(int argc, char const* argv[])
{
    rclcpp::init(argc, argv);
    auto motor_fl = std::make_shared<Motor>(/* constructor args for motor */);
    auto motor_fr = std::make_shared<Motor>(/* constructor args for motor */);
    auto motor_bl = std::make_shared<Motor>(/* constructor args for motor */);
    auto motor_br = std::make_shared<Motor>(/* constructor args for motor */);
    auto flight_controller = std::make_shared<FlightController>(motor_fl, motor_fr, motor_bl, motor_br);

    rclcpp::executors::StaticSingleThreadedExecutor executor;
    executor.add_node(motor_fl);
    executor.add_node(motor_fr);
    executor.add_node(motor_br);
    executor.add_node(motor_bl);
    executor.add_node(flight_controller);
    executor.spin();

    rclcpp::shutdown();
}

2. Taking a node object as input for your constructor.

You are not obligated to inherit from rclcpp::Node. You could easily take a node as argument to a constructor. For example you could modify your Motor and FlightController classes as follows.

class Motor // Not inheriting from rclcpp::Node anymore
{
public:
    // Constructor now takes a pointer to a node object.
    Motor(std::shared_ptr<rclcpp::Node> node) : node_{node}
    {
        // Use the pointer to create publishers, subscribers, etc.
        speed_subscription_ = node_->create_subscribtion</*message type*/>(...);
    }

private:
    void speed_callback(const std_msgs::msg::Float64::SharedPtr msg);

    std::shared_ptr<rclcpp::Node> node_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr speed_subscription_;
};

class FlightController
{
public:
    FlightController(std::shared_ptr<rclcpp::Node> node)
        : motor_fl_{node}
        , motor_fr_{node}
        , motor_bl_{node}
        , motor_br_{node}
        , node_{node}
    {
        // do other constructor stuff here.
    }

private:
    std::shared_ptr<Motor> motor_fl_;
    std::shared_ptr<Motor> motor_fr_;
    std::shared_ptr<Motor> motor_bl_;
    std::shared_ptr<Motor> motor_br_;
    std::shared_ptr<rclcpp::Node> node_;
};

Then in your main loop you can create a single rclcpp::Node object and pass it to multiple classes like so

int main(int argc, char const* argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("node name");
    auto flight_controller = std::make_shared<FlightController>(node);
    rclcpp::spin(node)

    rclcpp::shutdown();
}

Note that this only creates a single node in stead of separate ones.

3. Spin the motor nodes inside your FlightController

Since your FlightController has an update method, i assume you are calling this method periodically from your main loop. You could extend the update method to spin the motor nodes at the end.

void FlightController::update()
{
    // Implementation for updating flight status or performing periodic checks

    rclcpp::spin_some(motor_fl_);
    rclcpp::spin_some(motor_fr_);
    rclcpp::spin_some(motor_bl_);
    rclcpp::spin_some(motor_br_);
}

For more info on spin_some see this post on the ROS forums.

4. Create separate executables

You could extract the motor control functionallity into a separate executable. Using launch files, you can easily start this executable 4 times with different parameters.

Then you can create a flight controller executable.

conclusion

I would recommend a combination of option 1 and 2, as option 3 and 4 generate a lot of computational overhead and will slow your program down.