urdf 怎么添加头文件_URDF教程

创建自己的URDF文件 1.1创建树形结构文件 在这部分教程中要创建的将是下面的图形所描述的机器人的urdf文件

 图片中这个机器人是一个树形结构的。让我们开始非常简单的创建这个树型结构的描述文件，不用担心维度等的问题。创建一个my_robot.urdf文件，内容如下：

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

所以，这里是仅仅创建了一个非常简单的结构。现在我们来看一看我们能否解读这个文件。可以使用一些简单的命令行工具，来检查你的语法是否正确。 需要安装urdfdom作为一个上游的ROS独立包sudo apt-get install liburdfdom-tools1

现在运行检查命令(基于indigo)check_urdf my_robot.urdf1

如果一切顺利的话，将会看到的是

 1.2添加坐标系(维度)信息 现在有了一个基本的树形结构，让我们来添加合适的坐标系信息。如你在图片中看到的，每一个连接的参考系都在连接的底部，而且关节的参考系是完全相同的。所以，要添加维度到我们的树上，我们要确认的就是从一个连接到自己子连接的关机的偏移量。为了完成这一部分，我们将会为每一个关节添加. 比如，我们来看第二个关节。joint2是link1在Y方向上的偏移，在X轴负方向上有一点点偏移，而且绕Z轴旋转了90度，所以我们需要添加下面的元素。1

在每一个关节重复这一改变，这个urdf文件看起来就是这样子的：

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

更新你的my_robot.urd文件，并通过解析器运行它。check_urdf my_robot.urdf1

1.3 完成运动学部分 我们现在还不确定的部分就是关节绕着哪个轴旋转。一旦我们添加了这个部分，我们实际上有了这个机器人完整的运动学模型。我们要做的就是添加元素到每一个joint中。axis确定在局部的旋转轴。 所以，如果你看joint2，你会看到它是绕着Y轴正方向旋转。所以，需要简单添加下面的内容到关节元素中：1

相似的，需要向joint1中添加下面的内容：1

如果向所有关节中添加了这一元素的话，这个urdf文件看起来就是下面这样的：

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

更新你的my_robot.urdf文件，并用解析器运行它check_urdf my_robot.urdf1

到这里为止，你就创建了你的第一个URDF机器人描述。现在你可以尝试用graphiz将你的URDF可视化。urdf_to_graphiz my_robot.urdf1

打开产生的文件，比如用evince test_robot.pdf解析一个URDF文件 2.1解析一个URDF文件 首先，我们创建一个依赖于urdf解析器的包。$cd ~/catkin_ws/src

$catkin_create_pkg testbot_description urdf

$cd testbot_description1

2

3

现在我们来创建一个urdf文件夹来存储我们刚刚创建的urdf文件。mkdir urdf1

按照一般的惯例，urdf文件是在一个名为MYROBOT_description的包中，它的标准的子文件夹还包括有/meshes, /media 和/cd，就像是下面这样：/MYROBOT_description

package.xml

CMakeLists.txt

/urdf

/meshes

/materials

/cad1

2

3

4

5

6

7

接下来，将我们创建的my_robot.urdf拷贝到我们刚刚创建的文件夹中。$ cp /path/to/.../testbot_description/urdf/my_robot.urdf1

创建一个src文件夹，并创建src/parser.cpp文件#include

#include "ros/ros.h"

int main(int argc, char** argv){

ros::init(argc, argv, "my_parser");

if (argc != 2){

ROS_ERROR("Need a urdf file as argument");

return -1;

}

std::string urdf_file = argv[1];

urdf::Model model;

if (!model.initFile(urdf_file)){

ROS_ERROR("Failed to parse urdf file");

return -1;

}

ROS_INFO("Successfully parsed urdf file");

return 0;

}1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

真正的动作是发生在 urdf::Model model; if (!model.initFile(urdf_file)){这两行。如果URDF文件能够成功解析的话initFile方法返回true。 然后我们来尝试运行这个程序，首先向CmakeList.txt文件中添加这两行add_executable(parser src/parser.cpp)

target_link_libraries(parser ${catkin_LIBRARIES})1

2

构建你的包，并运行它$ catkin_make

$ ./parser my_robot.urdf

# ./devel/lib/robot_description/parser /src/robot_description/urdf/my_robot.urdf (for example)1

2

3

输出应该看起来是这样的：[ INFO] 1254520129.560927000: Successfully parsed urdf file1

现在，可以看一下code API(http://docs.ros.org/api/urdf/html/)来看看如何使用你刚刚创建的URDF模型。一个很好的URDF模型类的例子在 https://github.com/ros-visualization/rviz/blob/groovy-devel/src/rviz/robot/robot.cpp - 在自己的机器人上使用robot state publisher 这部分教程将解释如何使用机器人状态发布者来发布机器人状态到tf。 当你的机器人是有很多关联坐标系的机器人时，把它们全部发布到tf也是一项相当可观的任务。robot state publisher是一个可以帮助你处理这项任务的工具。

 robot state publisher帮助你把你的机器人状态发布到tf转换库中。robot state publisher内部有一个机器人的运动学模型，所以给定机器人位置，robot state publisher能够计算和发布机器人每一个link的3D位姿。 你可以用robot state publisher作为一个单独的节点或者一个库。 3.1 作为一个单独的ROS节点运行 3.1.1 robot_state_publisher 运行机器人状态发布者最简单的方式就是作为一个节点运行。对于一般使用者来说，我们推荐这样使用。你需要两样东西来运行机器人状态发布者： - 一个从Parameter Server下载的urdf xml机器人描述。 - 一个将关节位置用sensor_msgs/JointState格式发布的源。 如何为robot_state_publisher配置参数和主题，请阅读下面的部分。 3.1.1.1 订阅的主题： joint_states(sensor_msgs/JointState) 关节位置信息 3.1.1.2 参数 robot_description(urdf map) urdf xml机器人描述。这可以通过urdf_model::initParam来完成。 tf_prefix(string) 为命名空间发布变化设置tf前缀。 publish_frequency(double) 状态发布者的发布频率，默认50赫兹。 3.1.2 例子launch文件 一旦已经设置了XML机器人描述文件和一个关节位置信息源文件，简单创建一个launch文件如下：

1

2

3

4

5

6

3.2 作为一个库运行 高级用户也可以将机器人状态发布者在他们自己的C++代码中作为一个库来使用。在你包含了头文件之后:#include 1

你需要的就是一个采用了KDL树的构造函数：RobotStatePublisher(const KDL::Tree& tree);1

现在，每次你想要发布你的机器人状态，你调用publishTransform函数即可：//publish moving joints

void publishTransforms(const std::map<:string double>& joint_positions, const ros::Time& time):

//publish fixed joints

void publishFixedTransforms();1

2

3

4

第一个变量是一个关节名称和关节位置的map，第二个参数是关节位置记录的时间。如果这个map不包括所有的关节名称也是没有问题的。如果这个map包含了一些关节名称，而这些关节名称不是运动模型的一部分，也是没有问题的。要是注意，如果你不告诉关节状态发布者你运动模型的一些关节，你的tf树将不能完成。 4.开始使用KDL解析器 4.1 构建KDL解析器rosdep install kdl_parser1

这条命令将会安装kdl_parser所有的外部依赖包。构建包，运行：rosmake kdl_parser1

4.2 使用你的代码 首先，添加KDL解析器作为一个依赖包，到你的package.xml文件中

...

...

1

2

3

4

5

在你的C++代码开始阶段，添加KDL解析器，包括下面的文件#include 1

现在，有不同的方法来继续进行，你可以从一个urdf用多种方法构建一个KDL树。 4.2.1从一个文件KDL::Tree my_tree;

if (!kdl_parser::treeFromFile("filename", my_tree)){

ROS_ERROR("Failed to construct kdl tree");

return false;

}1

2

3

4

5

创建PR2的urdf文件，运行下面的命令rosrun xacro xacro.py `rospack find pr2_description`/robots/pr2.urdf.xacro > pr2.urdf1

4.2.2 从一个参数服务器KDL::Tree my_tree;

ros::NodeHandle node;

std::string robot_desc_string;

node.param("robot_description", robot_desc_string, string());

if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){

ROS_ERROR("Failed to construct kdl tree");

return false;

}1

2

3

4

5

6

7

8

4.2.3 从一个xml元素KDL::Tree my_tree;

TiXmlDocument xml_doc;

xml_doc.Parse(xml_string.c_str());

xml_root = xml_doc.FirstChildElement("robot");

if (!xml_root){

ROS_ERROR("Failed to get robot from xml document");

return false;

}

if (!kdl_parser::treeFromXml(xml_root, my_tree)){

ROS_ERROR("Failed to construct kdl tree");

return false;

}1

2

3

4

5

6

7

8

9

10

11

12

4.2.4 从一个URDF模型KDL::Tree my_tree;

urdf::Model my_model;

if (!my_model.initXml(....)){

ROS_ERROR("Failed to parse urdf robot model");

return false;

}

if (!kdl_parser::treeFromUrdfModel(my_model, my_tree)){

ROS_ERROR("Failed to construct kdl tree");

return false;

}1

2

3

4

5

6

7

8

9

10

关于更多地细节，可以参考API文档： http://docs.ros.org/api/kdl_parser/html/namespacekdl__parser.html 5.用robot_state_publisher使用URDF 这部分教程完整的机器人URDF模型使用robot_state_publisher的例子。首先，我们创建所有URDF模型的需要的部分。然后我们写一个节点来发布关键状态和转换信息。最后，我们运行所有的部分。 5.1 创建URDF文件 这里有一个7自由度的接近R2-D2的模型。

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

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

5.2 发布状态 现在我们需要一个方法来确定机器人是在哪个状态下。为了完成这个目标，我们必须确定所有三个关节和全部的里程。从创建一个包开始：cd %TOP_DIR_YOUR_CATKIN_WS%/src

catkin_create_pkg r2d2 roscpp rospy tf sensor_msgs std_msgs1

2

然后打开编辑器，添加下面的内容到src/state_publisher.cpp文件中：#include

#include

#include

#include

int main(int argc, char** argv) {

ros::init(argc, argv, "state_publisher");

ros::NodeHandle n;

ros::Publisher joint_pub = n.advertise<: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 (tilt0) 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;

}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

5.3 Launch文件 这个launch文件假设你正在使用的包名称为“r2d2”节点名称为“state_publisher”，你已经保存了上面的urdf文件到r2d2包中

1

2

3

4

5

5.4 查看结果 首先我们应该编辑一下我们上面的代码所在的Cmake.txt文件。确认添加tf依赖。find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs tf)1

注意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})1

2

3

现在运行这个包 现在，我们应该到工作空间文件夹下构建包：catkin_make1roslaunch r2d2 display.launch1

在一个新的终端中运行rviz：rosrun rviz rviz1

选择odom作为你的固定坐标系。然后选择”Add Display“并添加一个机器人模型显示和一个TF显示。