本教程提供了使用URDF的机器人模型的完整示例,该模型使用robot_state_publisher。 首先,我们创建包含所有必要部分的URDF模型。 然后,我们编写一个节点,该节点发布JointState并进行转换。 最后,我们一起运行所有部分。
1.Create the URDF File
Here is the URDF file for a 7-link model roughly approximating R2-D2. Save the following link to your computer: model.xml
2.Publishing the State
现在,我们需要一种方法来指定机器人所处的状态。为此,我们必须指定所有三个关节和整个里程表。 首先创建一个包:
cd %TOP_DIR_YOUR_CATKIN_WS%/src
catkin_create_pkg r2d2 roscpp rospy tf sensor_msgs std_msgs
然后启动您喜欢的编辑器,并将以下代码粘贴到src / state_publisher.cpp文件中:
1 #include <string>
2 #include <ros/ros.h>
3 #include <sensor_msgs/JointState.h>
4 #include <tf/transform_broadcaster.h>
5
6 int main(int argc, char** argv) {
7 ros::init(argc, argv, "state_publisher");
8 ros::NodeHandle n;
9 ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
10 tf::TransformBroadcaster broadcaster;
11 ros::Rate loop_rate(30);
12
13 const double degree = M_PI/180;
14
15 // robot state
16 double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;
17
18 // message declarations
19 geometry_msgs::TransformStamped odom_trans;
20 sensor_msgs::JointState joint_state;
21 odom_trans.header.frame_id = "odom";
22 odom_trans.child_frame_id = "axis";
23
24 while (ros::ok()) {
25 //update joint_state
26 joint_state.header.stamp = ros::Time::now();
27 joint_state.name.resize(3);
28 joint_state.position.resize(3);
29 joint_state.name[0] ="swivel";
30 joint_state.position[0] = swivel;
31 joint_state.name[1] ="tilt";
32 joint_state.position[1] = tilt;
33 joint_state.name[2] ="periscope";
34 joint_state.position[2] = height;
35
36
37 // update transform
38 // (moving in a circle with radius=2)
39 odom_trans.header.stamp = ros::Time::now();
40 odom_trans.transform.translation.x = cos(angle)*2;
41 odom_trans.transform.translation.y = sin(angle)*2;
42 odom_trans.transform.translation.z = .7;
43 odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);
44
45 //send the joint state and transform
46 joint_pub.publish(joint_state);
47 broadcaster.sendTransform(odom_trans);
48
49 // Create new robot state
50 tilt += tinc;
51 if (tilt<-.5 || tilt>0) tinc *= -1;
52 height += hinc;
53 if (height>.2 || height<0) hinc *= -1;
54 swivel += degree;
55 angle += degree/4;
56
57 // This will adjust as needed per iteration
58 loop_rate.sleep();
59 }
60
61
62 return 0;
63 }
3.Launch File
该启动文件假定您正在使用程序包名称“ r2d2”和节点名称“ state_publisher”,并且已将此urdf保存到“ r2d2”程序包中。
切换行号显示
1 <launch>
2 <param name="robot_description" command="cat $(find r2d2)/model.xml" />
3 <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
4 <node name="state_publisher" pkg="r2d2" type="state_publisher" />
5 </launch>
4.Viewing the Results
首先,我们必须在保存上述源代码的程序包中编辑CMakeLists.txt。 确保除了其他依赖项之外还添加tf依赖项:
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs tf)
注意,roscpp用于解析我们编写的代码并生成state_publisher节点。 我们还必须在CMakelists.txt的末尾添加以下内容,以生成state_publisher节点:
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(state_publisher src/state_publisher.cpp)
target_link_libraries(state_publisher ${catkin_LIBRARIES})
现在,我们应该转到工作空间的目录并使用以下命令进行构建:
$ catkin_make
现在启动该软件包(假设我们的启动文件名为display.launch):
$ roslaunch r2d2 display.launch
使用以下命令在新终端中运行rviz:
$ rosrun rviz rviz
选择odom作为您的固定框架(在“全局选项”下)。 然后选择“添加显示”并添加机器人模型显示和TF显示(请参见http://wiki.ros.org/rviz/UserGuide)。
接着会显示:
[ INFO] [1593491745.493525939]: rviz version 1.13.12
[ INFO] [1593491745.493636863]: compiled against Qt version 5.9.5
[ INFO] [1593491745.493672751]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1593491745.505394669]: Forcing OpenGl version 0.
[ INFO] [1593491746.428393136]: Stereo is NOT SUPPORTED
[ INFO] [1593491746.428455243]: OpenGl version: 3 (GLSL 1.3).