ROS学习笔记(十六)：Using urdf with robot_state_publisher

本教程提供了使用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).