绑架估计(Kidnapping estimate)

ancientcc
  • 分数三个来源。1）未知栅格。2）点云覆盖的栅格。3）中间点覆盖的栅格。
  • 为什么在绑架估计过程中，要阻止move_base进入恢复性操作？——设想么个场景：makePlan失败，kidnap正在position_full2，若不阻止进入恢复情操作，那光position_full2执行时间段，就可能让move_base因为PLANNING_R进入两次恢复性操作，导致此次导航失改。阻止方法是在set_kidnap_vel_state，调用resetTimeouts。move_base不论在哪个状态，都会调用set_kidnap_vel_state。
  • 因为在绑架估计过程中，会阻止move_base进入恢复性操作，即进入CLEARING。那move_base处理“case CLEARING:”时，是否可认为不会在执行绑架估计？——如果主线程没有在处理“case CLEARING:”时触发manual原因的绑架估计，那是的。

 

机器人绑架，也叫机器人劫持，是外界将其放到另外一个位姿，位姿包括xy坐标和角度。

视频1 绑架估计

用户拿到机器人后，首先要建一张地图，类似数学上直角坐标系，地图上各个点是有坐标的，地图中“M”指示地图原点(0, 0)。视频中，强行挪动了两次机器人，也就是所谓的机器人绑架。红色显示的是雷达实时检测到的障碍物情况，它安装在机器人，挪动时它就发生剧烈变化。

第一次挪动到下面，因为设置目标点，开始导航了，会重新计算一次机器人此时应该的位姿。用上新位姿后，雷达实时障碍物和地图障碍物基本重合，这新位姿大体就对了。第二次挪动到起点M附近，然后单击“重定位”按钮，进行了一次手动full2重定位。

从这视频也大致可看出，解决思路是拿着雷达实时障碍物这个小图放到地图这个大地图暴力匹配，放到哪个位姿最匹配了，那这个就是应该的位姿。视频中地图尺寸是113x289，那暴力匹配次数是113x289xN，N是一个坐标上、角度候选个数。为让算法能可实用，就需减少暴力匹配次数。硬件IMU会实时提供这个角度。

 

一、优化算法

可通过预处理地图、增加点数，来提高算法准确率。

1.1 强化地图中“墙”栅格

图1 强化“墙”栅格

图1左侧是建图后栅格图。图中是条坚向过道，由于建图质量不好，使得隔出过道的两堵“墙”变得很薄。栅格值是蓝色的栅格指示它被一次点云覆盖到，点云和它们匹配时，碰到好多不是接近100的值，有的甚至是0。

为提高算法质量，需要强化属于“墙”的栅格，强化指的是增加占据栅格值。

  1. 找出属于“墙”的栅格。第一步，找出“值>=60”的栅格。第二步，以这些栅格生成轮廓（opencv），并剔除掉尺寸小于10的轮廓。认为这些障碍是房间中小物件，像箱子，不是“墙”。第三步，对留下的轮廓，以1膨胀，目的把毗邻那个栅格拉进来。这时属于“墙”的栅格就全找出来了。
  2. 对属于“墙”的栅格，栅格值+20。

图2是强化后栅格图。直观可看到，墙变宽了。具体到值，第二行58、18属于“墙”，于是都增加了20。对非“墙”部分，像中间5列，则保持着原值。对比此时蓝色文字栅格，它们都更接近100。

 

1.2 中间点

暴力匹配时，在每个候选位姿会生成一个分数，然后挑出个最好分数。在计算某位姿分数时，自然要考虑雷达点云中的点。但会遇到这么个问题，雷达点云的点不多。尤其加了机械臂后，最坏情况会失去点云中60度，即1/6个点。可比较的点数少，效果自然会变差，为提高精度，需要找到更多点。

想象下雷达中心到某个点云点的连线。如果正确，这条连线的中间点应该都没有障碍物，即占用概率是0。当然，不必累计所有中间点的占用情况，一是计算量太大，实际做不到。二来，现场路况毕竟和建图时不同。但总的来说，可以取一半距离的那个中间点。

点云中的点是占据概率越大分数越高，中间点和它相反，是占据概率越小分数越高。在代码中，ep（End Point）表示点云中的点，mp（Middle Point）表示中间点。

 

二、reposition_use_laser_scan、full_map_reposition、center_reposition

