Задача ВЫСОКОГО уровня: создать симуляцию летающего дрона с помощью 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}}"
В чем я уверен:
Сообщение опубликовано, я проверил его с помощью команды в терминале:
ввод: echo темы ros2 /cmd_vel
выход: линейный: х: 0,0 у: 0.0 г: 100,0 угловой: х: 0,0 у: 0.0 г: 0,0
любой тип связи тема-подписчик внутри моего пакета не работает, несмотря на то, что они инициализированы: я пытался публиковать сообщения непосредственно в моторе (в обход Flight_controller), та же история, но вне пакета темы ros работают нормально, это заставляет меня задуматься что «std::bind» работает неправильно.
Если у вас есть какие-либо идеи, как решить/улучшить/облегчить ту же задачу, свяжитесь с нами, спасибо!
Привет, @ALX23z. Благодарим за ваше предложение. 1) Я изменил имя подписки, как вы предложили - это не влияет на текущую проблему, но я уверен, что это поможет мне избежать проблем в будущем 2) QoS всех тем НЕОБХОДИМО, поэтому я не думаю, что это будет проблемой
Здесь есть что раскрыть, поэтому я постараюсь быть кратким.
Когда узел получает сообщение по теме, он не вызывает автоматически функцию обратного вызова. Вам необходимо создать исполнителя, который будет вызывать обратные вызовы подписок, таймеров, серверов служб, серверов действий и т. д. для входящих сообщений и событий.
Дополнительную информацию об исполнителях см. в документации по исполнителям.
Поскольку классы FlightController и Motor наследуют от rclcpp::Node, все экземпляры FlightController и Motor будут разными узлами. Все эти узлы должны быть запущены исполнителем для вызова функций обратного вызова. Вращение объекта контроллера полета не приводит к автоматическому вращению узлов двигателя.
Объекты двигателей полностью инкапсулированы внутри вашего класса FlightController, что делает невозможным вращение этих двигателей снаружи FlightController.
Есть несколько решений этой проблемы
Если вы отделите создание моторных объектов от их использования, у вас не возникнет проблем с вращением узлов. Например, если вы измените 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();
}
Вы не обязаны наследовать от 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();
}
Обратите внимание, что при этом создается только один узел вместо отдельных.
Поскольку ваш 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 раза с разными параметрами.
Затем вы можете создать исполняемый файл контроллера полета.
Я бы рекомендовал комбинацию вариантов 1 и 2, поскольку варианты 3 и 4 влекут за собой большие вычислительные затраты и замедляют работу вашей программы.
Привет @BadIsLife, спасибо за такой развернутый ответ! У меня есть палач в реализации «drone_node». Но я подозреваю, что он работает неправильно. Я прислушаюсь к вашему совету и перенесу всю инициализацию на палача. Но не могли бы вы уточнить еще один момент: почему вы считаете, что комбинация 1, 2 более эффективна, чем 3, 4? Создание узлов, абстрагированных от палача, это правда (я хотел скрыть инициализацию компонентов), но почему такой подход создаёт дополнительные накладные расходы?
Я считаю, что вам следует подписаться на «/cmd_vel», а не на «cmd_vel». В привязке нет ничего плохого. Другой проблемой может быть требование к отправке сообщений с качеством QoS, поскольку ros может фильтровать все сообщения из-за слишком высоких требований.