沉默杀手HomeArchivesTagsCategories
(一) TEB中的costmap converter源码部分
|路径规划TEB算法costmap converter|
Word count: 472|Reading time: 2 min

TEB默认情况下不使用Costmap Converter。事实上，此插件可以在复杂场景下极大提高运算效率，尤其是处理激光雷达分散的测量数据时，因为将障碍物视为系列孤立点效率极低。

使用costmap_converter转换障碍，结果Rviz没有显示出来，同时报警

首先确认安装sudo apt-get install -y ros-visualization-msgs，一般安装后就行了

初始化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
try
{
    costmap_converter_ = costmap_converter_loader_.createInstance(cfg_.obstacles.costmap_converter_plugin);
    std::string converter_name = costmap_converter_loader_.getName(cfg_.obstacles.costmap_converter_plugin);
    // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace
    boost::replace_all(converter_name, "::", "/");
    costmap_converter_->setOdomTopic(cfg_.odom_topic);
    costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name));
    costmap_converter_->setCostmap2D(costmap_);
    
    costmap_converter_->startWorker(ros::Rate(cfg_.obstacles.costmap_converter_rate), costmap_, cfg_.obstacles.costmap_converter_spin_thread);
    ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded.");        
}
catch(pluginlib::PluginlibException& ex)
{
    ROS_WARN("The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treated as point obstacles. Error message: %s", ex.what());
    costmap_converter_.reset();
}

配置使用的是costmap_converter::CostmapToLinesDBSRANSAC

computeVelocityCommands 函数中的调用

1
2
3
4
if (costmap_converter_)
  updateObstacleContainerWithCostmapConverter();
else
  updateObstacleContainerWithCostmap();
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
void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter()
{
  if (!costmap_converter_)
    return;
    
  //Get obstacles from costmap converter
  costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles();
  if (!obstacles)
    return;

  for (std::size_t i=0; i<obstacles->obstacles.size(); ++i)
  {
    const costmap_converter::ObstacleMsg* obstacle = &obstacles->obstacles.at(i);
    const geometry_msgs::Polygon* polygon = &obstacle->polygon;

    if (polygon->points.size()==1 && obstacle->radius > 0) // Circle
    {
      obstacles_.push_back(ObstaclePtr(new CircularObstacle(polygon->points[0].x, polygon->points[0].y, obstacle->radius)));
    }
    else if (polygon->points.size()==1) // Point
    {
      obstacles_.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y)));
    }
    else if (polygon->points.size()==2) // Line
    {
      obstacles_.push_back(ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y,
                                                        polygon->points[1].x, polygon->points[1].y )));
    }
    else if (polygon->points.size()>2) // Real polygon
    {
        PolygonObstacle* polyobst = new PolygonObstacle;
        for (std::size_t j=0; j<polygon->points.size(); ++j)
        {
            polyobst->pushBackVertex(polygon->points[j].x, polygon->points[j].y);
        }
        polyobst->finalizePolygon();
        obstacles_.push_back(ObstaclePtr(polyobst));
    }

    // Set velocity, if obstacle is moving
    if(!obstacles_.empty())
      obstacles_.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation);
  }
}