static void reposition_use_laser_scan(const sensor_msgs::LaserScan& scan_msg,
    const ::nav_msgs::MapMetaData& info, const int8_t* data, double imu_yaw, const treposition_range& range, 
	tpose2d& result_pose, tkidnap::tresult4_C* result3_ptr)

reposition_use_laser_scan功能是依据最新点云(雷达实时障碍物)，在指定范围内重计算出位姿。内中原理就是上面写的，拿着雷达实时障碍物这个小图放到地图这个大地图暴力匹配，参数result_pose存储那个最匹配位姿(xy坐标+角度)。

full_map_reposition、center_reposition分别是全地图重定位、限定区域的重定位，实现时都是调用reposition_use_laser_scan。在搜索范围上，两者角度范围一样，都是[-20, 20]，单位是度，不是弧度。不同的是区域范围，前者没有区域限制，即全地图，后者则限定一个以(center_x, center_y)为中心，半径2米的矩形。

  • [IN]scan_msg。最新雷达点云。
  • [IN]info。地图尺寸。
  • [IN]data。占据栅格图数据。
  • [IN]imu_yaw。从IMU读出的最新欧拉角。
  • [IN]range。指示要搜索范围。
  • [OUT]result_pose。暴力匹配得到的位姿。

range指示要搜索的范围，包括两个部分：区域范围和角度范围。

图2 reposition_use_laser_scan

2.1 区域范围

输入参数：radius_cm（区域半径，单位：厘米）、center_x、center_y（world坐标系下的区域中心，单位：米）。

输出：range.map_rect

map_rect存储着要搜索区域范围部分。x、y是该区域的左上角坐标，w、h是区域尺寸。结合图2，让看下如何计算map_rect。

center_x - origin.x / resolution = (-0.0070 + 2.2192 / 0.05) = 44;
center_y - origin.y / resolution = (0.00320 + 4.8847 / 0.05) = 97;

先从world坐标系参数转换到/map坐标系的栅格坐标。中心点(-0.0070, 0.0032)对应的栅格坐标是(44, 97)。

map_rect.x/y是区域的左上角坐标，于是还要减去半径(2米/0.05=40)，44 - 半径(40) = 4，即map_rect.x/y等于4/57。

map_rect.w/h是区域尺寸，区域整个在地图内时，那它就是2倍半径：2 * 40 = 80。

2.2 角度范围

输入参数：angle_radius（范围半径，单位：度）、angle_granularity（粒度，单位：度）

输出：range.yaw_offset，range.yaw_count。

yaw_offset、yaw_count联合指示了角度范围。yaw_offset是个double数组，yaw_count指示了数组中的有效角度数。结合图2，看如何计算yaw_count。

int steps = posix_pages(angle_radius, angle_granularity);
yaw_count = steps * 2 + 1;

angle_radius除以angle_granularity是整数20，即steps是20，那yaw_count就是41。然后在这个范围，从小到大等间隔计算出41个角度，angle_granularity是两角度间差值，DEG2RAD(1)=0.017453。而且中间那个单元[20]必定是0。

正如yaw_offset变量出现“offset”字眼，yaw_offset存储的是偏移，reposition_use_laser_scan使用时会固定加上值imu_yaw，yaw_offset[20]会是“imu_yaw+0”，其它类推。

 

