mkdir -p ~/<your-ros-learning-folder>/ws

mkdir -p ~/<your-ros-learning-folder>/ws/src
cd ~/<your-ros-learning-folder>/ws/src   # 外层文件夹叫什么无所谓，但要保证当前你所在的是 src 文件夹

ros2 pkg create pub_sub_cmd_vel --license MIT --build-type ament_cmake --dependencies rclcpp geometry_msgs tf2_ros tf2_geometry_msgs

add_executable(pub_sub_cmd_vel src/pub_sub_cmd_vel.cpp)
ament_target_dependencies(pub_sub_cmd_vel rclcpp geometry_msgs tf2_ros tf2_geometry_msgs)

colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON

#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/rclcpp.hpp>

class VelocityPublisher : public rclcpp::Node
{
public:
  VelocityPublisher() : Node("velocity_publisher")
  {
    // 创建一个发布器，发布 geometry_msgs::msg::Twist 类型的消息到 cmd_vel 话题
    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);

    // 创建一个定时器，每隔 1000 毫秒调用一次 publish_velocity() 函数
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(1000), std::bind(&VelocityPublisher::publish_velocity, this));
  }

private:
  void publish_velocity()
  {
    // 创建并初始化 Twist 消息
    auto message = geometry_msgs::msg::Twist();
    message.linear.x = velocity_.linear.x;
    message.angular.z = velocity_.angular.z;

    // 打印并发布消息
    RCLCPP_INFO(
      this->get_logger(), "Publishing: linear.x: '%f', angular.z: '%f'", message.linear.x,
      message.angular.z);
    publisher_->publish(message);

    // 更新线速度和角速度
    update_velocity();
  }

  void update_velocity()
  {
    // 更新线速度和角速度
    velocity_.linear.x += 0.2;
    velocity_.angular.z += 0.2;

    // 如果速度超过1，则重置为-1
    if (velocity_.linear.x > 1.0) {
      velocity_.linear.x = -1.0;
    }
    if (velocity_.angular.z > 1.0) {
      velocity_.angular.z = -1.0;
    }
  }

  // 发布器，用于发布 Twist 消息
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;

  // 定时器，用于定时调用 publish_velocity() 函数
  rclcpp::TimerBase::SharedPtr timer_;

  // 速度
  geometry_msgs::msg::Twist velocity_;
};

int main(int argc, char * argv[])
{
  // initializes ROS 2
  rclcpp::init(argc, argv);

  // Starts processing data from the node, including callbacks from the timer
  rclcpp::spin(std::make_shared<VelocityPublisher>());

  rclcpp::shutdown();
  return 0;
}

cmake_minimum_required(VERSION 3.8)
project(pub_sub_cmd_vel)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)

add_executable(pub_cmd_vel src/pub_cmd_vel.cpp)  # 这里的文件夹名称需要修该
ament_target_dependencies(pub_cmd_vel rclcpp geometry_msgs)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  set(ament_cmake_copyright_FOUND TRUE)
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

install(TARGETS
  pub_cmd_vel
  DESTINATION lib/${PROJECT_NAME})

ament_package()