ROS - tf（五）-阿里云开发者社区

ROS - tf（四）+https://developer.aliyun.com/article/1585201

8.2 作为库运行

高级用户还可以在自己的 C++ 代码中将机器人状态发布器作为库运行。加入头文件后

#include <robot_state_publisher/robot_state_publisher.h>

您只需使用构造函数，该构造函数将接收一棵 KDL 树

RobotStatePublisher(const KDL::Tree& tree);

现在，每次要发布机器人的状态时，都要调用 publishTransforms 函数：

// publish moving joints
  void publishTransforms(const std::map<std::string, double>& joint_positions,
                         const ros::Time& time);
  // publish fixed joints
  void publishFixedTransforms();

第一个参数是包含关节名称和关节位置的地图，第二个参数是记录关节位置的时间。如果地图不包含所有关节名，也没有关系。如果地图中包含一些不属于运动学模型的关节名称，也没有问题。但要注意的是，如果你没有告诉关节状态发布者运动学模型中的某些关节，那么你的 tf 树将是不完整的。

九、将 urdf 与 robot_state_publisher 结合使用

9.1 创建 URDF 文件

下面是一个大致与 R2-D2 相似的 7 link 模型的 URDF 文件。将以下链接保存到您的计算机：model.xml

<robot name="r2d2">
<link name="axis">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="1.57 0 0"/>
<geometry>
<cylinder radius="0.01" length=".5"/>
</geometry>
<material name="gray">
<color rgba=".2 .2 .2 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.57 0 0"/>
<geometry>
<cylinder radius="0.01" length=".5"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<link name="leg1">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.3"/>
<geometry>
<box size=".20 .10 .8"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -.3"/>
<geometry>
<box size=".20 .10 .8"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="leg1connect" type="fixed">
<origin xyz="0 .30 0"/>
<parent link="axis"/>
<child link="leg1"/>
</joint>
<link name="leg2">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.3"/>
<geometry>
<box size=".20 .10 .8"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -.3"/>
<geometry>
<box size=".20 .10 .8"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="leg2connect" type="fixed">
<origin xyz="0 -.30 0"/>
<parent link="axis"/>
<child link="leg2"/>
</joint>
<link name="body">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -0.2"/>
<geometry>
<cylinder radius=".20" length=".6"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0.2"/>
<geometry>
<cylinder radius=".20" length=".6"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="tilt" type="revolute">
<parent link="axis"/>
<child link="body"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit upper="0" lower="-.5" effort="10" velocity="10"/>
</joint>
<link name="head">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<geometry>
<sphere radius=".4"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin/>
<geometry>
<sphere radius=".4"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="swivel" type="continuous">
<origin xyz="0 0 0.1"/>
<axis xyz="0 0 1"/>
<parent link="body"/>
<child link="head"/>
</joint>
<link name="rod">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.1"/>
<geometry>
<cylinder radius=".02" length=".2"/>
</geometry>
<material name="gray"/>
</visual>
<collision>
<origin/>
<geometry>
<cylinder radius=".02" length=".2"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="periscope" type="prismatic">
<origin xyz=".12 0 .15"/>
<axis xyz="0 0 1"/>
<limit upper="0" lower="-.5" effort="10" velocity="10"/>
<parent link="head"/>
<child link="rod"/>
</joint>
<link name="box">
<inertial>
<mass value="1"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
<origin/>
</inertial>
<visual>
<geometry>
<box size=".05 .05 .05"/>
</geometry>
<material name="blue">
<color rgba="0 0 1 1"/>
</material>
</visual>
<collision>
<origin/>
<geometry>
<box size=".05 .05 .05"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="boxconnect" type="fixed">
<origin xyz="0 0 0"/>
<parent link="rod"/>
<child link="box"/>
</joint>
</robot>

9.2 发布状态

现在，我们需要一种方法来指定机器人所处的状态。为此，我们必须指定所有三个关节和整体运动轨迹。首先创建一个软件包：

cd %TOP_DIR_YOUR_CATKIN_WS%/src
catkin_create_pkg r2d2 roscpp rospy tf sensor_msgs std_msgs

然后启动你最喜欢的编辑器，将以下代码粘贴到 src/state_publisher.cpp 文件中：

#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(30);
    const double degree = M_PI/180;
    // robot state
    double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;
    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "axis";
    while (ros::ok()) {
        //update joint_state
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(3);
        joint_state.position.resize(3);
        joint_state.name[0] ="swivel";
        joint_state.position[0] = swivel;
        joint_state.name[1] ="tilt";
        joint_state.position[1] = tilt;
        joint_state.name[2] ="periscope";
        joint_state.position[2] = height;
        // update transform
        // (moving in a circle with radius=2)
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = cos(angle)*2;
        odom_trans.transform.translation.y = sin(angle)*2;
        odom_trans.transform.translation.z = .7;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);
        //send the joint state and transform
        joint_pub.publish(joint_state);
        broadcaster.sendTransform(odom_trans);
        // Create new robot state
        tilt += tinc;
        if (tilt<-.5 || tilt>0) tinc *= -1;
        height += hinc;
        if (height>.2 || height<0) hinc *= -1;
        swivel += degree;
        angle += degree/4;
        // This will adjust as needed per iteration
        loop_rate.sleep();
    }
    return 0;
}

9.3 启动文件

此启动文件假定您使用的软件包名称为 "r2d2"，节点名称为 "state_publisher"，并且您已将此 urdf 保存到 "r2d2 "软件包中。

<launch>
        <param name="robot_description" command="cat $(find r2d2)/model.xml" />
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
        <node name="state_publisher" pkg="r2d2" type="state_publisher" />
</launch>

9.4 查看结果

首先，我们必须编辑保存上述源代码的软件包中的 CMakeLists.txt。除了其他依赖关系外，确保添加 tf 依赖关系：

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs tf)

请注意，roscpp 用于解析我们编写的代码并生成 state_publisher 节点。为了生成 state_publisher 节点，我们还必须在 CMakelists.txt 末尾添加以下内容：

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）。

ROS - tf（五）

8.2 作为库运行

九、将 urdf 与 robot_state_publisher 结合使用

9.1 创建 URDF 文件

9.2 发布状态

9.3 启动文件

9.4 查看结果

热门文章

最新文章

相关课程

相关电子书

相关实验场景

推荐镜像