三、执行估计，kidnap_slice

  • bool has_imu_。系统是否有imu。绑架估计要求必须存在imu。
  • int estimate_。set_estimate/clear_estimate赋值。一旦不为nposm，意味着正处在绑架估计过程。
  • bool position_changed_;
  • vel_state_t vel_state_。为让估计更准备，一轮估计时，要确保机器人处在过3个位置，如果3个位置都得到相同“位姿”，那认为这才是最终位姿。为进入不同位置，那就要移动机器人。vel_state_==vel_req指示move_base可发生移动了，vel_state_==vel_finisehd则指示move_base已移动过。“四、需要move_base移动时，move_base得移动”有关于它的详细说明。
  • tpose2d last_reposition_pose2d_。上一次重定位得到的位姿。一旦连续3个位姿相同，认为此次估计得到了最终正确结果。
  • tpose2d last_ok_estimated_pose2d_。如果此时正在估计位姿，那它是上一次绑架估计成功后得到的位姿。它只存储成功，如果此轮估计失败，值不会变。为什么要用它？1）can_estimate_CLEARING(x, y)。move_base进入恢复性操作阶段，会执行一次绑架估计。但很快又可能进入恢复性操作，间隔太短，就没必要再进入绑架估计。方法是用距离min_CLEARING_dist(2)来否决第二次绑架估计，计算这距离时，就须要知道上次绑架估计时的位置。2）require_relay_estimate(x, y)。机器人不断移动，会偏离原地图，误差是累识的，会越来越大。距上次估计超过ESTIMATE_THRESHOLD(5)米了，就自动进入估计。要算出这个距离，也须要上次估计时的处在位置。注：快到达目标点时，会触发一次原因是neargoal的绑架估计，那次条件倒和last_ok_estimated_pose2d_没关系。
  • bool apply_immediately_。外面急须一个有效机器人位姿。当是true时，kidnap在第一次算出一个位姿后，就立即告诉出来。当然后绪还是要做的，等做完了再更新结果位姿。
  • bool applied_;
  • uint32_t start_ticks_;
  • int equals_;
  • tpose2d cartographer_pose2d。tkidnap赋值，cartographer节点读值，作为cartographer节点的初始位姿。
  • tpose2d last_pose2d。global_costmap赋值。机器人最近/basefoot_print坐标系到/map坐标系的tf，即机器人位姿。
  • int moved_times。在此轮估计过程，机器人移动了次数。最多不能超过MAX_MOVE_FAILS(5)次。

绑架估计不是靠一个函数就能完成，是一个过程，持续时间会是数秒。

在某个时刻进行点云暴力匹配，即使是全地图，得到的结果也可能不是正确位姿。为避免这误差，要在多个位置进行判断，当前是3个（REQUIRE_EQUALS(2)，即连续2次相等）。要在3个位置得到“相同”值(last_reposition_pose2d_)了，才认为这是此次估计的最终位姿。

如何判断两个位姿“相同”？——两个位姿是在机器人不同位置得到的，值不可能相等，但这两个位置有个特点，它们距离不远。以上一次重定位得到的位姿last_reposition_pose2d为中心、2米半径区域内进行中心重定位(center_reposition)，如果这个位姿和此刻全地图重定位算出的位姿(full_map_pose2d)一致，那就认为这两个位姿“相同”。

在某个位置得到一个重定位位姿，但还不到两次相等，为得到下一个位置，是通过让move_base节点移动机器人。方法是tkidnap把vel_state_置vel_req，move_base发现vel_state_是vel_req，就移动。

如果之前发生过导航，那开始此次估计时存在上次导航时最终位姿，此个位姿也算是一个有效位姿(last_reposition_pose2d_)。所以幸运地话，最小只要移动两次，就可以得一个正确的估计结果。

kidnap_slice执行着估计过程中的一次时间片，执行时机是在新收到一帧雷达点云时(tros_instance::did_scan_subscribed)。一次kidnap_slice，先是full_map_reposition，后是以上一次last_reposition_pose2d为中心的center_reposition，这两种重定位最多各执行一次。

在每次kidnap_slice，首先会调用full_map_reposition进行一次全地图重定位，如果这个重定位失败，那立即结束此次估计。

equals_指示着已经相等次数，过程中可能被清零。举个例子，在位置A、位置B，已有了一次相等，equals_=1，到位置C时，发现位姿不同了，这时要清零equals_。可能会遇到这么种情况，几次移动都没有让连续3个位置得到“相同”位姿，为不让无限制估计下去，设了移动次数阈值kidnap.MAX_MOVE_FAILS(5)，一旦移动次数超过它，立即结束此次估计。

除移动次数这个阈值外，还有一个估计时长阈值（TIMEOUT_MS(25秒)），一旦估计时长超过25秒，立即结束此次估计。

绑架估计依赖IMU，具体是当中的欧拉角Z。一旦IMU没有了，立即结束此次估计。

此轮无法估计出正确位姿时，像全地图重定位失败，超过移动次数、超过评估时长，没了IMU，是不会重启cartographer节点的。

 

3.1 何时执行绑架估计

void tkidnap::set_estimate(int reason, const tpose2d& robot_pose2d);

reason：为什么要绑架估计。目前五个值：reason_goal, reason_relay, reason_neargoal, reason_clearing, reason_manual。处理分为两种，前四个做是一种，区分它们是因为调试时可以显示不同场景。最后一个是第二种，它用于测试。

