ROS中7自由度机械臂自定义发布订阅节点

本篇用来记录一次作业的学习例程，错误之处敬请谅解，后续修改

作业要求：
写两个ROS节点，一个节点发布连续变化（可以按sin曲线变化）的7自由度的关节角信息；另一个节点订阅第一个节点发布的关节角信息，然后通过正运动学（可以用DH法建模）计算出最终的末端位姿，并把这个末端位姿在rviz中显示出来。机器人模型可以考虑用KUKA，其他的也可以。

1.准备工作

创建工作空间及功能包

创建工作空间及src文件夹，src文件夹下 init 使它变成工作空间的属性

mkdir -p ~/catkin_ws/src
cd ~/catkin_wc/src
catkin_init_workspace

在src文件夹下创建lwr_description功能包，依赖std_msgs、roscpp、message_generation、joint_state_controller、robot_state_publisher库

catkin_create_pkg lwr_description message_generation std_msgs roscpp joint_state_controller robot_state_publisher

lwr_description功能包中包含以下文件夹：

urdf：用于存放机器人模型的URDF或xacro文件

meshes：用于放置URDF中引用的模型渲染文件

launch：用于保存相关启动文件

config：用于保存Rviz的配置文件

src: 用于存放编写的节点

include: 放置功能包中需要用到的头文件

准备URDF模型

在GitHub上找到一个7自由度KUKA-LWR的urdf模型文件 lwr.urdf，放在urdf文件夹下

https://github.com/ipabslmc/exotica/blob/master/exotica_examples/resources/robots/lwr_simplified.urdf

check_urdf
check_urdf命令检查urdf文件没有问题

Rviz中显示模型

找到URDF模型后，可以在Rviz中将该模型显示出来

在lwr_description功能包下的launch文件夹中创建显示lwr.urdf模型的launch文件，display_lwr_urdf.launch

参考《ROS机器人开发实践》6.2.4，初步编写launch文件

<launch><param name="robot_description" textfile="$(find lwr_description)/urdf/lwr.urdf" /><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /><node name="rviz" pkg="rviz" type="rviz" args="-d $(find lwr_description)/config/lwr_urdf.rviz" required="true" />   </launch>

通过launch文件启动

roslaunch lwr_description display_lwr_urdf.launch

可以看到KUKA-LWR URDF模型在Rviz中成功显示

2.编写发布者代码

创建一个发布连续变化 (按sin曲线变化) 的7自由度关节角信息的节点

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>           //使用的消息类型为sensor_msgs::JointState
#include <math.h>
#include <string>
#define PI acos(-1.0)int main(int argc, char** argv)
{ros::init(argc, argv, "joint_state_publisher_sy");   //节点初始化          ros::NodeHandle nh;                                        //创建节点句柄ros::Publisher joint_state_pub1 = nh.advertise<sensor_msgs::JointState>("joint_states", 100);//创建发布者joint_state_pub1,发布话题joint_states,消息类型sensor_msgs::JointStateros::Rate loop_rate(0.5);                             //设置循环的频率0.5HZ(2s)//值设为10时有利于观察Rvizint count = 0;while (ros::ok()){sensor_msgs::JointState joint_msg;                              //声明 sensor_msgs::JointState 类的对象为 joint_msgjoint_msg.header.stamp = ros::Time::now();    //消息头的赋值    joint_msg.name.resize(7);                                                       joint_msg.position.resize(7);//7个joint对应数组长度为7joint_msg.name[0] = "lwr_arm_0_joint";                    joint_msg.position[0] = sin(count * PI / 18);joint_msg.name[1] = "lwr_arm_1_joint";                       joint_msg.position[1] = sin(count * PI / 18);joint_msg.name[2] = "lwr_arm_2_joint";joint_msg.position[2] = sin(count * PI / 18);joint_msg.name[3] = "lwr_arm_3_joint";joint_msg.position[3] = sin(count * PI / 18);joint_msg.name[4] = "lwr_arm_4_joint";joint_msg.position[4] = sin(count * PI / 18);joint_msg.name[5] = "lwr_arm_5_joint";joint_msg.position[5] = sin(count * PI / 18);joint_msg.name[6] = "lwr_arm_6_joint";joint_msg.position[6] = sin(count * PI / 18);joint_state_pub1.publish(joint_msg);               //publish()方法 发布消息                 ROS_INFO("publish success");                           //输出 publish success                      loop_rate.sleep();                                    //按照循环频率延时                                           count++;}return 0;
}

