1 创建一个package
catkin_create_pkg myurdf joint_state_controller robot_state_publisher roscpp std_msgs tf
2 创建urdf文件夹
cd myurdf
mkdir urdf
3 创建urdf文件
gedit myfirstrobot.urdf
输入以下内容,并保存:
<?xml version="1.0"?>
<robot name="myfirstrobot">
<!-- Base Link -->
<link name="base_link">
<visual>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
</visual>
</link>
</robot>
4 创建launch文件
cd myurdf
mkdir launch
gedit display.launch
输入并保存以下内容:
<launch>
<param name="robot_description"
textfile="$(find myurdf)/urdf/myfirstrobot.urdf"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
<node name="rviz" pkg="rviz" type="rviz" respawn="false" output="screen"/>
</launch>
5 启动launch文件
roslaunch myurdf display.launch
得到下图结果:
6 增加一个连杆
更新后的urdf文件如下:
<?xml version="1.0"?>
<robot name="myfirstrobot">
<!-- Base Link -->
<link name="base_link">
<visual>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
</visual>
</link>
<joint name="joint1" type="continuous">
<parent link="base_link"/>
<child link="middle_link"/>
<origin rpy="0 0 0" xyz="0 0 1"/>
<axis xyz="0 1 0"/>
</joint>
<link name="middle_link">
<visual>
<geometry>
<box size="0.2 0.2 1"/>
</geometry>
</visual>
</link>
</robot>
得到下图结果:
发现问题:
(1) Global Status: WARN状态,显示信息为No tf data. Actual error: Fixed Frame [base_link] does not exist.
(2) 在TF显示控件中,并没优tf tree存在,由于此时机器人模型由两个杆件组成,因此tf tree 应包含 base_link 和 middle_link.
问题原因:
urdf文件中,joint type=“continuous”,说名该joint为旋转型。对于旋转型关节,必须给出robot_state_publisher 节点所需的sensor_msgs/JointState型topic :joint_states。可参见:robot_state_publisher
解决方案:
以任意方式发布sensor_msgs/JointState型topic :joint_states。
7 以下是自定义节点发布joint_states
创建state_publisher.cpp
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.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);
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
sensor_msgs::JointState joint_state;
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] ="joint1";
joint_state.position[0] = swivel;
joint_pub.publish(joint_state);
// This will adjust as needed per iteration
loop_rate.sleep();
}
return 0;
};
正确配置CMakelists文件,在此不再赘述,完成后编译。
8 配置launch文件
<launch>
<param name="robot_description"
textfile="$(find myurdf)/urdf/myfirstrobot.urdf"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
<node name="state_publisher" pkg="myurdf" type="state_publisher"/>
<node name="rviz" pkg="rviz" type="rviz" respawn="false" output="screen"/>
</launch>
8 再次启动launch文件
roslaunch myurdf display.launch
得到如下结果:
发现:
(1) Global Status:状态正常
(2) tf tree 中包含 base_link 和 middle_link
(3) 显示窗口显示base_link和middle_link frame
还可以通过其他方式处理,如通过joint_state_publisher节点,相关处理可参考wiki。
至此,问题解决。