robot_pose2d：最近一次机器人位姿。是在/map坐标系下的位姿，来自global_costmap。tkidnap.last_pose2d得到这个位姿。

 

3.2 kidnap_slice

1、收到一雷达点云时，会调用tros_instance::did_scan_subscribed，后者无条件调用tros_instance::kidnap_slice。

2、(kidnap_slice)已初始化mobe_base节点，并且要求绑架估计(set_estimate设置了估计原因)，进入步骤3，否则结束此轮kidnap_slice。

3、(kidnap_slice)估计过程花费时间已超过25秒，结束估计。

4、(kidnap_slice)如果机器人已按要求移动过了(vel_state_ == vel_finished)，进入步骤5，否则结束此轮kidnap_slice。

5、(kidnap_slice)进行一次全地图重定位(full_map_reposition)，结果放在full_map_pose2d。

6、(kidnap_slice)中心定位阶段。如果第5步得有一个有效的full_map_pose2d，之前又有个有效的重定位位姿(last_reposition_pose2d)，那要判断这两个是不是“相同”位姿。因为这两个位姿是在机器人不同位置得到的，值不可能相等，但这两个位置有个特点，它们距离不远。以last_reposition_pose2d的坐标为中心、2米半径区域内重定位(center_reposition)，得到结果存放在center_pose2d。如果center_pose2d和full_map_pose2d一致，那就认为这两个位姿“相同”。一旦相同，center_pose2d是个有效位姿。

7.1 前后两次重定位有着一样位姿，即center_pose2d是个有效位姿。equals_加1。(case1)如果equals_>=REQUIRE_EQUALS(2)，那认为满足相同次数了，可以结束此次绑架估计，center_pose2d将作为此次估计最终结果，进入步骤8。(case2)否则把vel_state_置vel_req，意味着还要让机器移动另一个位置，继续绑架估计，并结束此次kidnap_slice。

7.2 前后两次重定位有着不一样位姿，center_pose2d不是个有效位姿。把equals_置0。(case1)如果moved_times <= kidnap.MAX_MOVE_FAILS(5)，那意味着还要继续移动机器人，希望从新位置重新估计，并结束此次kidnap_slice。(case2)否则认为移动次数已达到阈值，不能估计继续下去了，要标记失败，结束此次绑架估计，kidnap.last_pose2d将作为一次估计最终结果，进入步骤8。

8、如果是成功评估出了位姿，即7.1的case1，以该位姿为初始位姿重启cartographer节点。

9、kidnap.clear_estimate()结束此次绑架估计。

 

四、cauclate_match_score_negative

cauclate_match_score_negative负责计算每个候选位姿时的得分。

如何计算该候选位姿时分数？直观上看，计算每个点云点处的占据值，这些值的和就是分数。也就是说，数值越大，该位姿的得分就越高。但实际在用的是取每个点云处的“100-占据值”，这些值的和才是分数。值越小，该位姿的得分反而越高。这么做的原因是为加快计算。对每次cauclate_match_score_negative，参数best_score存储着之前最好分数，一旦此次出现score大于bast_score，意味着此次已不可能比best_socre好，不必再计算后面点了，这能降低点cauclate_match_score_negative计算时间。

函数名中“_negative”字节表示数值越小，得分反越高。100则是由cartographer生成的占据栅格图时，栅格可能出现的最大值。

因为每个候选位姿都要调一次cauclate_match_score_negative，该函数执行效率严重影响暴力匹配效率。

  1. 如果原点处栅格是未知栅格(NO_INFORMATION)，认为该位姿无效。
  2. 如果原点处栅格是障碍物，认为该位姿无效。认为障碍物的方法是栅格值>=NEAR_OBSTACLE(60)。因为这个，去噪时也是以60为分隔点。
  3. 一旦超过MAX_UNKNOWN_CELLS(20)个NO_INFORMATION栅格，认为该位姿无效。
  4. 每个NO_INFORMATION栅格的值是100 * 20。这个定义有点主观，不过可以确定的是100 * 10肯定太小。另外，这个值越大，结合“_negative”，能较大减小cauclate_match_score_negative的计算时间。
