用ros2和c++写运动代码

在ROS 2 (Robot Operating System) 中，使用C++编写运动控制代码通常涉及到几个关键组件：

1. **msg**: ROS 2 使用名为`message`的数据结构，比如`geometry_msgs/Twist`或`tf2_msgs/TransformStamped`等，用于表示机器人位置、速度以及姿态信息。

2. **actionlib** 或 **rclcpp_action**: 这些库提供了一种标准的方式来实现服务（Service）或动作（Action），允许发送和接收包含复杂操作的消息。例如，你可以创建一个`MoveBaseAction`的动作服务器，处理移动机器人的请求。

3. **rclcpp_node**: 创建一个RCLcpp节点（Node）来运行你的C++程序，并通过消息传递系统与其他节点通信。

4. **控制器（Controller）**：编写函数来解析收到的命令，计算所需的关节角度或者其他控制变量，这通常是基于PID控制器或其他算法。

#include <rclcpp/rclcpp.hpp>
#include "geometry_msgs/msg/twist.h"
#include "your_custom_action_node.hpp"

class MoveController : public rclcpp::Node
{
public:
  explicit MoveController(rclcpp::node::NodeOptions options)
      : Node("move_controller", options),
        cmd_sub_(this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10,
          [this](const geometry_msgs::msg::Twist::SharedPtr msg) {
            // 在这里处理cmd_vel消息，如更新运动计划并控制运动
            this->get_logger().info("Received Twist message: %s", msg->toString().c_str());
            move_base(msg);
          })),
        status_pub_(this->create_publisher<rclcpp_components_action_msgs::msg::ExecutionFeedback>("feedback", 10))
  {}

private:
  void move_base(const geometry_msgs::msg::Twist::SharedPtr twist_msg)
  {
    // 根据twist_msg计算并执行动作，然后发布反馈
    auto feedback = your_custom_feedback_message(twist_msg);
    status_pub_.publish(feedback);
  }

  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
  rclcpp::Publisher<rclcpp_components_action_msgs::msg::ExecutionFeedback>::SharedPtr status_pub_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = MoveController::make_shared();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}