关于sensor_msgs::JointState 消息类型

为了使创建的URDF机器人模型正确运动，必须给出robot_state_publisher 节点所需的sensor_msgs::JointState型topic: joint_states
sensor_msgs::JointState 消息格式为：

std_msgs/Header header
string[] name
float64[] position
float64[] velocity
float64[] effort

声明消息变量

sensor_msgs::JointState joint_state;

消息头的赋值

joint_state.header.stamp = ros::Time::now();

消息内容的赋值

从上述消息格式中可知，该消息数据包含多个不同含义的数组，并且该数组没有指定数组长度，因此在赋值时需要明确指定其数组长度，并赋值。方法有两种：

1.resize指定数组长度，再赋值

joint_state.name.resize(3);      //指定name数组长度
joint_state.position.resize(3);     //指定position数组长度
joint_state.name[0] ="joint1";       //name数组的第一个元素赋值
joint_state.position[0] = 0;       //position数组的第一个元素赋值

2.{}赋值该方法类似于C/C++声明数组并赋初值

joint_state.name={"joint1","joint2","joint3"};    //指定数组大小，并赋值
joint_state.position={0,0,0};

velocity和effort相同，经过上述两步赋值后，可正常发布消息

此处参考：
http://wiki.ros.org/robot_state_publisher?distro=melodic
https://wenku.baidu.com/view/e93ac74d551252d380eb6294dd88d0d233d43cab.html

3.编写订阅者代码

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/JointState.h>
#include <string>void stateInfoCallback(const sensor_msgs::JointState::ConstPtr& msg)
{ROS_INFO("I heard: [%s],position: [%f],[%s],position: [%f][%s],position: [%f][%s],position: [%f][%s],position: [%f][%s],position: [%f][%s],position: [%f]",msg->name[0].c_str(),msg->position[0],msg->name[1].c_str(),msg->position[1],msg->name[2].c_str(),msg->position[2],msg->name[3].c_str(),msg->position[3],msg->name[4].c_str(),msg->position[4],msg->name[5].c_str(),msg->position[5],msg->name[6].c_str(),msg->position[6]//输出7个 joint的名字和position);
}int main(int argc, char **argv)
{ros::init(argc, argv, "joint_state_subscriber_sy");  //节点初始化          ros::NodeHandle n;                                     //创建节点句柄ros::Subscriber joint_state_sub1 = n.subscribe("joint_states", 10, stateInfoCallback);//创建订阅者 joint_state_sub1 订阅话题 joint_states 注册回调函数ros::spin();                                          //循环等待回调函数                                          return 0;
}

配置CMakeLists.txt中的编译规则

add_executable(joint_state_publisher_sy src/joint_state_publisher_sy.cpp)        target_link_libraries(joint_state_publisher_sy ${catkin_LIBRARIES})

将joint_state_publisher_sy.cpp编译成可执行文件joint_state_publisher_sy
C++接口通过target_link_libraries库与ROS进行连接

add_executable(joint_state_subscriber_sy src/joint_state_subscriber_sy.cpp) target_link_libraries(joint_state_subscriber_sy ${catkin_LIBRARIES})

终端编译并运行订阅者和发布者

cd catkin_ws
catkin_make
roscore
rosrun lwr_description joint_state_subscriber_sy
rosrun lwr_description joint_state_publisher_sy

可以看到相应的发布订阅信息

4.完善launch启动文件

加入编写的发布订阅节点，使他们能够在屏幕中显示

<launch><param name="robot_description" textfile="$(find lwr_description)/urdf/lwr.urdf" /><!-- 运行joint_state_publisher节点,发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /><!-- 运行robot_state_publisher节点,发布TF,没有这个无法看到模型 -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /><!-- 运行编写的joint_state_subscriber_sy节点 -->
<node name="joint_state_subscriber_sy" pkg="lwr_description" type="joint_state_subscriber_sy" output="screen" /><!-- 运行编写的joint_state_publisher_sy节点 -->
<node name="joint_state_publisher_sy" pkg="lwr_description" type="joint_state_publisher_sy" output="screen" /><!-- 运行rviz可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find lwr_description)/config/lwr_urdf.rviz" required="true" /> </launch>

通过启动文件运行：

roslaunch lwr_description display_lwr_urdf.launch

可以看到机械臂在rviz中连续不断运动

5.正运动学DH法解算末端位姿

URDF模型标签的解读：

origin: 定义joint的起点