static int cauclate_match_score_negative(int origin_x, int origin_y, const ::nav_msgs::MapMetaData& info, 
	const int8_t* data, const tes_point* points, int point_count, int& unknown_cells, 
	int& mp_score, int best_score)
{
  ...
  int unknown_cell_cost = MAX_CARTOGRAPHER_CELL_VAL * 20;

  int score = 0;
  for (int at = 0; at < point_count; at ++) {
    const tes_point& point = points[at];

    int new_x = origin_x + points[at].x;
    int new_y = origin_y + points[at].y;
    ...
    int value = data[getIndex(info.width, new_x, new_y)];
    if (value == (int8_t)costmap_2d::NO_INFORMATION) {
      unknown_cells ++;
      if (unknown_cells > MAX_UNKNOWN_CELLS) {
        return INT32_MAX;

此个候选位姿未知栅格太多，不考虑。

      }
      value = unknown_cell_cost;

分数第一个来源：未知栅格。

    } else if (point.ep) {
      value = MAX_CARTOGRAPHER_CELL_VAL - value;

分数第二个来源：点云覆盖的栅格。栅格分数值就是和标准值之间差值。

得分和差值比例乘数是1。有人可能认为，如果增加这里乘数，像“value=(MAX_CARTOGRAPHER_CELL_VAL - value) * 2”，可以提供准确度。——不行的。设想这么种情况，此次unknown_cells是0，即没有未知栅格，也没中间点，那最终得分score全来这里的value，socre值只是比乘数1加大一倍而已。举个例子，之前14927变成了29854，相应地，各候选分数之间差值都扩大一倍。

    } else {
      // middle-point, more little more good
      mp_score += value;

分数第三个来源：中间点覆盖的栅格。栅格分数值就是栅格值。mp_score是调试用参数，算不算，对结果无影响。

    }

    score += value;
    if (score > best_score) {
      return score;
    }
  }
  return score;
}

cauclate_match_score_negative.rar存储着一个可测试cauclate_match_score_negative的素材。包括一张地图、点云数据。可以发现unknown_cell_cost是100 * 10时，重定位还是错的。用这素材还能测角度粒度由1度缩小到0.5度，新出的-118.500比之前最好的-118.000分数好了32，不是提高很多。考虑到缩小到0.5后，计算量要增大一倍，于是采用1度的粒度。

 

五、需要move_base移动时，move_base得移动

一次雷达实时障碍物匹配有一定概率不可靠，估计过程中须要移动机器人，让在多个位置进行匹配，从而提高此次估结果的可靠性。

  1. (kidnap_slice)须要移动了，kidnap_slice让估计进入vel_req。指示move_base可以移动机器人了。
  2. (move_base)发布vel_cmd。可能要发布多次。
  3. (move_base)如果发布过vel_cmd了，让进入vel_finisehd。
  4. (kidnap_slice)发现在vel_finisehd了，继续后绪操作。如果第3步move_base发布不出vel_cmd，那会一直等，直到因为时间溢出，此次估计失败。

在move_base，发布vem_vel和让进入vel_finished，都是通过set_kidnap_vel_state。

<libros>/move_base/src/move_base.cpp
------
bool MoveBase::set_kidnap_vel_state(geometry_msgs::Twist& cmd_vel)
{
  bool valid = false;
  threading::lock lock(kidnap.last_pose2d_mutex);
  if (kidnap.vel_state() == tkidnap::vel_req) {

[1/3]ros_instance让进入vel_req。move_base发现vel_req，或继续发布cmd_vel，或让进入vel_finished。

    const tpose2d& from = kidnap.vel_req_start_pose2d;
    const tpose2d& to = kidnap.last_pose2d;
    double dist = hypot(from.x - to.x, from.y - to.y);
    double yaw_diff = fabs(angles::normalize_angle(to.yaw - from.yaw));
    uint32_t elapsed = SDL_GetTicks() - kidnap.vel_req_start_ticks;
 
    if (kidnap.vel_pub_times >= 1 && (dist >= 0.1 || yaw_diff > DEG2RAD(45) || (int)elapsed >= kidnap.MOVE_TIMEOUT_MS)) {
      // for get into 'vel_finished', cmd_vel needs to be published at least once.
      kidnap.set_vel_state(tkidnap::vel_finished, state_);

[3/3]进入vel_finished。为进入，至少需要发布过一次cmd_vel。这包括因为vel_req_start_ticks时间到了而进入。宁可让时间溢出而失败。一旦ros_instance认为此次估计成功，会修改imu航偏角，为安全修改，要确保是估计是真的成功。出现以下三种形让之一，就认为机器人是移过了。

  1. 距离上至少移动了10厘米。
  2. 角度上至少转过了45度。
  3. 此次移动至少耗费了MOVE_TIMEOUT_MS(3秒)。注意，此种情况下依旧要求发布过cmd_vel。
    } else {
      if (state_ != CONTROLLING) {
        valid = tc_->a_star_search_move(planner_goal_, cmd_vel);

在CONTROLLING，可以肯定tc_->computeVelocityCommands已得到一个可用cmd_val。

      } else {
        valid = true;
      }
      if (valid) {
        kidnap.vel_pub_times ++;
        kidnap.add_dbg_msg(tkidnap::msg_set_vel_state, tkidnap::vel_pub, ...);

[2/3]继续发布vel_cmd。vel_pub不是set_vel_state可设置的状态，引入它是为可视频kidnap输出信息。

      }
    }
    resetTimeouts();

在绑架估计过程中，要阻止move_base进入恢复性操作。阻止方法是在这里调用resetTimeouts。

  } else if (state_ == CONTROLLING) {
    valid = true;
  }
  return valid;
}

