ROS 2 Скромные подписки/функция std::bind не работает

Задача ВЫСОКОГО уровня: создать симуляцию летающего дрона с помощью ROS2 Humble.

Текущая задача: я пытаюсь изменить скорость двигателей с помощью контроллера полета высокого уровня посредством сочетания связи между темой и подписчиком и прямых вызовов функций (график «узел-тема» выглядит следующим образом):

Важны только те части, которые отмечены красным.

Алгоритм: опубликовать команду «cmd_vel», которая должна использоваться /flight_controller Скорость_подписчика -> скорость_подписчика привязать к функции control_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_

мотор_контроллер.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);
}

я публикую следующую команду, ожидая, что будет вызвана функция control_motors, но не получаю никакой обратной связи:

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}}"

В чем я уверен:

  1. Сообщение опубликовано, я проверил его с помощью команды в терминале:

    ввод: echo темы ros2 /cmd_vel

    выход: линейный: х: 0,0 у: 0.0 г: 100,0 угловой: х: 0,0 у: 0.0 г: 0,0

  2. любой тип связи тема-подписчик внутри моего пакета не работает, несмотря на то, что они инициализированы: я пытался публиковать сообщения непосредственно в моторе (в обход Flight_controller), та же история, но вне пакета темы ros работают нормально, это заставляет меня задуматься что «std::bind» работает неправильно.

Если у вас есть какие-либо идеи, как решить/улучшить/облегчить ту же задачу, свяжитесь с нами, спасибо!

Я считаю, что вам следует подписаться на «/cmd_vel», а не на «cmd_vel». В привязке нет ничего плохого. Другой проблемой может быть требование к отправке сообщений с качеством QoS, поскольку ros может фильтровать все сообщения из-за слишком высоких требований.

ALX23z 07.04.2024 13:01

Привет, @ALX23z. Благодарим за ваше предложение. 1) Я изменил имя подписки, как вы предложили - это не влияет на текущую проблему, но я уверен, что это поможет мне избежать проблем в будущем 2) QoS всех тем НЕОБХОДИМО, поэтому я не думаю, что это будет проблемой

Виталий Аллахвердянц 11.04.2024 18:55
Стоит ли изучать PHP в 2023-2024 годах?
Стоит ли изучать PHP в 2023-2024 годах?
Привет всем, сегодня я хочу высказать свои соображения по поводу вопроса, который я уже много раз получал в своем сообществе: "Стоит ли изучать PHP в...
Поведение ключевого слова "this" в стрелочной функции в сравнении с нормальной функцией
Поведение ключевого слова "this" в стрелочной функции в сравнении с нормальной функцией
В JavaScript одним из самых запутанных понятий является поведение ключевого слова "this" в стрелочной и обычной функциях.
Приемы CSS-макетирования - floats и Flexbox
Приемы CSS-макетирования - floats и Flexbox
Здравствуйте, друзья-студенты! Готовы совершенствовать свои навыки веб-дизайна? Сегодня в нашем путешествии мы рассмотрим приемы CSS-верстки - в...
Тестирование функциональных ngrx-эффектов в Angular 16 с помощью Jest
В системе управления состояниями ngrx, совместимой с Angular 16, появились функциональные эффекты. Это здорово и делает код определенно легче для...
Концепция локализации и ее применение в приложениях React ⚡️
Концепция локализации и ее применение в приложениях React ⚡️
Локализация - это процесс адаптации приложения к различным языкам и культурным требованиям. Это позволяет пользователям получить опыт, соответствующий...
Пользовательский скаляр GraphQL
Пользовательский скаляр GraphQL
Листовые узлы системы типов GraphQL называются скалярами. Достигнув скалярного типа, невозможно спуститься дальше по иерархии типов. Скалярный тип...
2
2
118
1
Перейти к ответу Данный вопрос помечен как решенный

Ответы 1

Ответ принят как подходящий

Здесь есть что раскрыть, поэтому я постараюсь быть кратким.

Когда узел получает сообщение по теме, он не вызывает автоматически функцию обратного вызова. Вам необходимо создать исполнителя, который будет вызывать обратные вызовы подписок, таймеров, серверов служб, серверов действий и т. д. для входящих сообщений и событий.

Дополнительную информацию об исполнителях см. в документации по исполнителям.

Поскольку классы FlightController и Motor наследуют от rclcpp::Node, все экземпляры FlightController и Motor будут разными узлами. Все эти узлы должны быть запущены исполнителем для вызова функций обратного вызова. Вращение объекта контроллера полета не приводит к автоматическому вращению узлов двигателя.

Объекты двигателей полностью инкапсулированы внутри вашего класса FlightController, что делает невозможным вращение этих двигателей снаружи FlightController.

Есть несколько решений этой проблемы

1. Отделение создания объекта от использования объекта

Если вы отделите создание моторных объектов от их использования, у вас не возникнет проблем с вращением узлов. Например, если вы измените FlightController, чтобы он принимал указатели на объекты двигателей в его конструкторе, вот так.

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_;
};

Затем вы можете добавить все узлы к одному исполнителю и запустить его.

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. Использование объекта узла в качестве входных данных для вашего конструктора.

Вы не обязаны наследовать от rclcpp::Node. Вы можете легко использовать узел в качестве аргумента конструктора. Например, вы можете изменить классы Motor и FlightController следующим образом.

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_;
};

Затем в вашем основном цикле вы можете создать один объект rclcpp::Node и передать его нескольким классам, например:

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();
}

Обратите внимание, что при этом создается только один узел вместо отдельных.

3. Вращайте узлы двигателя внутри вашего FlightController.

Поскольку ваш FlightController имеет метод update, я предполагаю, что вы периодически вызываете этот метод из основного цикла. Вы можете расширить метод update, чтобы в конце вращать двигательные узлы.

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_);
}

Дополнительную информацию о spin_some смотрите в этом сообщении на форумах ROS.

4. Создайте отдельные исполняемые файлы.

Функциональность управления двигателем можно выделить в отдельный исполняемый файл. Используя файлы запуска, вы можете легко запустить этот исполняемый файл 4 раза с разными параметрами.

Затем вы можете создать исполняемый файл контроллера полета.

заключение

Я бы рекомендовал комбинацию вариантов 1 и 2, поскольку варианты 3 и 4 влекут за собой большие вычислительные затраты и замедляют работу вашей программы.

Привет @BadIsLife, спасибо за такой развернутый ответ! У меня есть палач в реализации «drone_node». Но я подозреваю, что он работает неправильно. Я прислушаюсь к вашему совету и перенесу всю инициализацию на палача. Но не могли бы вы уточнить еще один момент: почему вы считаете, что комбинация 1, 2 более эффективна, чем 3, 4? Создание узлов, абстрагированных от палача, это правда (я хотел скрыть инициализацию компонентов), но почему такой подход создаёт дополнительные накладные расходы?

Виталий Аллахвердянц 20.04.2024 08:06

Другие вопросы по теме