ROS 2 Humble 订阅/std::bind 功能不起作用

问题描述 投票：0回答：1

高级任务：使用 ROS2 Humble 创建飞行无人机的模拟。

当前任务：我正在尝试通过主题订阅者通信和直接函数调用相结合，通过高级飞行控制器来改变电机速度（节点主题图如下所示）：

只有标有红色的部分才是重要的。

Algorytm：发布命令“cmd_vel”，该命令将由 /flight_controller Velocity_subscriber 使用 -> Velocity_subscriber 绑定到 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_

飞行控制器.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);
    }

电机_控制器.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. 消息已发布，我通过终端中的命令检查了它：

    输入： ros2主题 echo /cmd_vel

    输出： 线性： x：0.0 y：0.0 坐标：100.0 角度： x：0.0 y：0.0 z：0.0

  2. 我的包内任何类型的主题订阅者通信都不起作用，尽管它们已初始化：我尝试直接向电机发布消息（绕过flight_controller），这是同样的故事，但在包外ros主题工作正常，这使得我认为“std::bind”不能正常工作。

如果您有任何线索如何解决/改进/使相同的任务更容易，请联系，谢谢！

c++ publish-subscribe ros2 gazebo-simu
1个回答
0
投票

这里有很多东西需要解开，所以我会尽量简短。

当节点收到某个主题的消息时，它不会自动调用回调函数。您必须创建一个执行器，它将在传入消息和事件上调用订阅、计时器、服务服务器、操作服务器等的回调。

有关执行器的更多信息，请参阅执行器文档。

由于 FlightController 和 Motor 类都继承自 rclcpp::Node，所以 FlightController 和 Motor 的所有实例化都将是不同的节点。所有这些节点都需要由执行器旋转，以便调用回调函数。旋转飞控不会自动旋转电机节点。

电机对象完全封装在 FlightController 类中，这使得无法从 FlightController 外部旋转这些电机。

这个问题有几种解决方案

1.将对象创建与对象使用分开

如果你

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。您可以轻松地将节点作为构造函数的参数

class Motor
{
public:
    Motor(std::shared_ptr<rclcpp::Node> node) : node_{node}
    {
        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_);
}

4.创建单独的可执行文件

您可以将电机控制功能提取到单独的可执行文件中。使用启动文件，您可以使用不同的参数轻松启动此可执行文件 4 次。

然后您可以创建飞行控制器可执行文件。

结论

我建议结合使用选项 1 和 2，因为选项 3 和 4 会产生大量计算开销，并且会减慢程序速度。

最新问题
© www.soinside.com 2019 - 2024. All rights reserved.