axis: 定义该joint的旋转轴

截取 lwr.urdf 中的部分需要用到的代码<joint name="base_joint" type="fixed">                                                   <origin rpy="0 0 3.14159265359" xyz="0 0 0.02"/>
<joint name="lwr_arm_0_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.11"/><axis xyz="0 0 1"/>
<joint name="lwr_arm_1_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.2005"/><axis xyz="0 1 0"/>
<joint name="lwr_arm_2_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.20"/><axis xyz="0 0 1"/>
<joint name="lwr_arm_3_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.20"/><axis xyz="0 -1 0"/>
<joint name="lwr_arm_4_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.20"/><axis xyz="0 0 1"/>
<joint name="lwr_arm_5_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.19"/><axis xyz="0 1 0"/>
<joint name="lwr_arm_6_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.078"/><axis xyz="0 0 1"/>

D-H法建模

D-H table

i	α i-1	θ i	d i
1	0	θ1-PI	0.11
2	PI/2	θ2	0
3	-PI/2	θ3	0.2
4	-PI/2	θ4+PI	0
5	-PI/2	θ5-PI	0.2
6	PI/2	θ6	0
7	-PI/2	θ7	0.078

通过公式求解末端位姿

正运动学解算方法参考：https://www.bilibili.com/video/BV1v4411H7ez?p=3&spm_id_from=pageDriver

完善订阅者代码

