从零开始基于ROS-Noetic使用gazebo操控真实机械臂（附完整运行代码）
一、前言
1.1 总体概述
本文从零开始介绍如何使用ROS来操控真实的机械臂，其中包括创建机械臂的xacro模型，为机械臂创建仿真环境以及路径规划环境，使用了包括rviz，gazebo的使用。在ROS上订阅关节数据节点来操控真实的机械臂。本文在文末有完整的开源代码，只要在~/arm_ws/src/arm-robot下，在次目录下运行./start.sh，就能直接实现所有功能。
1.2 硬件系统
本文采用的硬件包括：arduino mega2560，pca9685多舵机控制模块，USB转TTL调试模块，测试用的舵机或直接是自己做的机械臂，杜邦线若干。
1.3 控制系统
PC端上的ROS-Noetic，操作系统是Ubuntu20.04。调试用的有串口调试助手，在Ubuntu上安装的Arduino IDE。
二、环境的配置
2.1 使用前环境的配置
这部分的配置可以去参考作者写的一篇详细文章，里面详细写了从Ubuntu安装到ROS-Noetic的详细过程，里面也有Arduino的安装教程，本文就不详细叙述了。
基于树莓派ubuntu20.04的ros-noetic小车
要注意的是，作者那篇文章使用的是PC端虚拟机和树莓派的分布式通信，本文目前并未使用到树莓派，如果读者使用的是树莓派来驱动Arduino的话，也可以按照作者那篇文章的来设置。如果不是，就要在 3.3.1 修改.bashrc的那一步， 将ROS_MASTER_URI这个的IP地址改回你自己虚拟机Ubuntu上的IP地址，因为目前主机和从机都是你自己的虚拟机。
2.2 为机械臂创建一个工作空间
mkdir -p arm_ws/src这步会创建一个文件夹，进入到arm_ws里面后执行catkin_make,执行完之后就会多出两个文件夹，一个是build，一个是devel，src是我们的源代码目录，build是编译过程中的中间文件存放的目录，devel是开发环境目录，编译后的目标文件和开发环境变量。
2.3 创建URDF文件夹
在src目录下创建一个arm-robot来存放该项目相关的文件夹。
mkdir -p arm-robot/在arm-robot下创建一个用来放URDF文件。std_msgs rospy roscpp tf urdf xacro为该ros包的依赖项。
catkin_create_pkg marm_description std_msgs rospy roscpp tf urdf xacro 创建存放各个功能包的文件夹。
mkdir config launch meshes scripts urdfconfig：存放配置文件，通常用于参数设置，比如机器人模型的参数、传感器配置等。文件通常为 YAML 格式。
launch：存放启动文件（.launch），用于启动一个或多个节点及其参数。这些文件定义了如何配置和启动你的 ROS 系统。
meshes：存放三维模型文件，通常为 STL 或 DAE 格式，用于可视化和仿真中表示机器人的物理形状。
scripts：存放可执行的 Python 脚本或其他脚本文件。这些脚本可以是 ROS 节点，也可以是辅助工具。
urdf：存放机器人的 URDF 文件，描述机器人的结构和关节信息。URDF 文件定义了机器人的几何形状、关节类型、连杆等。
三、可视化操作步骤
3.1 编写arm.xacro文件
此步骤如果是会建模的读者，或是已经有建模文件的读者，就可以跳过此步骤了，由于读者对机械臂的仿真要求不是很高，所以这里就直接使用圆柱体来代替了。如果是有建模文件的同学，就直接用solidwork来导出ros的urdf文件，具体的操作步骤请读者自行去搜索。
小鱼ros一键安装已经在安装的过程中帮我们把必要的rviz等依赖都安装好了，所以我们不需要另外再去安装什么东西了。
在urdf文件夹下创建一个arm.xacro文件
sudo nano arm.xacro该文件的完整内容如下：
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Defining the colors used in this robot -->
<material name="Black">
<color rgba="0 0 0 1"/>
</material>
<material name="White">
<color rgba="1 1 1 1"/>
</material>
<material name="Blue">
<color rgba="0 0 1 1"/>
</material>
<material name="Red">
<color rgba="1 0 0 1"/>
</material>
<!-- Constants -->
<xacro:property name="M_PI" value="3.14159"/>
<!-- link1 properties -->
<xacro:property name="link1_width" value="0.03" />
<xacro:property name="link1_len" value="0.10" />
<!-- link2 properties -->
<xacro:property name="link2_width" value="0.03" />
<xacro:property name="link2_len" value="0.14" />
<!-- link3 properties -->
<xacro:property name="link3_width" value="0.03" />
<xacro:property name="link3_len" value="0.22" />
<!-- link4 properties -->
<xacro:property name="link4_width" value="0.025" />
<xacro:property name="link4_len" value="0.06" />
<!-- link5 properties -->
<xacro:property name="link5_width" value="0.03" />
<xacro:property name="link5_len" value="0.06" />
<!-- link6 properties -->
<xacro:property name="link6_width" value="0.04" />
<xacro:property name="link6_len" value="0.02" />
<!-- Left gripper -->
<xacro:property name="left_gripper_len" value="0.08" />
<xacro:property name="left_gripper_width" value="0.01" />
<xacro:property name="left_gripper_height" value="0.01" />
<!-- Right gripper -->
<xacro:property name="right_gripper_len" value="0.08" />
<xacro:property name="right_gripper_width" value="0.01" />
<xacro:property name="right_gripper_height" value="0.01" />
<!-- Gripper frame -->
<xacro:property name="grasp_frame_radius" value="0.001" />
<!-- Inertial matrix -->
<xacro:macro name="inertial_matrix" params="mass">
<inertial>
<mass value="${mass}" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0" />
</inertial>
</xacro:macro>
<!-- /// bottom_joint // -->
<joint name="bottom_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="base_link"/>
<child link="bottom_link"/>
</joint>
<link name="bottom_link">
<visual>
<origin xyz=" 0 0 -0.02" rpy="0 0 0"/>
<geometry>
<box size="1 1 0.02" />
</geometry>
<material name="Black" />
</visual>
<collision>
<origin xyz=" 0 0 -0.02" rpy="0 0 0"/>
<geometry>
<box size="1 1 0.02" />
</geometry>
</collision>
<xacro:inertial_matrix mass="500"/>
</link>
<!-- / BASE LINK // -->
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.04" />
</geometry>
<material name="White" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.04" />
</geometry>
</collision>
<!--<xacro:inertial_matrix mass="1"/>-->
</link>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0.02" rpy="0 ${M_PI/2} 0" />
<axis xyz="-1 0 0" />
<limit effort="300" velocity="1" lower="-2.96" upper="2.96"/>
<dynamics damping="50" friction="1"/>
</joint>
<!-- / LINK1 // -->
<link name="link1" >
<visual>
<origin xyz="-${link1_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link1_width}" length="${link1_len}"/>
</geometry>
<material name="Blue" />
</visual>
<collision>
<origin xyz="-${link1_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link1_width}" length="${link1_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="joint2" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="-${link1_len} 0 0.0" rpy="-${M_PI/2} 0 ${M_PI/2}" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-2.35" upper="2.35" />
<dynamics damping="50" friction="1"/>
</joint>
<!-- /// LINK2 // -->
<link name="link2" >
<visual>
<origin xyz="0 0 ${link2_len/2}" rpy="0 0 0" />
<geometry>
<cylinder radius="${link2_width}" length="${link2_len}"/>
</geometry>
<material name="White" />
</visual>
<collision>
<origin xyz="0 0 ${link2_len/2}" rpy="0 0 0" />
<geometry>
<cylinder radius="${link2_width}" length="${link2_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="joint3" type="revolute">
<parent link="link2"/>
<child link="link3"/>
<origin xyz="0 0 ${link2_len}" rpy="0 ${M_PI} 0" />
<axis xyz="-1 0 0" />
<limit effort="300" velocity="1" lower="-2.62" upper="2.62" />
<dynamics damping="50" friction="1"/>
</joint>
<!-- / LINK3 / -->
<link name="link3" >
<visual>
<origin xyz="0 0 -${link3_len/2}" rpy="0 0 0" />
<geometry>
<cylinder radius="${link3_width}" length="${link3_len}"/>
</geometry>
<material name="Blue" />
</visual>
<collision>
<origin xyz="0 0 -${link3_len/2}" rpy="0 0 0" />
<geometry>
<cylinder radius="${link3_width}" length="${link3_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="joint4" type="revolute">
<parent link="link3"/>
<child link="link4"/>
<origin xyz="0.0 0.0 -${link3_len}" rpy="0 ${M_PI/2} ${M_PI}" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-2.62" upper="2.62" />
<dynamics damping="50" friction="1"/>
</joint>
<!-- /// LINK4 -->
<link name="link4" >
<visual>
<origin xyz="${link4_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link4_width}" length="${link4_len}"/>
</geometry>
<material name="Black" />
</visual>
<collision>
<origin xyz="${link4_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link4_width}" length="${link4_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="joint5" type="revolute">
<parent link="link4"/>
<child link="link5"/>
<origin xyz="${link4_len} 0.0 0.0" rpy="0 ${M_PI/2} 0" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-2.62" upper="2.62" />
<dynamics damping="50" friction="1"/>
</joint>
<!-- // LINK5 / -->
<link name="link5">
<visual>
<origin xyz="0 0 ${link4_len/2}" rpy="0 0 0" />
<geometry>
<cylinder radius="${link5_width}" length="${link5_len}"/>
</geometry>
<material name="White" />
</visual>
<collision>
<origin xyz="0 0 ${link4_len/2} " rpy="0 0 0" />
<geometry>
<cylinder radius="${link5_width}" length="${link5_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="joint6" type="revolute">
<parent link="link5"/>
<child link="link6"/>
<origin xyz="0 0 ${link4_len}" rpy="${1.5*M_PI} -${M_PI/2} 0" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-6.28" upper="6.28" />
<dynamics damping="50" friction="1"/>
</joint>
<!-- LINK6 / -->
<link name="link6">
<visual>
<origin xyz="${link6_len/2} 0 0 " rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link6_width}" length="${link6_len}"/>
</geometry>
<material name="Blue" />
</visual>
<collision>
<origin xyz="${link6_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${link6_width}" length="${link6_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="finger_joint1" type="prismatic">
<parent link="link6"/>
<child link="gripper_finger_link1"/>
<origin xyz="0.0 0 0" />
<axis xyz="0 1 0" />
<limit effort="100" lower="0" upper="0.06" velocity="1.0"/>
<dynamics damping="50" friction="1"/>
</joint>
<!-- // gripper // -->
<!-- LEFT GRIPPER AFT LINK -->
<link name="gripper_finger_link1">
<visual>
<origin xyz="0.04 -0.03 0"/>
<geometry>
<box size="${left_gripper_len} ${left_gripper_width} ${left_gripper_height}" />
</geometry>
<material name="White" />
</visual>
<collision>
<origin xyz="0.04 -0.03 0"/>
<geometry>
<box size="${left_gripper_len} ${left_gripper_width} ${left_gripper_height}" />
</geometry>
<material name="White" />
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<joint name="finger_joint2" type="fixed">
<parent link="link6"/>
<child link="gripper_finger_link2"/>
<origin xyz="0.0 0 0" />
</joint>
<!-- RIGHT GRIPPER AFT LINK -->
<link name="gripper_finger_link2">
<visual>
<origin xyz="0.04 0.03 0"/>
<geometry>
<box size="${right_gripper_len} ${right_gripper_width} ${right_gripper_height}" />
</geometry>
<material name="White" />
</visual>
<collision>
<origin xyz="0.04 0.03 0"/>
<geometry>
<box size="${right_gripper_len} ${right_gripper_width} ${right_gripper_height}" />
</geometry>
<material name="White" />
</collision>
<xacro:inertial_matrix mass="1"/>
</link>
<!-- Grasping frame -->
<link name="grasping_frame"/>
<joint name="grasping_frame_joint" type="fixed">
<parent link="link6"/>
<child link="grasping_frame"/>
<origin xyz="0.08 0 0" rpy="0 0 0"/>
</joint>
<!-- / Gazebo // -->
<gazebo reference="bottom_link">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="base_link">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="link1">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="link2">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="link3">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="link4">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="link5">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="link6">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="gripper_finger_link1">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="gripper_finger_link2">
<material>Gazebo/White</material>
</gazebo>
<!-- Transmissions for ROS Control -->
<xacro:macro name="transmission_block" params="joint_name">
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${joint_name}">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<xacro:transmission_block joint_name="joint1"/>
<xacro:transmission_block joint_name="joint2"/>
<xacro:transmission_block joint_name="joint3"/>
<xacro:transmission_block joint_name="joint4"/>
<xacro:transmission_block joint_name="joint5"/>
<xacro:transmission_block joint_name="joint6"/>
<xacro:transmission_block joint_name="finger_joint1"/>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/arm</robotNamespace>
</plugin>
</gazebo>
</robot>接下来解析代码。
颜色定义，定义了不同的颜色，用于机器人部件的视觉表示。每种颜色都有一个名称和 RGBA 值。
链接属性，定义了不同链接的宽度和长度属性，便于在后续使用。
惯性矩阵宏，定义了一个宏，用于创建惯性矩阵，接受一个质量参数。这样可以在多个链接中复用这一部分代码。
链接定义，定义了链接（link）的视觉和碰撞特性，并设置了质量。每个链接都有视觉（visual）和碰撞（collision）部分。
关节定义，定义了一个可转动关节joint1，其父链接和子链接分别是base_link 和link。还定义了关节的原点、旋转轴、限制和动态特性。
type为revolute时，表示这是一个可转动的关节，type的类型有以下这几种：
revolute：旋转关节，允许围绕一个轴旋转。
prismatic：平移关节，允许沿一个轴移动。
fixed：固定关节，不允许相对运动。
floating：浮动关节，允许在三维空间中自由移动。
planar：平面关节，允许在一个平面内移动。那个lower和upper代表的是弧度制的0到Π（3.1415926），转换为角度大概就是0到180°，作者使用的是180°舵机，所以就使用了这个弧度作为限制。如果读者使用的是360°舵机，那请自行修改，计算的公式如下：
度 = 弧度 × 180/Π
geometry的就是你定义的几何体形状，几何体的类型有：
box：立方体，定义一个长方体的几何形状。
<box>
<size>x y z</size>
</box>
cylinder：圆柱体，定义一个圆柱的几何形状。
<cylinder>
<radius>r</radius>
<length>l</length>
</cylinder>
sphere：球体，定义一个球的几何形状。
<sphere>
<radius>r</radius>
</sphere>
mesh：网格，定义一个复杂形状的几何体，通常用于导入3D模型。
<mesh>
<uri>path/to/your/model.stl</uri>
</mesh>
夹爪定义，定义了一个用于抓取的链接，包括视觉和碰撞特性，表明它的形状和材料。
Gazebo 相关定义，为 Gazebo 中的链接定义了材料属性，用于在仿真中显示的外观。
传动系统宏，定义了传动系统的宏，接受关节名称作为参数，用于简化多个关节的传动系统定义。
ros_control 插件， 为 Gazebo 添加了 ROS 控制插件，允许 ROS 控制机器人并与 Gazebo 进行交互。
3.2 检查xacro格式
由于作者的xarco不能直接检查，只能将其转化为urdf文件后再进行检查，所以作者直接就写了一个脚本来执行这个过程，在读者写了xacro，如果想要检查一下自己写的是否有错误，也可以进行这个操作。
报错的内容如下
检查格式的脚本，先创建一个check.sh文件，sudo nano check.sh，将以下内容填进去。
#!/bin/bash
xacro arm.xacro > arm.urdf
check_urdf arm.urdf 如果出现下面的这个的话，就说明你的xacro文件没错了，下面的这些child是你的关节的父子类。
3.3 编写可视化执行启动文件
在这步会创建一个启动文件，会发布机械臂的关节状态，并使用rviz加载出来。在marm_description/launch目录下：
sudo nano view_arm.launch写入节点，在这个启动文件里，启动了一个gui节点，读者可以使用此gui，使用滑块来控制机械臂，在滑动机械臂运动之后，机械臂可能会一直不断的抽搐，也就是在不断的往返往返运动，这个是正常现象，无需担心。
<launch>
<arg name="model" />
<!-- 加载机器人模型参数 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find marm_description)/urdf/arm.xacro" />
<!-- 设置GUI参数，显示关节控制插件 -->
<!--<param name="use_gui" value="true"/>-->
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<!-- 运行robot_state_publisher节点，发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 运行rviz可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find marm_description)/urdf.rviz" required="true" />
</launch>随后在工作空间下执行
catkin_make
source devel/setup.bash
roslaunch marm_description view_arm.launch 
执行后的效果如下图：
四、Moveit运动规划
在弄完rviz之后，就可以开始弄moveit运动规划了。在终端输入
rosrun moveit_setup_assistant moveit_setup_assistant要注意：你的电脑网络最好是连你自己的热点，如果是连公网的话，你的主机请求可能会被防火墙拦截，导致启动不了moveit。
先启动ros节点
roscore再开另外一个终端来运行moveit配置（打开终端的快捷方式ctrl alt t）。
rosrun moveit_setup_assistant moveit_setup_assistant开始界面如下，新建一个Package，导入刚刚我们写的那个urdf文件， /home/pi/arm_ws/src/arm-robot/marm_description/urdf/arm.urdf
首先点Self-Collisions，点Generate Collision Matrix生成碰撞矩阵，用于自动生成和管理机器人各个部分之间的碰撞检测信息。使用默认的配置就行。
Virtual Joints本文是没有用到虚拟关节的，所以就不需要管他了。
然后是Planning Groups，规划组是将机器人的的关节或链接组合在一起，以便于进行运动规划和控制。点击Add Group,为组别起名字，机械臂运动是符合运动学规律的，所以要给机械臂添加kdl kinematics plugin/KDLKinematicsPlugin，机器人运动学计算。
点击Add Kin.chain，为机械臂添加和配置运动学链（kinematic chain）。运动学链是指机器人的关节和链接的集合，描述了从基座到末端执行器的运动关系。通过添加运动学链，用户可以明确机器人模型的各个部分如何相互连接和运动。配置运动学链后，MoveIt可以进行逆向运动学和正向运动学的计算，帮助确定关节位置。
再添加一个gripper组，这个组不需要添加运动学配置，这个组需要添加关节和链接。点击Add Joints，添加夹爪
保存完之后，再次点击gripper组的Links，添加链接。
Robot Pose是我们用来定义机械臂的姿态，用来表示当前机械臂的状态。Robot Pose用于表示机器人的当前状态，便于在运动规划和控制过程中跟踪机器人的位置和方向。这里作者就只定义一个初始化的姿态。读者可以根据自己的需求设置到想要的姿态。
然后是填写Author Information，这个一定要填，随便填也要填一个进去，不然他是会导出失败的。
最后是Configuration Files，我们先去arm-robot下创建一个存放Moveit配置文件的文件夹。
mkdir robot_moveit_config
在Configuration 中选择该保存路径。
如果生成成功应该会出现下面这些东西。
在launch文件夹下，会有一个示例文件demo.launch，我们可以试着启动他一下。
启动之后，就可以进行路径规划了。你可以通过移动那个球（Goal Pose）来将机械臂移动到指定的位置，也可以选择你之前在Moveit上设置的Goal State，弄完之后点Plan & Execute就会开始路径规划了。
这时候你再开启另外一个终端，用来获取此时的关节数据，首先先输入 rostopic list获取此时在运行的节点，发现有一个节点名字叫做/joint_states，这个就是驱动你机械臂运动的关节数据，我们可以看一下这个关节数据的内容
rostopic echo /joint_states 下面这些就是我们关节的数据，这个/joint_states提供的数据非常重要，在我们后续用gazebo来驱动真实的机械臂上起的至关重要的作用。
五、gazebo仿真
在moveit的配置文件当中，在launch中也有一个文件是叫做gazebo的文件的，我们将其运行一下就会进入到gazebo当中了。
source devel/setup.bash
roslaunch robot_moveit_config gazebo.launch 这个就是我们的gazebo界面了，接下来我们要试着来写一个逻辑控制节点来控制此机械臂。
在marm_description的script目录下创建一个simple_mover.py的运动控制节点，写入以下代码。
#!/usr/bin/env python
import math
import rospy
from std_msgs.msg import Float64
def mover():
# 发布每个关节的控制指令
pub_j1 = rospy.Publisher('/arm/joint1_position_controller/command', Float64, queue_size=10)
pub_j2 = rospy.Publisher('/arm/joint2_position_controller/command', Float64, queue_size=10)
pub_j3 = rospy.Publisher('/arm/joint3_position_controller/command', Float64, queue_size=10)
pub_j4 = rospy.Publisher('/arm/joint4_position_controller/command', Float64, queue_size=10)
pub_j5 = rospy.Publisher('/arm/joint5_position_controller/command', Float64, queue_size=10)
pub_j6 = rospy.Publisher('/arm/joint6_position_controller/command', Float64, queue_size=10)
rospy.init_node('arm_mover')
rate = rospy.Rate(10) # 10Hz 发布频率
start_time = 0
# 等待时间初始化
while not start_time:
start_time = rospy.Time.now().to_sec()
# 主循环
while not rospy.is_shutdown():
elapsed = rospy.Time.now().to_sec() - start_time
# 每个关节周期性移动，使用正弦函数
position = math.sin(2 * math.pi * 0.1 * elapsed) * (math.pi / 2)
pub_j1.publish(position)
pub_j2.publish(position)
pub_j3.publish(position)
pub_j4.publish(position)
pub_j5.publish(position)
pub_j6.publish(position)
rate.sleep()
if __name__ == '__main__':
try:
mover()
except rospy.ROSInterruptException:
pass
此代码通过sin函数，产生一个关节控制数据，通过/arm/joint1_position_controller/command节点来控制各个关节的运动。这个路径名字是取决于你在urdf中所定义的名字。
roslaunch marm_description gazebo.launch
rosrun marm_description simple_mover.py #打开另外一个终端这时候你就可以看见你的机械臂在一直不断的运动了，懂得了如何操控机械臂之后，接下来就是自己编写ros节点来控制机械臂运动了，控制机械臂运动可以用逻辑节点来控制，也就是你自己编写一个逻辑代码，使机械臂自主决策来运动，本文这里没有采用这种方式，而是采用一个gui界面来手动控制机械臂运动。
六、编写节点控制gazebo机械臂
在上面我们使用一个gui节点来控制rviz的机械臂运动，在gazebo中我们也可以采用一个gui节点调出这个界面，从而用这个界面来控制gazebo的机械臂。
我们首先先启动gazebo
source devel/setup.bash
roslaunch robot_moveit_config gazebo.launch在另外一个终端启动gui界面
source devel/setup.bash
rosrun joint_state_publisher_gui joint_state_publisher_gui这时候我们滑动这个滑块，是不是不能控制我们的gazebo，这是当然的，在上面作者也说了，gazebo的关节控制是通过/arm/joint1_position_controller/command节点来控制的，所以我们还需要一个转换节点来将/joint_states中的关节数据订阅，然后发布到command上。
在marm_description的script目录中创建一个joint_states_to_command.py文件，这个节点文件将帮助我们订阅/joint_states节点然后发布给command。
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64
# 定义关节的命令发布器
joint_pubs = []
def joint_states_callback(msg):
# 将收到的 joint_states 数据转换为各个关节的控制命令并发布
if len(msg.position) >= 6: # 确保有至少6个关节数据
joint_pubs[0].publish(msg.position[0]) # joint1
joint_pubs[1].publish(msg.position[1]) # joint2
joint_pubs[2].publish(msg.position[2]) # joint3
joint_pubs[3].publish(msg.position[3]) # joint4
joint_pubs[4].publish(msg.position[4]) # joint5
joint_pubs[5].publish(msg.position[5]) # joint6
def main():
global joint_pubs
rospy.init_node('joint_states_to_command', anonymous=True)
# 创建发布器，将关节位置发布到对应的控制器话题
joint_pubs = [
rospy.Publisher('/arm/joint1_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/arm/joint2_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/arm/joint3_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/arm/joint4_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/arm/joint5_position_controller/command', Float64, queue_size=10),
rospy.Publisher('/arm/joint6_position_controller/command', Float64, queue_size=10)
]
# 订阅 /arm/joint_states 话题
rospy.Subscriber("/joint_states", JointState, joint_states_callback)
# 保持节点运行
rospy.spin()
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass再另外开一个终端，将节点激活并开始运行。
source devel/setup.bash
rosrun marm_description joint_states_to_command.py这时候你再滑动gui的滑块就发现能够成功控制到机械臂的运作了。
七、ros与arduino通讯
弄了这么多软件方面的东西，终于也是回到了我们的硬件环节了，ros和arduino的通讯方式主要有两种，一种是直接用串口来通讯，但是直接用串口来通讯的话，处理的时候可能会比较麻烦，所以本文这里直接就选择了第二种方法，也就是直接使用rosserial来通讯，在使用这个之前，我们首先要在Ubuntu上安装rosserial的依赖。版本名字依据你自己的ros版本来更改。
sudo apt install ros-noetic-rosserial ros-noetic-rosserial-client ros-noetic-rosserial-arduino然后在arduino上，安装rosserial的库，如果你是和参考我的这篇文章安装的arduino的话，那你就完全跟着我的步骤走，但是步骤其实都差不多的。
点击左边导航栏的第三个库管理，然后搜索rosserial，点击安装。如果没有左边的导航栏的话，就在工具——>库管理，也是可以下载这个rosserial库的。
安装完毕之后，接下来我们就可以测试一下这个库了。
我们可以打开示例中的 hello world来测试我们的rosserial，如果有读者发现打不开，这是正常的，因为那个hello world的后缀名还是旧版的ide格式，我们现在的arduino已经是ino后缀了，所以我们只需要去arduino的库里面把那个示例的后缀名改一下就行了。
在arduino内点击文件——>首选项，在那里可以看见项目的文件夹地址，点击浏览，进入arduino的库。
选择Rosserial_Arduino_Library库，找到examples里面的hello world示例，将ide改成ino，再返回arduino，就发现这个示例能够打开了。如果有想看看其他示例的，也可以去一个个更改那个文件的格式。
如果有懒得去修改的，这里也粘贴出完整的测试代码。
/*
* rosserial Publisher Example
* Prints "hello world!"
*/
#include <ros.h>
#include <std_msgs/String.h>
ros::NodeHandle nh;
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
char hello[13] = "hello world!";
void setup()
{
nh.initNode();
nh.advertise(chatter);
}
void loop()
{
str_msg.data = hello;
chatter.publish( &str_msg );
nh.spinOnce();
delay(1000);
}
将你的arduino接到虚拟机上，把程序烧录进去，烧进去之后，按一下arduino的复位，返回Ros上，打开rosserial。USB根据你自己的实际来修改。在arduino上可以看见Arduino连接到了哪个USB上。
rosrun rosserial_python serial_node.py /dev/ttyUSB0如果运行之后发现没有权限操控ttyUSB0，那就要把ttyUSB0添加到dialout用户组当中。
sudo usermod -aG dialout pi #你自己的用户运行完上面的命令后，需要重新登录你的用户账户，以使组权限更改生效。你可以注销并重新登录，或者重启虚拟机。
重启后，通过以下命令来检查/dev/ttyUSB0的权限。
ls -l /dev/ttyUSB0输出中应该显示设备属于dialout组，并且拥有读取和写入权限（通常为crw-rw----）。
这时再运行一次rosrun rosserial_python serial_node.py /dev/ttyUSB0应该就没有报错了。
没报错之后我们就可以来订阅arduino发布的/chatter节点了。
rostopic echo /chatter这时候我们就可以看见来自arduino发布的消息了。
八、真实机械臂的驱动
解决完ros与arduino的通讯之后，接下来的问题就是如何驱动机械臂了，这里作者采用了PCA9685的模块来驱动机械臂，PCA9685是一款可以驱动多路舵机的模块，使用的是IIC通讯，在arduino上是有pca9685的相关库的。
在arduino上搜索Adafruit PwM Servo Driver Library，随后可以使用一个舵机来测试这个库。
测试代码如下，是在该库的servo示例上：
/***************************************************
This is an example for our Adafruit 16-channel PWM & Servo driver
Servo test - this will drive 8 servos, one after the other on the
first 8 pins of the PCA9685
Pick one up today in the adafruit shop!
------> http://www.adafruit.com/products/815
These drivers use I2C to communicate, 2 pins are required to
interface.
Adafruit invests time and resources providing this open source code,
please support Adafruit and open-source hardware by purchasing
products from Adafruit!
Written by Limor Fried/Ladyada for Adafruit Industries.
BSD license, all text above must be included in any redistribution
****************************************************/
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);
// Depending on your servo make, the pulse width min and max may vary, you
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define SERVOMIN 150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
// our servo # counter
uint8_t servonum = 0;
void setup() {
Serial.begin(9600);
Serial.println("8 channel Servo test!");
pwm.begin();
/*
* In theory the internal oscillator (clock) is 25MHz but it really isn't
* that precise. You can 'calibrate' this by tweaking this number until
* you get the PWM update frequency you're expecting!
* The int.osc. for the PCA9685 chip is a range between about 23-27MHz and
* is used for calculating things like writeMicroseconds()
* Analog servos run at ~50 Hz updates, It is importaint to use an
* oscilloscope in setting the int.osc frequency for the I2C PCA9685 chip.
* 1) Attach the oscilloscope to one of the PWM signal pins and ground on
* the I2C PCA9685 chip you are setting the value for.
* 2) Adjust setOscillatorFrequency() until the PWM update frequency is the
* expected value (50Hz for most ESCs)
* Setting the value here is specific to each individual I2C PCA9685 chip and
* affects the calculations for the PWM update frequency.
* Failure to correctly set the int.osc value will cause unexpected PWM results
*/
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
delay(10);
}
// You can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. It's not precise!
void setServoPulse(uint8_t n, double pulse) {
double pulselength;
pulselength = 1000000; // 1,000,000 us per second
pulselength /= SERVO_FREQ; // Analog servos run at ~60 Hz updates
Serial.print(pulselength); Serial.println(" us per period");
pulselength /= 4096; // 12 bits of resolution
Serial.print(pulselength); Serial.println(" us per bit");
pulse *= 1000000; // convert input seconds to us
pulse /= pulselength;
Serial.println(pulse);
pwm.setPWM(n, 0, pulse);
}
void loop() {
// Drive each servo one at a time using setPWM()
Serial.println(servonum);
for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) {
pwm.setPWM(servonum, 0, pulselen);
}
delay(500);
for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) {
pwm.setPWM(servonum, 0, pulselen);
}
delay(500);
// Drive each servo one at a time using writeMicroseconds(), it's not precise due to calculation rounding!
// The writeMicroseconds() function is used to mimic the Arduino Servo library writeMicroseconds() behavior.
for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) {
pwm.writeMicroseconds(servonum, microsec);
}
delay(500);
for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {
pwm.writeMicroseconds(servonum, microsec);
}
delay(500);
servonum++;
if (servonum > 7) servonum = 0; // Testing the first 8 servo channels
}
编译后烧录到arduino上，就可以驱动舵机运动了，接下来就可以使用该库来驱动我们的机械臂了。
在ros上，我们还需要编写一个节点，用来订阅/joint_states的消息和将该消息转换为我们舵机的PWM驱动值，作者使用的舵机是180°舵机（标准180°舵机的信号脉冲宽度范围为600微秒到2400微秒），所以限幅SERVOMIN和SERVOMAX是150到600，作者对精度的要求目前不是很高，所以就直接使用了pwm.setPWM来编写，如果读者的精度的要求较高的话，也可以使用pwm.writeMicroseconds编写。
转换的公式为：SERVOMIN = （脉冲长度/1000000）*频率*4096
所以SERVOMIN = （600/20000）*4096 = 150 （50hz约为20毫秒）
在marm_description/script下创建一个states_to_positions.py节点文件，写入以下内容：
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64MultiArray,Float32MultiArray
SERVOMIN = 150 # 最小脉冲长度
SERVOMAX = 600 # 最大脉冲长度
def map_value(value, in_min, in_max, out_min, out_max):
# 将输入值从一个范围映射到另一个范围
return out_min + (out_max - out_min) * (value - in_min) / (in_max - in_min)
def joint_states_callback(msg):
# 创建一个新的消息来发布关节位置
position_msg = Float32MultiArray()
# 将关节位置从弧度转换为PWM值
position_msg.data = [
int(map_value(round(pos, 7), 0, 3.1415926, SERVOMIN, SERVOMAX))
for pos in msg.position[:7] # 只取前7个关节的位置
]
# 发布到新的话题
joint_positions_pub.publish(position_msg)
if __name__ == '__main__':
rospy.init_node('states_test') # 初始化节点
# 订阅 /joint_states 话题
rospy.Subscriber('/joint_states', JointState, joint_states_callback)
# 创建一个发布者，发布到 /joint_positions 话题
joint_positions_pub = rospy.Publisher('/joint_positions', Float32MultiArray, queue_size=10)
rospy.loginfo("states_test node is running, publishing joint positions...")
rate = rospy.Rate(10) # 设置频率为 10 Hz
# 循环等待回调
#rospy.spin()
while not rospy.is_shutdown():
# 循环等待回调
rate.sleep()
在这个节点上，也顺便把关节数据映射为角度了，因为弧度转换为角度是浮点数运算，所以为了避免arduino端算力不足，还可以加快运算的速度，就直接在ros端直接把要做的全做了。
启动这个节点之后，我们也可以再写一个订阅节点，来查看states_to_positions节点运作是否正常。也可以直接rostopic echo /joint_positions
创建joint_positions_listener.py文件。
#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64MultiArray
def joint_positions_callback(msg):
# 打印收到的关节位置
rospy.loginfo("Received joint positions: %s", msg.data)
def main():
rospy.init_node('joint_positions_listener', anonymous=True)
# 订阅 /joint_positions 话题
rospy.Subscriber("/joint_positions", Float64MultiArray, joint_positions_callback)
# 保持节点运行
rospy.spin()
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass移动gui的滑块界面，如果数据的范围都是在你设定的范围内，那就是成功订阅到/joint_states并成功转换了。
回到arduino端，现在到arduino端来订阅/joint_positios的数据了，arduino端的代码建议使用serial1串口一来作为调试串口来输出你订阅到的数据，因为你不知道会不会出现什么问题，然后又找不到问题所在，arduino端的代码如下：
#include <Arduino.h>
#include <ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN 150
#define SERVOMAX 600
#define SERVO_FREQ 50
#define SERVO_NUMS 7
ros::NodeHandle nh;
// 创建 Float32MultiArray 消息
std_msgs::Float32MultiArray joint_positions_msg;
// 回调函数，处理接收到的关节位置消息
void joint_positions_callback(const std_msgs::Float32MultiArray& msg) {
Serial1.print("Received joint positions: ");
for (int i = 0; i < msg.data_length; i++) {
Serial1.print(i);
Serial1.print(", ");
Serial1.print(msg.data[i]);
if (i < msg.data_length - 1) {
Serial1.print(", "); // 添加逗号分隔
}
}
Serial1.println(); // 换行
}
void servo_set(const std_msgs::Float32MultiArray& msg){
for(int i=0;i<msg.data_length;i++){
pwm.setPWM(i,0,msg.data[i]);
}
}
// 创建订阅者
//ros::Subscriber<std_msgs::Float32MultiArray> sub("/joint_positions", joint_positions_callback);
ros::Subscriber<std_msgs::Float32MultiArray> sub("/joint_positions", servo_set);
void setup() {
Serial1.begin(57600); // 初始化串口
Serial1.println("Arduino ROS Node Started");
pwm.begin();
pwm.setPWMFreq(SERVO_FREQ);
delay(10);
nh.initNode();
nh.subscribe(sub); // 订阅 joint_positions 话题
}
void loop() {
nh.spinOnce(); // 处理 ROS 消息
delay(100); // 设置一个适当的延时
}这里串口一是作为我的调试串口来使用的，要特别注意：arduino订阅的数据长度是有限的，例如作者之前使用的是Float64的数组，所以导致回调函数根本就没有触发，因为他数据溢出了，所以要注意你数据的类型，如果你数据太多的话，建议将数据类型缩小，不然就会导致空空如也的情况，使用Float64经过作者一个个的测试之后，发现最多只能容纳5个Float64的数据，多了都会导致回调函数不能正常被触发，所以作者这里将数据类型改成了Float32，如果读者的关节数据没有那么多的话，就可以直接不用管这个了。
现在有了这么多的节点需要启动，一个个启动实在是太麻烦了，所以作者直接就将要启动的东西写成了一个脚本，直接运行就能实现想要运作的节点。
选择一个你喜欢的位置来安放你的脚本，作者这里选择的是arm-robot的目录下。
sudo nano start.sh写入要启动的东西。
#!/bin/bash
# 确保脚本在失败时停止
set -e
# 源代码环境
source ~/catkin_test/devel/setup.bash
# 启动 Gazebo
roslaunch robot_moveit_config gazebo.launch &
# 等待 Gazebo 启动
sleep 15 # 根据需要调整等待时间
# 启动 joint_state_publisher_gui
rosrun joint_state_publisher_gui joint_state_publisher_gui &
# 启动 joint_states_to_command.py
rosrun marm_description joint_states_to_command.py &
sleep 1
rosrun marm_description states_to_positions.py &
sleep 1
rosrun rosserial_python serial_node.py /dev/ttyUSB0
sleep是为了等待进程启动完毕，作者的电脑是比较卡的，所以启动时间就放得长一点了，读者可根据自己的需求来更改。
在arm-robot下直接执行./start，启动所有的东西，移动你的滑块，就能驱动你的机械臂了。
九、结语
本文做的gazebo驱动真实机械臂还只是一个很粗略很粗略的东西，如果是想要完全在gazebo中驱动的结果和你在真实驱动的机械臂做到完全一样，那就需要读者自己的画3d模型，然后在states_to_positions.py中，发送数据的时候为每个关节都添加PID控制，这样应该就能做到大致一样了。
附录：
本文的Gitee开源如下：
arm_ws: 在Ubuntu的ros-noetic上，使用gazebo来控制真实的机械臂运作
也可以直接在你的Ubuntu上运行，这样就能获取到完整的ros工作空间了
git clone https://gitee.com/luxi_an/arm_ws.git