为什么在绑架估计过程中，要阻止move_base进入恢复性操作？——设想么个场景：makePlan失败，kidnap正在position_full2，若不阻止进入恢复情操作，那光position_full2执行时间段，就可能让move_base因为PLANNING_R进入两次恢复性操作，导致此次导航失改。阻止方法是在set_kidnap_vel_state，调用resetTimeouts。move_base不论在哪个状态，都会调用set_kidnap_vel_state。

 

六、机器人偏航角

上面说到的角度就是机器人偏航角。如果机器人能实时、准确给出偏航角，在暴力匹配时，角度上就没啥候选值，可以只关心枚举x、y，这极大减小匹配计算量。为得到角度，使用IMU。市面上有6轴、9轴IMU分法，6轴指的是3轴陀螺仪加3轴加速度，9轴则多了3轴磁力计。对6轴，IMU上电后，偏航角总是零，9轴则可以通过磁力计给出正确初始角度。很显然，机器人上电时没法保证偏航角总是0度，得使用9轴。可9轴中多出的磁力计却给使用条件提出了额外要求。

  • 不同纬度，地磁场不一样。一次在广州、一次在上海，同样设置下，读出的偏航角会不一样。
  • 周围含有能产出磁场物体时，会影响磁力计。像铁块、工作中的电机、雷达等。

通过在特定环境下执行校准，以及让磁力计尽可能远离能产生磁场物体等，可让磁力计保证正确。好的设备则不必用户校准，也没问题，像手机。可是，考虑到机器人使用环境，还是要求能使用6轴IMU。

6.1 基础偏航角

机器人偏航角=基础偏航角+从IMU实时读出的偏航角

假设机器人上电时偏航角是60度，因为6轴IMU初始时总是0度，这个60度就是基础偏航角。如何计算基础偏航角？——这需要进行一次full2重定位，得到的角度减去此时从IMU实时读出的偏航角，就做为接下要用的基础偏航角。

没法保证full2重定位一定能得到正确位姿，万一出现错误，需要后绪绑架估计调整过来。

 

七、进入CLEARING，执行绑架估计条件

满足以下条件中一个，就允许move_base进入CLEARING状态时，执行CLEARING原因的绑架估计。

  1. 到现在没估计成功过，
  2. 不管上次估计是成功还是失败，距上次估计结束，时间已超过9秒，
  3. 机器人距离上次估计成功位置超过2米，
