ROS开发专栏---ROS2 机械臂应用入门（1）---JointState 消息解析与机械臂往复运动控制实验---适配Ubuntu 22.04

🎬 渡水无言 ：个人主页渡水无言

❄专栏传送门 ：《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》

❄专栏传送门 ：《freertos专栏》《STM32 HAL库专栏》《linux裸机开发专栏》

❄专栏传送门 ：《产品测评专栏》《Ai智能体专栏）《ROS开发专栏》

⭐️流水不争先，争的是滔滔不绝

📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生

| 省级优秀毕业生获得者 | csdn新星杯TOP1 | 半导纵横专栏博主 | 211在读研究生

在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连

前言

一、实验原理

[1.1、 ROS2里的机械臂消息包格式](#1.1、 ROS2里的机械臂消息包格式)

1.2、机械臂控制

1.3、控制话题

二、编写机械臂控制程序

[2.1、新建机械臂功能包 mani_pkg](#2.1、新建机械臂功能包 mani_pkg)

2.2、编写机械臂控制源码

[2.3、配置 CMakeLists.txt](#2.3、配置 CMakeLists.txt)

[2.4、配置 package.xml](#2.4、配置 package.xml)

前言

在机器人开发中，机械臂是实现抓取、搬运、操作任务的核心执行机构。ROS2 提供了标准的关节状态消息格式，可以快速实现机械臂各关节角度、位置控制。

本期博客我们从零实现机械臂基础控制， 掌握 JointState 消息格式、发布机械臂控制话题，让仿真机械臂在折叠姿态与展开姿态之间循环往复运动，为后续视觉物品抓取打下基础。

一、实验原理

1.1、 ROS2里的机械臂消息包格式

在ROS2中，用于描述机械臂状态的消息格式为：

cpp 复制代码

sensor_msgs::JointState

在官方Wiki页面，对它的格式定义,如下图所示：

具体含义如下：

std_msgs/Header header：消息时间戳、坐标系。

string\[\] name：关节名称数组。

float64\[\] position：关节位置数组。

float64\[\] velocity：关节速度数组。

float64\[\] effort：关节力矩数组。

上述的后四个成员都是数组，通常每个数组都包含相同个数的成员，这样可以保持一一对应的关系。比如name $0$ 的关节，它对应角度是position $0$ ，对应速度是velocity $0$ ，以此类推。如果某个项目没有数值，比如effort，可以让这个数组的成员个数为零。

1.2、机械臂控制

机器人的手臂，只需要两个关节控制量。一个是机械臂基座的升降高度，另一个就是手爪的指间宽度。如下所示，对应sensor_msgs::JointState消息包成员：

lift：机械臂基座升降高度（单位：m）。

gripper：手爪两指开合间距（单位：m）。

1.3、控制话题

机械臂控制发布话题：

cpp 复制代码

/wpb_home/mani_ctrl

只要向该话题发布 JointState 消息，即可控制 Gazebo 中机械臂运动。

二、编写机械臂控制程序

2.1、新建机械臂功能包 mani_pkg

进入工作空间 src 目录，创建专用功能包，打开终端，输入以下命令：

cpp 复制代码

cd ~/ros2_ws/src
ros2 pkg create mani_pkg

2.2、编写机械臂控制源码

在 mani_pkg/src 新建 mani_ctrl.cpp，完整代码如下所示：

cpp 复制代码

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joint_state.hpp"

std::shared_ptr<rclcpp::Node> node;

int main(int argc, char * argv[])
{
  // 初始化ROS2
  rclcpp::init(argc, argv);
  // 创建节点
  node = std::make_shared<rclcpp::Node>("mani_ctrl_node");
  
  // 创建发布器，发布机械臂控制话题
  auto mani_pub = node->create_publisher<sensor_msgs::msg::JointState>(
    "/wpb_home/mani_ctrl", 
    10
  );

  // 定义关节消息
  sensor_msgs::msg::JointState mani_msg;
  // 设置2个关节
  mani_msg.name.resize(2);
  mani_msg.name[0] = "lift";     // 机械臂升降关节
  mani_msg.name[1] = "gripper";  // 夹爪开合关节
  mani_msg.position.resize(2);

  // 设置循环频率 0.1Hz
  rclcpp::Rate loop_rate(0.1);

  while (rclcpp::ok()) 
  {
    // 姿态1：机械臂折叠，夹爪微张
    RCLCPP_WARN(node->get_logger(), "Pose 1: 机械臂折叠");
    mani_msg.position[0] = 0.0;
    mani_msg.position[1] = 0.01;
    mani_pub->publish(mani_msg);
    loop_rate.sleep();

    // 姿态2：机械臂升起展开，夹爪张开
    RCLCPP_WARN(node->get_logger(), "Pose 2: 机械臂升起展开");
    mani_msg.position[0] = 1.0;
    mani_msg.position[1] = 0.1;
    mani_pub->publish(mani_msg);
    loop_rate.sleep();
  }
  
  rclcpp::shutdown();
  return 0;
}

创建 JointState 消息，绑定 lift、gripper 两个关节；

循环切换两组位置参数；

定时发布控制指令，让机械臂往复运动。

2.3、配置 CMakeLists.txt

打开 CMakeLists.txt，追加节点编译配置，全部代码如下：

cpp 复制代码

cmake_minimum_required(VERSION 3.8)
project(mani_pkg)

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

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)

# 编译可执行文件
add_executable(mani_ctrl src/mani_ctrl.cpp)
ament_target_dependencies(mani_ctrl 
  rclcpp 
  sensor_msgs
)

# 安装节点
install(TARGETS  
  mani_ctrl
  DESTINATION  lib/${PROJECT_NAME}
)

ament_package()

2.4、配置 package.xml

新增机械臂控制的相关依赖，全部代码如下：

cpp 复制代码

<?xml version="1.0"?>
<package format="3">
  <name>mani_pkg</name>
  <version>0.0.0</version>
  <description>ROS2机械臂基础控制功能包</description>
  <maintainer email="robot@todo.com">robot</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
</package>

三、编译与仿真运行

3.1、编译功能包

打开终端，输入以下指令：

cpp 复制代码

cd ~/ros2_zice
colcon build --packages-select mani_pkg --parallel-workers 1
colcon build --packages-select wpr_simulation2 --parallel-workers 1
source install/setup.bash

3.2、运行过程

启动Gazebo仿真环境

启动 Gazebo，加载机器人与机械臂模型。打开终端，输入以下指令：

cpp 复制代码

cd ~/ros2_zice
source install/setup.bash
ros2 launch wpr_simulation2 wpb_mani.launch.py

如下图所示：

运行机械臂控制节点

新开终端，输入以下指令:

cpp 复制代码

cd ~/ros2_zice
source install/setup.bash
ros2 run mani_pkg mani_ctrl

节点运行后，终端循环打印两种姿态提示信息，同时 Gazebo 仿真窗口中可以清晰观察到机械臂运动规律：机械臂首先保持折叠收拢状态，手爪小幅张开；间隔一段时间后机械臂缓缓抬升、向前完全展开，手爪张开幅度变大；随后自动回落恢复折叠姿态，如此循环往复。如下图所示：

总结

本期实验实现了机械臂两种姿态自动循环切换。