devbox
煮酒与君饮
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

ROS中7自由度机械臂自定义发布订阅节点_如何在rviz中通过订阅其他节点发布法话题让机械臂模型动起来

作者：煮酒与君饮 | 2024-07-26 21:30:28

赞

踩

如何在rviz中通过订阅其他节点发布法话题让机械臂模型动起来

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

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

1.准备工作

创建工作空间及功能包

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

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

  • 1
  • 2
  • 3

在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

  • 1

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>

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

通过launch文件启动

roslaunch lwr_description display_lwr_urdf.launch

  • 1

可以看到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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
关于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

  • 1
  • 2
  • 3
  • 4
  • 5

声明消息变量

sensor_msgs::JointState joint_state;

  • 1

消息头的赋值

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

  • 1

消息内容的赋值

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

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数组的第一个元素赋值

  • 1
  • 2
  • 3
  • 4

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

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

  • 1
  • 2

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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
配置CMakeLists.txt中的编译规则
add_executable(joint_state_publisher_sy src/joint_state_publisher_sy.cpp)		

target_link_libraries(joint_state_publisher_sy ${catkin_LIBRARIES})

  • 1
  • 2
  • 3

将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})

  • 1
  • 2
  • 3
终端编译并运行订阅者和发布者
cd catkin_ws
catkin_make
roscore
rosrun lwr_description joint_state_subscriber_sy
rosrun lwr_description joint_state_publisher_sy

  • 1
  • 2
  • 3
  • 4
  • 5

可以看到相应的发布订阅信息
在这里插入图片描述

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>

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21

通过启动文件运行：

roslaunch lwr_description display_lwr_urdf.launch

  • 1

可以看到机械臂在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"/>


  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
D-H法建模

在这里插入图片描述

D-H table

iα i-1a i-1θ id i
100θ1-PI0.11
2PI/20θ20
3-PI/20θ30.2
4-PI/20θ4+PI0
5-PI/20θ5-PI0.2
6PI/20θ60
7-PI/20θ70.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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122

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

6.最终效果和总结

启动launch文件后可以看到模型在Rviz中按照发布的关节角信息连续不断运动，终端显示发布的关节角度和对应的末端位姿。
在这里插入图片描述
在终端单独运行订阅者和发布者时，会看到发布订阅连续变化的关节角度，但在Rviz中显示时，模型每次运动完成后会回到起始位置，原因暂时没有搞清楚。

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

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

过了几天再次launch发现不会出现回到起始位置的情况了，很奇怪。
在这里插入图片描述

声明：本文内容由网友自发贡献，转载请注明出处：【wpsshop博客】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号