修改订阅者代码使其能够输出末端位姿

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/JointState.h>
#include <string>
#include<math.h>
#define PI acos(-1.0)void personInfoCallback(const sensor_msgs::JointState::ConstPtr& msg)
{float pos[7];              pos[0] = msg->position[0];pos[1] = msg->position[1];pos[2] = msg->position[2];pos[3] = msg->position[3];pos[4] = msg->position[4];pos[5] = msg->position[5];pos[6] = msg->position[6];float theta[] = { pos[0] - PI , pos[1] , pos[2] , pos[3] , PI + pos[4] ,pos[5] - PI ,pos[6] };float d1 = 0.11;float d3 = 0.2;float d5 = 0.2;float d7 = 0.078;//此处只是为了矩阵计算,应当有更好的计算方法,暂时不会,学习中，后续修改float a11 = (cos(theta[0]) * cos(theta[1]) * cos(theta[2]) * cos(theta[3])) + (sin(theta[3]) * cos(theta[0]) * sin(theta[1])) - (sin(theta[0]) * sin(theta[2]) * cos(theta[3]));float a12 = (cos(theta[0]) * sin(theta[1]) * cos(theta[3])) - (cos(theta[0]) * cos(theta[1]) * cos(theta[2]) * sin(theta[3])) + (sin(theta[0]) * sin(theta[2]) * sin(theta[3]));float a13 = ((-1) * cos(theta[0]) * cos(theta[1]) * sin(theta[2])) - (sin(theta[0]) * cos(theta[2]));float a14 = ((-1) * cos(theta[0]) * sin(theta[1]) * d3);float a21 = (sin(theta[0]) * cos(theta[1]) * cos(theta[2]) * cos(theta[3])) + (sin(theta[0]) * sin(theta[1]) * sin(theta[3])) + (cos(theta[0]) * sin(theta[2]) * cos(theta[3]));float a22 = ((-1) * sin(theta[0]) * cos(theta[1]) * cos(theta[2]) * sin(theta[3])) + (sin(theta[0]) * sin(theta[1]) * cos(theta[3])) - (sin(theta[2]) * sin(theta[3]) * cos(theta[0]));float a23 = ((-1) * sin(theta[0]) * cos(theta[1]) * sin(theta[2])) + (cos(theta[0]) * cos(theta[2]));float a24 = ((-1) * sin(theta[0]) * sin(theta[1]) * d3);float a31 = (sin(theta[1]) * cos(theta[2]) * cos(theta[3])) - (sin(theta[3]) * cos(theta[1]));float a32 = ((-1) * sin(theta[1]) * cos(theta[2]) * sin(theta[3])) - (cos(theta[1]) * cos(theta[3]));float a33 = ((-1) * sin(theta[1]) * sin(theta[2]));float a34 = (cos(theta[1]) * d3) + d1;float a41 = 0;float a42 = 0;float a43 = 0;float a44 = 1;float b11 = (cos(theta[4]) * cos(theta[5]) * cos(theta[6])) - (sin(theta[4]) * sin(theta[6]));float b12 = ((-1) * cos(theta[4]) * cos(theta[5]) * sin(theta[6])) - (sin(theta[4]) * cos(theta[6]));float b13 = ((-1) * cos(theta[4]) * sin(theta[5]));float b14 = ((-1) * cos(theta[4]) * sin(theta[5]) * d7);float b21 = (sin(theta[5]) * cos(theta[6]));float b22 = ((-1) * sin(theta[5]) * sin(theta[6]));float b23 = cos(theta[5]);float b24 = (cos(theta[5]) * d7) + d5;float b31 = ((-1) * sin(theta[4]) * cos(theta[5]) * cos(theta[6])) - (cos(theta[4]) * sin(theta[6]));float b32 = (sin(theta[4]) * cos(theta[5]) * sin(theta[6])) - (cos(theta[4]) * cos(theta[6]));float b33 = sin(theta[4]) * sin(theta[5]);float b34 = sin(theta[4]) * sin(theta[5]) * d7;float b41 = 0;float b42 = 0;float b43 = 0;float b44 = 1;float c11 = (a11 * b11) + (a12 * b21) + (a13 * b31) + (a14 * b41);float c12 = (a11 * b12) + (a12 * b22) + (a13 * b32) + (a14 * b42);float c13 = (a11 * b13) + (a12 * b23) + (a13 * b33) + (a14 * b43);float c14 = (a11 * b14) + (a12 * b24) + (a13 * b34) + (a14 * b44);float c21 = (a21 * b11) + (a22 * b21) + (a23 * b31) + (a24 * b41);float c22 = (a21 * b12) + (a22 * b22) + (a23 * b32) + (a24 * b42);float c23 = (a21 * b13) + (a22 * b23) + (a23 * b33) + (a24 * b43);float c24 = (a21 * b14) + (a22 * b24) + (a23 * b34) + (a24 * b44);float c31 = (a31 * b11) + (a32 * b21) + (a33 * b31) + (a34 * b41);float c32 = (a31 * b12) + (a32 * b22) + (a33 * b32) + (a34 * b42);float c33 = (a31 * b13) + (a32 * b23) + (a33 * b33) + (a34 * b43);float c34 = (a31 * b14) + (a32 * b24) + (a33 * b34) + (a34 * b44);float c41 = (a41 * b11) + (a42 * b21) + (a43 * b31) + (a44 * b41);float c42 = (a41 * b12) + (a42 * b22) + (a43 * b32) + (a44 * b42);float c43 = (a41 * b13) + (a42 * b23) + (a43 * b33) + (a44 * b43);float c44 = (a41 * b14) + (a42 * b24) + (a43 * b34) + (a44 * b44);ROS_INFO("I heard: [%s],position: [%f],[%s],position: [%f][%s],position: [%f][%s],position: [%f][%s],position: [%f][%s],position: [%f][%s],position: [%f]",msg->name[0].c_str(),msg->position[0],msg->name[1].c_str(),msg->position[1],msg->name[2].c_str(),msg->position[2],msg->name[3].c_str(),msg->position[3],msg->name[4].c_str(),msg->position[4],msg->name[5].c_str(),msg->position[5],msg->name[6].c_str(),msg->position[6] );ROS_INFO("[%f] [%f] [%f]",c11,c12,c13);ROS_INFO("[%f] [%f] [%f]",c21,c22,c23);ROS_INFO("[%f] [%f] [%f]",c31,c32,c33);ROS_INFO("Px:[%f] Py:[%f] Pz:[%f]", c14, c24, c34);}int main(int argc, char** argv)
{ros::init(argc, argv, "joint_state_subscriber_sy");ros::NodeHandle n;ros::Subscriber joint_state_sub1 = n.subscribe("joint_states", 10, personInfoCallback);ros::spin();return 0;
}

此处可以参考：
https://www.freesion.com/article/7089625820/

6.最终效果和总结

启动launch文件后可以看到模型在Rviz中按照发布的关节角信息连续不断运动，终端显示发布的关节角度和对应的末端位姿。

在终端单独运行订阅者和发布者时，会看到发布订阅连续变化的关节角度，但在Rviz中显示时，模型每次运动完成后会回到起始位置，原因暂时没有搞清楚。

正运动学求解末端位姿处应该有更好的方法，可以参考：
https://blog.csdn.net/weixin_37834269/article/details/111183412

本篇没有使用MoveIt，后续会继续学习。

过了几天再次launch发现不会出现回到起始位置的情况了，很奇怪。