bool tkidnap::can_estimate_CLEARING(double x, double y) const
{
	if (!can_estimate()) {
		return false;
	}

	if (!last_ok_estimated_pose2d_.valid) {

到现在，一次绑架估计都没有成功过，那让可以进入估计。有个疑问，因这里入口，是否会导致进入move_base无法结束导航？——不会。以PLANNING_R为例，进入，或再次进入CLEARING，会有个2秒时间间隔。第一次进入时，从这里执行了一次绑架估计。估计成功后，假设之后makePlan仍旧失败，2秒后进入CLEARING。由于没有解决PLANNING_R错误，执行下一个恢复性操作。因为这里入口，又让进入绑架估计。已执行两次恢复操作，再次CLEARING，那move_base会中止导航。

		return true;
	}

	const int min_CLEARING_interval_ms = 7000; // 7 sec (DEFAULT_OSCILLATION_TIMOUT - 3 sec)
	const int elapse = SDL_GetTicks() - last_estimated_ticks_;
	if (elapse >= min_CLEARING_interval_ms) {

为什么设置为7秒？——oscillation_timeout_是10秒，让OSCILLATION_R进入CLEARING的一定会以触发估计。因为这个，PLANNING_R要多个5秒(planner_patience_=2秒)，CONTROLLING_R要多个4秒(controller_patience_=3)，才允许它们触发估计。但如果真这么快就再次进CLEARING，那或许没必要再触发。

		return true;
	}

	double x_diff = last_ok_estimated_pose2d_.x - x;
	double y_diff = last_ok_estimated_pose2d_.y - y;
	double sq_dist = x_diff * x_diff + y_diff * y_diff;
	const double min_CLEARING_dist = 2.0; // 2.0 m
	bool result = sq_dist > min_CLEARING_dist * min_CLEARING_dist;

机器人距离上次估计成功位置超过2米，允许触发估计。

	return result;
}

但是，能进入CLEARIN状态，本身就有一个时间间隔。即使每次都让执行绑架估计，真有问题吗？我想到一个能阻止现象：目标点是障碍栅格（这个是/map坐标系，不受绑架估计结果影响），makePlan自然失败。进入CLEARING，即使经过绑架估计，后面makePlan仍旧会失败。这个7秒会让避免执行第二次CLEARING时绑架估计。

 

八、确保cartographer重启就绪后，再让move_base移动机器人

这里使用了trose_slot，那里增加一个表示cartographer是否就绪的变量node_ready_。将要重启cartotographer时，把node_ready_置为false。当运行到LocalTrajectoryBuilder2D::AddAccumulatedRangeData的AddPose，node_ready_置为true，表示cartographer节点就绪了。

void tros_instance::kidnap_slice(const sensor_msgs::LaserScan& scan_msg)
{
  ...
  if (clear_reason != nposm) {
    if (to_pose2d.valid) {
      restart_cartographer_node(cartographer_ok_esitmated);
      kidnap.set_position_changed();
      // wait until cartographer ready.
      wait_cartographer_node_ready();

此次绑架估计成功，wait_cartographer_node_ready()逻辑就是死等node_ready_变成true。因为在这时还处在绑架估计中，move_base发现正在绑架估计，不会发布cmd_vel，于是就阻止了move_base移动机器人。

    }
    kidnap.clear_estimate(clear_reason);
  }
}

因为apply_immediately导致重启cartographer时，也会执行wait_cartographer_node_ready。

 

九、调试

launcher提供了数种方法，让可查看执行reposition_use_laser_scan时更多细节。

 

9.1 保存、加载某个雷达点云(sensor_msgs::LaserScan)

你可能发现某个点云有问题，像定位总是不准，那可以保存下来。然后让重定时时固定用此点云，通过多次reposition_use_laser_scan，看发生了什么。

  1. 用save_laser_scan抓出一个需要关注的点云，存入文件，假设文件名：laser_scan-full2-unkcells.dat。
  2. 在tros_instance::did_scan_subscribed，执行kidnap_slice前，用load_laser_scan_from_file加载laser_scan-full2-unkcells.dat。取出此个LaserScan，并以它为参数调用kidnap_slice。

 

9.2 生成在指定位姿叠加了点云的栅格图图像

reposition_use_laser_scan算出一个位姿后，可以立即生成在这位姿上叠加点云后图像，方法是反变量save_png置为true。这时会生成两张图像，一张是一像素一栅格的栅格图，另一张则标示出所有栅格值。

 

9.3 specials

一次重定位置，reposition_use_laser_scan会产生大量候选位资，specials让可以关注重点几条。举个例子，发现此次重位定失败了，应该是tpose2d_C{80, 64, 1.343903}，可错误定位在了{45, 134, 4.537856}，这时就只想看这两个位姿时计算情况。

const tpose2d_C specials[] = {tpose2d_C{80, 64, 1.343903}, tpose2d_C{45, 134, 4.537856}};
const int special_count = sizeof(specials) / sizeof(specials[0]);

使用这两条语句，会输出针对两位置时，cauclate_match_score_negative执行情况日志，像统计中间分数的mp_score。

发布于 2024-02-24 19:20:13

全部评论: 0 条

    写评论: