实验编号： 实验指导书 实验项目： 机器人导航包 所属课程： ROS机器人操作系统基础与实战 课程代码： 面向专业： 机电专业 课程负责人： 朱笑笑 年月 日
实验编号： 实 验 指 导 书 实验项目： 机器人导航包 所属课程: ROS 机器人操作系统基础与实战 课程代码: 面向专业: 机电专业 课程负责人: 朱笑笑 年 月 日
一、实验目的 让学生掌握导航包中机器人的搭建方法。掌握导航包需要的一些信息量，对 导航包的工作流程有直观的了解。 二、 实验内容 1. 创建transform 2. 发布传感器信息 3. 发布里程计信息 4. 创建一个基座控制器 5. 创建一个环境地图 二、 实验过程或其他示意图 导航包提供了一组使用传感器、里程仪，和对机器人进行标准化控制的算法集合。可以 让你的机器人从一个位置移动到另外一个位置，而不发生意外情况如，碰撞、堵在一个位置 或者走丢了。 可以认为这个包可以用在任何移动机器人的导航应用中，但是也需要针对不同的机器人 平台做一些配置文件修改或者写一些node来调用这个导航包的功能等等。 在使用导航包前机器人必须满足一些要求： 导航包只能处理差分驱动器和全向轮机器人。机器人的形状必须是正方形或长方形。 但是，它也可以用双足机器人做某些事情，比如机器人定位，只要机器人不做侧身移动。 它要求机器人发布有关所有的关节和传感器的位置之间关系的信息。 机器人能够接受线性速度和角速度的控制 平面激光必须安装在机器人上用以创建地图和定位。或者，你可以生成一些相当于几个 激光或一个声纳，或者你可以把这些值投影到地面上，如果传感器安装在机器人的另一 个位置。 下图显示了导航包的组织结构。在白色实线框的里面是导航包提供的信息。深灰色虚线框是 可以选择的node,浅灰色虚线框内是机器人平台需要提供的node。 -2
- 2 - 一、 实验目的 让学生掌握导航包中机器人的搭建方法。掌握导航包需要的一些信息量，对 导航包的工作流程有直观的了解。 二、 实验内容 1. 创建transform 2. 发布传感器信息 3. 发布里程计信息 4. 创建一个基座控制器 5. 创建一个环境地图 三、 实验过程或其他示意图 导航包提供了一组使用传感器、里程仪，和对机器人进行标准化控制的算法集合。可以 让你的机器人从一个位置移动到另外一个位置，而不发生意外情况如，碰撞、堵在一个位置 或者走丢了。 可以认为这个包可以用在任何移动机器人的导航应用中，但是也需要针对不同的机器人 平台做一些配置文件修改或者写一些 node 来调用这个导航包的功能等等。 在使用导航包前机器人必须满足一些要求： 导航包只能处理差分驱动器和全向轮机器人。 机器人的形状必须是正方形或长方形。 但是，它也可以用双足机器人做某些事情，比如机器人定位，只要机器人不做侧身移动。 它要求机器人发布有关所有的关节和传感器的位置之间关系的信息。 机器人能够接受线性速度和角速度的控制 平面激光必须安装在机器人上用以创建地图和定位。或者，你可以生成一些相当于几个 激光或一个声纳，或者你可以把这些值投影到地面上，如果传感器安装在机器人的另一 个位置。 下图显示了导航包的组织结构。在白色实线框的里面是导航包提供的信息。深灰色虚线框是 可以选择的 node，浅灰色虚线框内是机器人平台需要提供的 node
"move base_simple/goal" geometry_msgs/PoseStamped map_server amcl nove_base /map" nav msgs/GetMap giobal_planner global_costmap intema nav msgs/Pa ecovery beha transtorms sensor topics ocaI olann cal_costmap provided node optional provided node platform specific node 1. 创建transform 导航包需要知道传感器，车轮和关节相对于机器人的位置。 为此，我们使用TF(Transform Frames)软件库。TF管理一个变换树。当然你可以手 动用数学公式来做这个变换，但是如果有很多帧需要计算，这会有点复杂和凌乱。 利用TF,我们可以为机器人添加更多的传感器和部件，而T℉将会为我们处理好所有的位 姿关系。 如果我们把激光放在坐标系base_1ink的原点以上20厘米，以后10厘米的我们需要添加 在转换树中添加一个带有这些偏移量的新的坐标系。 一旦插入和创建成功，我们可以很容易地知道激光 相对于base_link或车轮的位置。我们唯一需要做的是调用TF库并获得这个转换。 1.1发布传感器信息 在chapter7?_tutorials/src下创建一个新文件名为tf_broadcaster.cpp,输入一下代码 #include #include int main(int argc,char**argv)( ros::init(argc,argv,"robot_tf_publisher"); ros::NodeHandle n; ros::Rate r(100); tf::TransformBroadcaster broadcaster; while(n.ok0){ broadcaster.sendTransform( tf::StampedTransform( tf::Transform(tf::Quaternion(0,0,0,1),tf::Vector3(0.1, 0.0,0.2) 3
- 3 - 1. 创建transform 导航包需要知道传感器，车轮和关节相对于机器人的位置。 为此，我们使用TF（Transform Frames）软件库。TF管理一个变换树。 当然你可以手 动用数学公式来做这个变换，但是如果有很多帧需要计算，这会有点复杂和凌乱。 利用TF，我们可以为机器人添加更多的传感器和部件，而TF将会为我们处理好所有的位 姿关系。 如果我们把激光放在坐标系base_link的原点以上20厘米，以后10厘米的我们需要添加 在转换树中添加一个带有这些偏移量的新的坐标系。 一旦插入和创建成功，我们可以很容易地知道激光 相对于base_link或车轮的位置。 我们唯一需要做的是调用TF库并获得这个转换。 1.1发布传感器信息 在chapter7_tutorials/src下创建一个新文件名为tf_broadcaster.cpp,输入一下代码 #include #include int main(int argc, char** argv){ ros::init(argc, argv, "robot_tf_publisher"); ros::NodeHandle n; ros::Rate r(100); tf::TransformBroadcaster broadcaster; while(n.ok()){ broadcaster.sendTransform( tf::StampedTransform( tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2))
ros::Time::now(,"base_link","base_laser")); r.sleep(; } 在CMakelist.txt中添加以下代码创建新的可执行文件 rosbuild_add_executable(tf_broadcaster src/tf_broadcaster.cpp) 这样我们可以创建另一个node来使用这个变换，可以得到传感器坐标系的一个点在 base link坐标系下的坐标。 1.2创建一个监听器 在chapter77_tutorials/src下创建一个新文件名为tf_listener.cpp,输入一下代码 #include #include #include void transformPoint(const tf::TransformListener&listener){ //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame geometry_msgs::PointStamped laser_point; laser_point.header.frame_id ="base_laser"; //we'll just use the most recent transform available for our simple example laser_point.header.stamp ros::Time0; //just an arbitrary point in space laser_point.point.x =1.0; laser_point.point.y =2.0; laser_point.point.z =0.0; geometry_msgs::PointStamped base_point; listener.transformPoint("base_link",laser_point,base_point); ROS_INFO("base_laser:(%.2f,%.2f.%.2f)----->base_link:(%.2f, %.2f,%.2f)at time %.2f", laser_point.point.x,laser_point.point.y,laser_point.point.z, base_point.point.x,base_point.point.y,base_point.point.z, base_point.header.stamp.toSec(); ROS_ERROR("Received an exception trying to transform a point from -4-
- 4 - ros::Time::now(),"base_link", "base_laser")); r.sleep(); } } 在CMakelist.txt中添加以下代码创建新的可执行文件 rosbuild_add_executable(tf_broadcaster src/tf_broadcaster.cpp) 这样我们可以创建另一个node来使用这个变换，可以得到传感器坐标系的一个点在 base_link坐标系下的坐标。 1.2创建一个监听器 在chapter7_tutorials/src下创建一个新文件名为tf_listener.cpp,输入一下代码 #include #include #include void transformPoint(const tf::TransformListener& listener){ //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame geometry_msgs::PointStamped laser_point; laser_point.header.frame_id = "base_laser"; //we'll just use the most recent transform available for our simple example laser_point.header.stamp = ros::Time(); //just an arbitrary point in space laser_point.point.x = 1.0; laser_point.point.y = 2.0; laser_point.point.z = 0.0; geometry_msgs::PointStamped base_point; listener.transformPoint("base_link", laser_point, base_point); ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", laser_point.point.x, laser_point.point.y, laser_point.point.z, base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); ROS_ERROR("Received an exception trying to transform a point from
\"base_laser to \"base_link\":%s",ex.what(); int main(int argc,char**argv)( ros::init(argc,argv,"robot_tf_listener"); ros::NodeHandle n; tf::TransformListener listener(ros::Duration(10)); //we'll transform a point once every second ros::Timer timer n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint,boost::ref(listener))); ros::spin(; } 同样在CMakelist..tbxt中添加新可执行文件。编译包并运行两个node rosmake chapter7_tutorials rosrun chapter7_tutorials tf_broadcaster rosrun chapter7_tutorials tf_listener 可以在终端上获得以下输出 [INFO][1368521854.336910465J:base_laser::(1.00,2.00.0.00)--> base_link:(1.10,2.00,0.20)at time1368521854.33 [INFo][1368521855.336347545J:base_laser::(1.00,2.00.0.00)--> base_link:(1.10,2.00,0.20)at time1368521855.33 这就意味着在节点中发布的相对于base_laser的一个(1,2,0)的点，在base_Iink中的位 置为(1.1,2,0.2)。 这样可以看到，tf函数库提供了所以需要数学函数可以用来获得一个点或者joit在其他 坐标系中的位姿。 一个TF树定义了所有不同坐标系间的移动和旋转变换。 测试以下例子，进一步理解℉。使用前面D模型和仿真实验中完成的一个小车，并在车 背上添加一个其它的laser -5-
- 5 - \"base_laser\" to \"base_link\": %s", ex.what()); } int main(int argc, char** argv){ ros::init(argc, argv, "robot_tf_listener"); ros::NodeHandle n; tf::TransformListener listener(ros::Duration(10)); //we'll transform a point once every second ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener))); ros::spin(); } 同样在CMakelist.txt中添加新可执行文件。编译包并运行两个node $ rosmake chapter7_tutorials $ rosrun chapter7_tutorials tf_broadcaster $ rosrun chapter7_tutorials tf_listener 可以在终端上获得以下输出 [ INFO] [1368521854.336910465]: base_laser: (1.00, 2.00. 0.00) -----> base_link: (1.10, 2.00, 0.20) at time 1368521854.33 [ INFO] [1368521855.336347545]: base_laser: (1.00, 2.00. 0.00) -----> base_link: (1.10, 2.00, 0.20) at time 1368521855.33 这就意味着在节点中发布的相对于base_laser的一个（1,2,0）的点，在base_link中的位 置为（1.1,2，0.2）。 这样可以看到，tf函数库提供了所以需要数学函数可以用来获得一个点或者joint在其他 坐标系中的位姿。 一个TF树定义了所有不同坐标系间的移动和旋转变换。 测试以下例子，进一步理解TF。使用前面3D模型和仿真实验中完成的一个小车，并在车 背上添加一个其它的laser
1.3查看变换树 可以用tf工具来查看机器人的变换树结构，首先用前面额例子载入一个机器人系统。 roslaunch chapter7_tutorials gazebo_map_robot.launch model:="rospack find chapter7_tutorials/urdf/robot1_base_04.xacro" rosrun tf view_frames 可以得到以下的结果 d密e1455343707得 现在再次运行tf broadcasterf的node然后用view frame来观察新创建的坐标系 rosrun chapter7_tutorials tf_broadcaster rosrun tf view_frames 将会得到如下结果，红圈中为新添加的坐标系 5m 2.发布传感器信息 一个机器人可以使用很多传感器来观察世界，可以通过开发许多的ode来使用这些数据 做一些事情，但是导航包目前仅使用平面激光传感器的数据。所以，确保机器人的传感器 topic包含：sensor_.msgs/LaserScan或者sensor_.msgs/PointCloud:类型数据。 这里将使用机器人前端的激光传感器来在Gazebo中进行导航。注意这里的激光传感器 是在Gazebo中仿真出来的，发布的topic为base_scan/scan 在这里我们无需配置我们的激光传感器，因为在urdf文件里己经有了相对机器人的位姿 信息数据。 首先，可以查看这个消息sensor_.msgs/LaserScan的数类型。 -6-
- 6 - 1.3查看变换树 可以用tf工具来查看机器人的变换树结构，首先用前面额例子载入一个机器人系统。 $ roslaunch chapter7_tutorials gazebo_map_robot.launch model:="`rospack find chapter7_tutorials`/urdf/robot1_base_04.xacro" $ rosrun tf view_frames 可以得到以下的结果 现在再次运行tf_broadcaster的node然后用view_frame来观察新创建的坐标系 $ rosrun chapter7_tutorials tf_broadcaster $ rosrun tf view_frames 将会得到如下结果，红圈中为新添加的坐标系 2. 发布传感器信息 一个机器人可以使用很多传感器来观察世界，可以通过开发许多的node来使用这些数据 做一些事情，但是导航包目前仅使用平面激光传感器的数据。所以，确保机器人的传感器 topic包含：sensor_msgs/LaserScan 或者sensor_msgs/PointCloud类型数据。 这里将使用机器人前端的激光传感器来在Gazebo中进行导航。注意这里的激光传感器 是在Gazebo中仿真出来的，发布的topic为base_scan/scan. 在这里我们无需配置我们的激光传感器，因为在urdf文件里已经有了相对机器人的位姿 信息数据。 首先，可以查看这个消息sensor_msgs/LaserScan的数类型
rosmsg show sensor_msgs/LaserScan std_msgs/Header header uint32 seq time stamp string frame_id float32 angle_min float32 angle_max float32 angle_increment float32 time_increment float32 scan_time float32 range_min float32 range_max float320 ranges float320 intensities 2.1创建一个激光node 在chapter77_tutorials/src文件夹创建一个新的laser..cpp然后输入以下代码。 #include #include int main(int argc,char**argv){ ros::nit(argc,argv,"laser_scan_publisher"); ros::NodeHandle n; ros::Publisher scan_pub n.advertise("scan",50) unsigned int num_readings =100; double laser_frequency =40; double ranges[num_readings]; double intensities[num_readings]; int count =0; ros::Rate r(1.0); while(n.ok(){ //generate some fake data for our laser scan for(unsigned inti =0;i num_readings;++i) -7-
- 7 - $ rosmsg show sensor_msgs/LaserScan std_msgs/Header header uint32 seq time stamp string frame_id float32 angle_min float32 angle_max float32 angle_increment float32 time_increment float32 scan_time float32 range_min float32 range_max float32[] ranges float32[] intensities 2.1创建一个激光node 在chapter7_tutorials/src文件夹创建一个新的laser.cpp然后输入以下代码。 #include #include int main(int argc, char** argv){ ros::init(argc, argv, "laser_scan_publisher"); ros::NodeHandle n; ros::Publisher scan_pub = n.advertise("scan", 50); unsigned int num_readings = 100; double laser_frequency = 40; double ranges[num_readings]; double intensities[num_readings]; int count = 0; ros::Rate r(1.0); while(n.ok()){ //generate some fake data for our laser scan for(unsigned int i = 0; i < num_readings; ++i){
ranges[i]count; intensities[i]100 count; } ros::Time scan_time ros::Time::now(; //populate the LaserScan message sensor_msgs::LaserScan scan; scan.header.stamp scan_time; scan.header.frame_id "base_link"; scan.angle_min =-1.57; scan.angle_max 1.57; scan.angle_increment 3.14/num_readings; scan.time_increment =(1/laser_frequency)/(num_readings); scan.range_min 0.0; scan.range_max 100.0; scan.ranges.resize(num_readings); scan.intensities.resize(num_readings); for(unsigned int i =0;i num_readings;++i){ scan.ranges[i]ranges[i]; scan.intensities[i]=intensities[i]; scan_pub.publish(scan); ++count r.sleep(; } } 可以看到，我们创建一个名为scan数据类型为sensor_.msgs/儿aserScan的topic。.这个消 息的名字一定要正确，因为后续导航包将会读取这个topc。以下代码中设定了该消息名字。 ros::Publisher scan_pub n.advertise("scan", 50: 同时，在发布topic的时候类似于header,stamp,frame_id等很多的元素都需要发布， 否则导航包也会无法正常工作 scan.header.stamp scan_time; scan.header.frame_id "base_link"; -8-
- 8 - ranges[i] = count; intensities[i] = 100 + count; } ros::Time scan_time = ros::Time::now(); //populate the LaserScan message sensor_msgs::LaserScan scan; scan.header.stamp = scan_time; scan.header.frame_id = "base_link"; scan.angle_min = -1.57; scan.angle_max = 1.57; scan.angle_increment = 3.14 / num_readings; scan.time_increment = (1 / laser_frequency) / (num_readings); scan.range_min = 0.0; scan.range_max = 100.0; scan.ranges.resize(num_readings); scan.intensities.resize(num_readings); for(unsigned int i = 0; i ("scan", 50); 同时，在发布topic 的时候类似于header，stamp，frame_id等很多的元素都需要发布， 否则导航包也会无法正常工作 scan.header.stamp = scan_time; scan.header.frame_id = "base_link";
frame_id是header中非常重要的属性。必须是在urdf中创建，同时在tf变换树种发布了 变换。这样导航包就可以利用这个信息，知道传感器实际的位置，并且将传感器数据变换到 机器人坐标系中。 Fn水p 0易 Too.Propertes E +2,crk lur■2 2D Ne Guul coal 01.Pontda... ,2DoeE安m3e 05.Lassrscn.. 了 ntNpete 7 SiLuOK h Tepc 1G=905T. Tnde,下ndcrn0E Tse ott Tapit eat a Style on世 别康 1 DeteyTrte D Q天LaserScan [Laverscan D651eHMh证TA TGtu长4e位nr有 护0tetl#e2 dereAn m pt、 Ad iorrove rootome me 22 wel nmeMwtl Fleed s0M Rosme Rntrwed 5iM 利用这个模版，就可以使用任何一个激光传感器，即使没有OS的驱动。只需要将上例 中的假激光数据替换为激光传感器的实际数据。这个模版还可以用来创建看起来像激光的传 感器。比如，利用立体激光或者超声传感器仿真一个激光传感器。 3.发布里程计信息 3.1导航包还需要获得机器人的里程计信息。 里程是相对与一个点的距离。在这里，就是机器人的base_Iink与odom坐标系中一个固 定点间的位移。导航包使用的里程信息的数据类型是nav_msgs/Odometry.我们使用rosmsg 来查看结构类型。 S rosmsg show nav_msgs/Odometry -9
- 9 - frame_id是header中非常重要的属性。必须是在urdf中创建，同时在tf变换树种发布了 变换。这样导航包就可以利用这个信息，知道传感器实际的位置，并且将传感器数据变换到 机器人坐标系中。 利用这个模版，就可以使用任何一个激光传感器，即使没有ROS的驱动。只需要将上例 中的假激光数据替换为激光传感器的实际数据。这个模版还可以用来创建看起来像激光的传 感器。比如，利用立体激光或者超声传感器仿真一个激光传感器。 3. 发布里程计信息 3.1导航包还需要获得机器人的里程计信息。 里程是相对与一个点的距离。在这里，就是机器人的base_link与odom坐标系中一个固 定点间的位移。导航包使用的里程信息的数据类型是nav_msgs/Odometry.我们使用rosmsg 来查看结构类型。 $ rosmsg show nav_msgs/Odometry
std msgs/Header header uint32 seq time sLarp string framc id string child frane 1d gearetry nsgs/PosewithCavar Lance pose gcometry msgs/Pase pose geonetry_msos/Potnt posttton fLoat64× Float64 y float64 z geonetry_nsgs/Quaternion ortentatton float64 x float64 y float64 z float64 w Float64[30]covariance georetry_nsgs/Twtstwlthcovartance twist geometry_msgs/Twist twist gepnetry_msgs/vector3 linear float64 x float64 y fLoat64 z geonetry msgs/vertor3 angular float64 x fLoat64 y float64 z float64[36]covariance 可以看到在里程消息结构中，nav_msgs/odometry?给出的是坐标系frame_id和 child frame id之间的位移。具体包含了位姿pose和速度矢量。 Pos有两个结构体一个是表示欧式空间的位置另一个是用四元素表示的机器人朝向。 这个朝向是表示机器人的方向的上的位移。 速度矢量包含了线性速度和角速度。针对本实验中的机器人，仅仅使用道x向线速度和z 向角速度。这样我们可以知道，机器人前进还是后退，左转还是右转。 因为里程是两个frame之间的位移，所以必须要发布两个坐标系frame之间的TF。 3.2 Gazebo?如何创建里程 正如我们所见，机器人在Gazebof的仿真环境中和真实机器人在实际环境中运动是一样的。我 们用了一个diffdrive plugin差速驱动器来控制两个轮子的运动。 正是这个差速驱动器发布了虚拟环境中的里程信息，所以就不在需要我们去写这个里程信息 的发布功能了。 执行Gazebo中的机器人仿真实例来观察里程工作的情况。 S roslaunch chapter7_tutorials gazebo xacro.launch model:=""rospack find chapter7_tutorials'/urdf/robot1_base 04.xacro" S rosrun erratic_teleop erratic_keyboard_teleop 利用键盘操作模式，可以控制机器人移动一段时间产生新的里程topic。 在Gazebot仿真器中，点击robot modell,可以看到物体的属性。点击其中的pose,可以看到各 个属性的值。移动机器人查看相应的变化。 -10-
- 10 - 可以看到在里程消息结构中，nav_msgs/odometry给出的是坐标系frame_id和 child_frame_id之间的位移。具体包含了位姿pose和速度矢量。 Pose 有两个结构体一个是表示欧式空间的位置另一个是用四元素表示的机器人朝向。 这个朝向是表示机器人的方向的上的位移。 速度矢量包含了线性速度和角速度。针对本实验中的机器人，仅仅使用道x向线速度和z 向角速度。这样我们可以知道，机器人前进还是后退，左转还是右转。 因为里程是两个frame之间的位移，所以必须要发布两个坐标系frame之间的TF。 3.2 Gazebo如何创建里程 正如我们所见，机器人在Gazebo的仿真环境中和真实机器人在实际环境中运动是一样的。我 们用了一个diffdrive_plugin 差速驱动器来控制两个轮子的运动。 正是这个差速驱动器发布了虚拟环境中的里程信息，所以就不在需要我们去写这个里程信息 的发布功能了。 执行Gazebo中的机器人仿真实例来观察里程工作的情况。 $ roslaunch chapter7_tutorials gazebo_xacro.launch model:="`rospack find chapter7_tutorials`/urdf/robot1_base_04.xacro" $ rosrun erratic_teleop erratic_keyboard_teleop 利用键盘操作模式，可以控制机器人移动一段时间产生新的里程topic。 在Gazebo仿真器中，点击robot_model1,可以看到物体的属性。点击其中的pose，可以看到各 个属性的值。移动机器人查看相应的变化
