fetch_open_auto_dock, Fetch robot automatic docking program notes-perception part

Perception of the perception part: the main idea of ​​fetch_open_auto_dock is to segment the 2d laser points, match the segmented 2d point set with the ideal point set model, and use ICP to match, so a good initial posture should be given. The matching of good initial values ​​performed badly.

The following records some details of the code, as well as some pits:

1 Construct an ideal point cloud model without pits here:

 // Front face is 300mm long
  for (double y = -0.15; y <= 0.15; y += 0.001)
  {
    geometry_msgs::Point p;
    p.x = p.z = 0.0;
    p.y = y;
    ideal_cloud_.push_back(p);
    front_cloud_.push_back(p);
  }
  // Each side is 100mm long, at 45 degree angle
  for (double x = 0.0; x < 0.05 /*0.0707106*/; x += 0.001)
  {
    geometry_msgs::Point p;
    p.x = x;
    p.y = 0.15 + x;
    p.z = 0.0;
    ideal_cloud_.push_back(p);
    p.y = -0.15 - x;
    ideal_cloud_.insert(ideal_cloud_.begin(), p);
  }

2 Publish matching point clouds, here are pits:
debug_points_ = nh.advertise <sensor_msgs :: PointCloud2> (“dock_points”, 10);
found by running the program, whether it is correctly matched or not, what is returned on rviz is the ideal point Cloud model, the code here should be changed as follows, the following code should be commented out, or return early.

    //Transform ideal cloud, and store for visualization
    candidate->points = icp_2d::transform(ideal_cloud_,
                                             transform.translation.x,
                                             transform.translation.y,
                                             icp_2d::thetaFromQuaternion(transform.rotation));

3 This program is very hypothetical, and thinks that the charging pile is directly in front of the car, so it assumes that the initial value is 1 meter directly in front of the car, and the direction is also perpendicular to the car. The code is as follows: Do not be scared by this for loop, he In fact, it breaks after performing one step, so you understand ... just like what it said before

        // If goal is invalid, set to a point directly ahead of robot
        for (size_t i = scan->ranges.size()/2; i < scan->ranges.size(); i++)
        {
            if (std::isfinite(scan->ranges[i]))
            {
                double angle = scan->angle_min + i * scan->angle_increment;
                dock_.header = scan->header;
                dock_.pose.position.x = cos(angle) * scan->ranges[i];
                dock_.pose.position.y = sin(angle) * scan->ranges[i];
                dock_.pose.orientation.x = 1.0;
                dock_.pose.orientation.y = 0.0;
                dock_.pose.orientation.z = 0.0;
                dock_.pose.orientation.w = 0.0;
                ROS_DEBUG_NAMED("dock_perception", "Set initial pose to (%f, %f, %f)",
                                dock_.pose.position.x, dock_.pose.position.y,
                                icp_2d::thetaFromQuaternion(dock_.pose.orientation));
                break;
            }
        }

4 The next two lines, the first line is useless (currently not available here). The purpose of these two lines is to convert the scan data into a format. In the constructor of ScanProcessor, there are only 1 data member, which is the current 1 frame of scan data. .

    // Cluster the laser scan
    laser_processor::ScanMask mask;
    laser_processor::ScanProcessor processor(*scan, mask);

5 The code that I think has the most effect is the following sentence. The meaning of the parameter 0.25 here is the maximum length of the docking charging pile (including the diagonal), and the function is used to split the scan data of one frame.

    processor.splitConnected(0.25);  // TODO(enhancement) parameterize //0.04

6 processor.splitConnected (0.25) detailed code analysis is as follows

void ScanProcessor::splitConnected(float thresh)
{
  std::list<SampleSet*> tmp_clusters;

  std::list<SampleSet*>::iterator c_iter = clusters_.begin();

  // For each cluster，其实就1个类
  while (c_iter != clusters_.end())
  {
    // Go through the entire list，就是循环当前一桢scan
    while ((*c_iter)->size() > 0)
    {
      // Take the first element，拿出第一个点
      SampleSet::iterator s_first = (*c_iter)->begin();

      // Start a new queue，sample_queue建立1个点集的队列，开始进行分割
      std::list<Sample*> sample_queue;
      //s_first存入sample_queue
      sample_queue.push_back(*s_first); 
     //c_iter移除放入队列的点s_first，记住sample_queue是c_iter的一部分
      (*c_iter)->erase(s_first); 

      // Grow until we get to the end of the queue ，增长直到当前sample_queue不满足要求，然后重新开始建立新的sample_queue
      std::list<Sample*>::iterator s_q = sample_queue.begin();
      while (s_q != sample_queue.end())
      {
        //expand是以当前点为起始端，预计之后包含多少个点可以包含整个充电桩，长度通过公式计算，thresh为充电桩的对大长度（包括对角线），angle_increment是scan的点间隔角度
        int expand = static_cast<int>(asin(thresh / (*s_q)->range) / scan_.angle_increment);

        SampleSet::iterator s_rest = (*c_iter)->begin();

        while ((s_rest != (*c_iter)->end() &&
                (*s_rest)->index < (*s_q)->index + expand))
        {
        //相邻2点之前的探测距离之差大于thresh，则不合格，表明这个点可能不是充电桩的点集
          if ((*s_rest)->range - (*s_q)->range > thresh)
          {
            break;
          }
          //相邻2点之前的距离之差大于thresh，则不合格，表明这个点可能不是充电桩的点集
          else if (sqrt(pow((*s_q)->x - (*s_rest)->x, 2.0f) + pow((*s_q)->y - (*s_rest)->y, 2.0f)) < thresh) //thresh
          {
            sample_queue.push_back(*s_rest);
            (*c_iter)->erase(s_rest++);
            break;
          }
          else
          {
            // 其实这里我认为可以直接break
            ++s_rest;
          }
        }
        s_q++;
      }
      //后面就好理解了，把分割好点push_back就可以了
      // Move all the samples into the new cluster
      SampleSet* c = new SampleSet;
      c->header = (*c_iter)->header;
      for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++)
        c->insert(*s_q);

      // Store the temporary clusters
      tmp_clusters.push_back(c);
    }

    // Now that c_iter is empty, we can delete
    delete(*c_iter);

    // And remove from the map
    clusters_.erase(c_iter++);
  }

  clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end());
}

Go here first