速度消息包格式

对机器人的速度控制是通过向机器人的核心节点发送速度消息来实现的，这个消息的类型在ROS里已经有了定义，就是geometry_msgs::Twist。

机器人运动控制的实现（速度的发布节点与速度的订阅节点）

本实例编程实现一个发布者节点，如图所示，向"/cmd_vel"话题发布geometry_msgs::Twist类型的消息包。以此来实现对机器人运动速度的控制。

实战：速度的发布节点

我们建议任意一个功能，都用package装起来。

cd ~/ros2_ws/src
ros2 pkg create vel_pkg

创建好后，vscode能看到三个pkg，进入vel_pkg 在src创建vel_node.cpp

复制：

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

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("velocity_command_node");
  auto vel_pub = node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
  geometry_msgs::msg::Twist vel_msg;
  vel_msg.linear.x = 0.1;
  vel_msg.linear.y = 0.0;
  vel_msg.linear.z = 0.0;
  vel_msg.angular.x = 0.0;
  vel_msg.angular.y = 0.0;
  vel_msg.angular.z = 0.0;

  rclcpp::Rate loop_rate(30);
  while (rclcpp::ok())
  {
    vel_pub->publish(vel_msg);
    loop_rate.sleep();
  }

  rclcpp::shutdown();
  return 0;
}

整个流程再过一遍：

我们类比你用微信在班群里发消息。

打开微信->创建昵称[确定自己名字]->点击班群[确定往哪个群发，最多缓存多少]->写消息->设置发送频率->开始发送[publish(message)]->关闭打字窗

先初始化->创建节点[确定Node名字]->创建发布者[确定往哪个群聊发，最多缓存多少]->写消息->设置发送频率->开始发送[publish(message)]->关闭ROS2上下文

只是写消息消息的内容比较复杂，是三个角速度，三个线速度

  geometry_msgs::msg::Twist vel_msg;
  vel_msg.linear.x = 0.1;
  vel_msg.linear.y = 0.0;
  vel_msg.linear.z = 0.0;
  vel_msg.angular.x = 0.0;
  vel_msg.angular.y = 0.0;
  vel_msg.angular.z = 0.0;

修改MakeList

在第一个实验中我们已经了解，所有的include都需要在MakeList中的find_package添加。

引入了geometry_msgs::Twist，因此要

find_package(rclcpp REQUIRED) 
find_package(geometry_msgs REQUIRED)

同时要添加这个ament_target，才能自动链接这两个库。

这两步就是先找到库在哪里，再链接上用到这两个库的文件。

ament_target_dependencies(vel_node "rclcpp" "geometry_msgs")

然后需要创建该节点的可执行文件exe，命名为publisher_node，引用src下的publisher_node.cpp

add_executable(vel_node src/vel_node.cpp)

接着要把上一步这个可执行文件（节点Node）安装到指定文件夹（package）下。这个文件夹ros2是能够访问的，这样才能通过ros2去启动他。

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

完整Cmakelist

cmake_minimum_required(VERSION 3.8)
project(vel_pkg)

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)

# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_executable(vel_node src/vel_node.cpp)
ament_target_dependencies(vel_node "rclcpp" "geometry_msgs")


if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

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


ament_package()

修改package.xml

还记得第一个示例的“注意”吧？

CMakeLists.txt 里的 find_package 是给编译器看的；

package.xml 里的 <depend> 是给包管理器（rosdep）和构建系统（colcon）看的。两者缺一不可，必须保持同步。

  <depend>rclcpp</depend>
  <depend>geometry_msgs</depend>

完整package.xml：

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>vel_pkg</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="unicorn@todo.todo">unicorn</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>geometry_msgs</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

在ros2_ws colcon build一下

colcon build

然后source一下

source install/setup.bash

执行

ros2 run vel_pkg vel_node

同第一个例程，你不会看到任何消息，因为发到了“cmd_vel”话题中

再开一个终端（ctrl+Alt+T）

搜索所有话题

ros2 topic list

监听cmd_vel

ros2 topic echo /cmd_vel

实战：速度的订阅节点

接下来用wpr节点调用一个仿真被控端

wpr是一个机器人仿真节点，作为订阅者订阅cmd_vel接收速度消息

这是humble（2204）版本的：

sudo apt install git
cd ~/ros2_ws/src/
git clone https://github.com/6-robot/wpr_simulation2.git
cd wpr_simulation2/scripts/
./install_for_humble.sh
cd ~/ros2_ws/
colcon build
source install/setup.bash
ros2 launch wpr_simulation2 wpb_simple.launch.py

由于星马用的2404，没有humble，因此要重下一些包：

sudo apt update
sudo apt install -y \
  ros-jazzy-ros-gz \
  ros-jazzy-nav2-msgs \
  ros-jazzy-pcl-ros \
  ros-jazzy-pcl-conversions \
  ros-jazzy-cv-bridge \
  ros-jazzy-image-transport \
  ros-jazzy-tf2 \
  ros-jazzy-tf2-geometry-msgs \
  ros-jazzy-visualization-msgs \
  ros-jazzy-nav-msgs \
  ros-jazzy-sensor-msgs \
  ros-jazzy-geometry-msgs \
  ros-jazzy-rclcpp \
  ros-jazzy-rclpy \
  ros-jazzy-rclcpp-action \
  ros-jazzy-rosidl-default-generators \
  ros-jazzy-ament-cmake \
  ros-jazzy-ament-cmake-python \
  ros-jazzy-std-msgs \
  libopencv-dev \
  libpcl-dev

花了很长时间：

日常：

cd ros2_ws
colcon build

source install/setup.bash
ros2 launch wpr_simulation2 wpb_simple.launch.py

然后启动速度发布节点：

新开一个终端（ctrl+Alt+T）

cd ros2_ws
source install/setup.bash
ros2 run vel_pkg vel_node

可以看到机器人动起来了