Autolabor

今天发现了一个支持二次开发的开源方案，说白了就是把karto、acml、navigation stack等几个ROS开源包整合的比较漂亮，代码结构值得借鉴。

1. 执行keyboard_control之前要先执行脚本，添加键盘。

   老AS包里面没有keyboard control，可以执行teleop_twist_keyboard包。autolabor_fake是虚拟小车的driver，建模了电机、odom相关信息，订阅cmd_vel信息。控制真实小车时这个节点要替换。

2. base是对机器人底盘的仿真，launch文件的默认Fixed Frame是base_link，想要控制小车运动可以将Frame切成real_map或odom。

   todolist:

   命令行里面控制量的显示不太好看，可以尝试在源文件里面优化，添加交互提示。

   

3. stage是对场景的仿真，场景由map_server读取，launch以后就能查看当前场景地图。

   rostopic里面有一个initialpose信息，由rviz发布。rostopic list里面好多topic都是rviz发布的，在displays栏目里面取消勾选就不会发布了。

   

4. object是对障碍物的仿真，调用stage，添加interactivemarker，然后选择Interact工具，理论上地图上应该出现障碍物，但是我没找到。。。状态显示waiting for tf info。

   

   

   修正：添加的marker要在topic里面选择，不要在type栏下。然后地图上放好障碍物以后要右键apply。

   

5. lidar是对雷达点云的仿真，launch中给了一个lidar和map的静态tf，实际使用中应该给lidar和base_link的。

   todolist:

   map和real_map给的有点混乱，明天会统一一下。

6. 老AS包create_map仿真过程中，由于场景提供mapserver和slam算法同时publish了/map这个topic，要进行区分，在launch文件里面对其中一个进行remap：

   <node pkg="map_server" type="map_server" name="map_server" args="$(find simulation_launch)/map/MG_map.yaml">
       <remap from="/map" to="real_map"  />
   </node>

   

   这时rostopic里面就会出现real_map这个话题，两个地图能够同时显示。

   

代码解析

首先是simulation包：

1. autolabor_description没啥好说的，urdf文件里面定义了一个robot，整个机器人被渲染成了一个base_link，没有子节点，懒。

2. autolabor_fake包是底盘驱动，提供了一个autolabor_fake_node节点，其订阅类型为geometry_msgs/Twist的话题cmd_vel，信息来源可以是joystick／keyboard（tele_op_xxx）／cmd line（rostopic pub xxx）。其发布类型为nav_msgs/Odometry的话题odom。同时该节点还会将odom frame到base_link frame的transform信息提供给tf node，用来tf_broadcast。

   

3. lidar_simulation包自身提供了两个节点lidar_simulation和obstacle_simulation。

   3.1 LidarSimulation::getPose这个函数中有一段代码开始比较困惑：

   // ROS_INFO("roll and pitch and yaw and esp :%lf %lf %lf %lf", roll, pitch, yaw, esp);  //esp=0.000001,r&p=0.000000
   if (pow(roll,2) + pow(pitch,2) > esp){
   	start_angle = yaw + max_angle_;
   	reverse = -1.0;
   }else{  // default situation: 
   	start_angle = yaw + min_angle_;
   	reverse = 1.0;

   通常情况下，我们都使用右手坐标系，二维平面下，global_frame_到lidar_frame_的坐标变换transform欧拉角形式下的r和p角应该始终是$0.0$，yaw代表了激光雷达$x$轴的变换，加上min_angle_就切换成了激光光束的初始发射角度start_angle。如果坐标系定义反了，r&p就应该有值，这时因为坐标轴定义反过来了，激光光束的初始发射角度就变成了从正方向上的max_angle_开始的。

   3.2 LidarSimulation::updateMap这个函数值得注意，这是一个service client，用来调用地图更新，在当前功能包的默认launch文件中，只加载了一次地图，没有体现出它的作用。当执行建图任务时，因为map frame和odom frame会不断进行矫正，建图包就会call这个request来实时更新地图。

   

   3.3 该功能包下还自定义了一个obstacle service，提供obstacle_simulation节点来更新障碍物信息。这里的障碍物是指手动添加的障碍物（interactiveMarker），launch文件中可以定义其形状顶点。

   ​ 其中的ObstacleSimulation::pnpoly函数用来判断某点是否落在多边形内，之前刷算法时有考虑过这个问题，这里给出的解法不知道是不是最优的，just for record：

   bool ObstacleSimulation::pnpoly(geometry_msgs::Polygon& footprint, float& x, float& y){
     int i,j;
     bool c = false;
     for (i=0, j=footprint.points.size()-1; i<footprint.points.size(); j = i++){
       if ( ( (footprint.points.at(i).y > y) != (footprint.points.at(j).y > y) ) &&
            (x < (footprint.points.at(j).x-footprint.points.at(i).x) * (y-footprint.points.at(i).y) / (footprint.points.at(j).y - footprint.points.at(i).y) + footprint.points.at(i).x) ){
         c = !c;
       }
     }
     return c;
   }

   这里面的for，循环条件遍历的是多边形的每一条边，如$(3,0), (0,1), (1,2), (2, 3)$这样。判定的是给定点是否落在给定边的内侧，这里所谓的内侧是以给定边的起始节点为原点，给定线段的顺时针方向。

   3.4 obstacle的具体操作定义在static_map中，这里面出现了世界坐标系World，值得注意的是，栅格地图的原点在地图的一角，栅格的位置用整型来表示，而世界坐标系中栅格的位置由其中心来表示，两者相差$0.5$个resolution。lidar_simulation里面创建了一个static_map对象map_，以及回调函数LidarSimulation::obstacleHandleServer。

   3.5 lidar_simulation功能包中的这两个节点：lidar_simulation是map级的，obstacle_simulation是obstacle级的。

接下来看simulation_launch包：

这个包里面没有源代码，只提供了几个launch文件，用来仿真几种不同的情况：

• sim_move_simulation.launch就是简单的底盘控制，控制小车在给定地图上运动，同时可视化雷达点云信息。
• create_map_simulation.launch用来建图，在底盘控制的基础上，启动了建图功能包。发布默认名字为/map的topic，命令行执行map_saver保存。
• move_base_simulation.launch用来导航，在底盘控制的基础上，启动了导航套件acml&move_base，这时小车的底盘控制节点autolabor_fake_node订阅的cmd_vel信息不再来自teleop_keyboard，而是来自move_base的规划结果。

最后是move_base_sim这个功能包：

是用作真实底盘控制的（目测就是对ROS开源的move_base包的二次封装，貌似删了一些不用的插件），先skip，接下来我会直接解析ROS导航套件。