本篇用来记录一次作业的学习例程，错误之处敬请谅解，后续修改
作业要求：
写两个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文件夹下
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::JointState
ros::Rate loop_rate(0.5); //设置循环的频率0.5HZ(2s)
//值设为10时有利于观察Rviz
int count = 0;
while (ros::ok())
{
sensor_msgs::JointState joint_msg;
//声明 sensor_msgs::JointState 类的对象为 joint_msg
joint_msg.header.stamp = ros::Time::now();
//消息头的赋值
joint_msg.name.resize(7);
joint_msg.position.resize(7);
//7个joint对应数组长度为7
joint_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 | a i-1 | θ i | d i |
|---|---|---|---|---|
| 1 | 0 | 0 | θ1-PI | 0.11 |
| 2 | PI/2 | 0 | θ2 | 0 |
| 3 | -PI/2 | 0 | θ3 | 0.2 |
| 4 | -PI/2 | 0 | θ4+PI | 0 |
| 5 | -PI/2 | 0 | θ5-PI | 0.2 |
| 6 | PI/2 | 0 | θ6 | 0 |
| 7 | -PI/2 | 0 | θ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发现不会出现回到起始位置的情况了，很奇怪。
8932
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
