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

{
  "version": "0.2.0",
  "configurations": [
    {
      "name": "ROS 2: Launch C++ Node",
      "type": "cppdbg",
      "request": "launch",
      "program": "${workspaceFolder}/install/${input:package}/lib/${input:package}/${input:node}",
      "args": [], 
      "stopAtEntry": false,
      "cwd": "${workspaceFolder}",
      "environment": [
        {
          "name": "AMENT_PREFIX_PATH",
          "value": "${workspaceFolder}/install/${input:package}:/opt/ros/foxy"
        },
        {
          "name": "LD_LIBRARY_PATH",
          "value": "${workspaceFolder}/install/${input:package}/lib:/opt/ros/foxy/lib"
        }
      ],
      "externalConsole": false,
      "MIMode": "gdb",
      "setupCommands": [
        {
          "description": "Enable pretty-printing for gdb",
          "text": "-enable-pretty-printing",
          "ignoreFailures": true
        }
      ],
      "preLaunchTask": "colcon: build selected package" 
    }
  ],
  "inputs": [
    {
      "id": "package",
      "type": "promptString",
      "description": "Enter the ROS 2 package name",
      "default": "lidar_detection"  //修改成你的ros2 package
    },
    {
      "id": "node",
      "type": "promptString",
      "description": "Enter the C++ node executable name",
      "default": "obstacle_detector_node"  //修改成你的C++节点可执行文件名
    }
  ]
}

{
  "version": "2.0.0",
  "tasks": [
    {
      "label": "colcon: build selected package",
      "type": "shell",
      "command": "colcon build --symlink-install --packages-select ${input:package} --cmake-args -DCMAKE_BUILD_TYPE=Debug",
      "options": {
        "cwd": "${workspaceFolder}"
      },
      "problemMatcher": []
    }
  ],
  "inputs": [
    {
      "id": "package",
      "type": "promptString",
      "description": "Enter the ROS 2 package name to build",
      "default": "lidar_detection"  // 修改成你的ros2 package
    }
  ]
}