沉默杀手HomeArchivesTagsCategories
(六) 静态障碍和自定义障碍
|路径规划TEB算法|
Word count: 933|Reading time: 4 min

对应的片段:

1
2
3
4
5
6
7
8
9
10
11
12
// clear currently existing obstacles
obstacles_.clear();   // 初始化时 obstacles_.reserve(500);
// 用代价地图信息更新 obstacle container 或者
// 用costmap_converter提供的polygons 
if (costmap_converter_)
  updateObstacleContainerWithCostmapConverter();
else
  updateObstacleContainerWithCostmap();

// 考虑custom obstacles (must be called after other updates,
//  since the container is not cleared  )
updateObstacleContainerWithCustomObstacles();

obstacles_来源于两部分：

  1. 来自代价地图updateObstacleContainerWithCostmap() 或者 CostmapConverter 的updateObstacleContainerWithCostmapConverter()

  2. 来自用户发布的障碍物消息updateObstacleContainerWithCustomObstacles()

分别由参数include_costmap_obstacles 和 costmap_converter_plugin决定，还有动态障碍include_dynamic_obstacles。 之后又对应buildGraph中的AddEdgesObstacles与AddEdgesDynamicObstacles

其实目前用的是costmap_converter

updateObstacleContainerWithCostmap

获得当前朝向的单位向量

1
2
3
4
Eigen::Vector2d  PoseSE2::orientationUnitVec() const
{
  return Eigen::Vector2d(std::cos(_theta), std::sin(_theta) );
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
// Add costmap obstacles if desired
if (cfg_.obstacles.include_costmap_obstacles)
{
  Eigen::Vector2d robot_orient = robot_pose_.orientationUnitVec();
  
  for (unsigned int i=0; i<costmap_->getSizeInCellsX()-1; ++i)
  {
    for (unsigned int j=0; j<costmap_->getSizeInCellsY()-1; ++j)
    {
      if (costmap_->getCost(i,j) == costmap_2d::LETHAL_OBSTACLE)
      {
        Eigen::Vector2d obs;
        costmap_->mapToWorld(i,j, obs.coeffRef(0), obs.coeffRef(1));

        // 检查障碍是否 not far behind the robot
        Eigen::Vector2d obs_dir = obs - robot_pose_.position();
        // if(obs_dir.norm() < 1.5)    //改变最大速度约束
        //   cfg_.robot.max_vel_x = 0.3;
        // 如果norm值过大 或者  obs_dir和robot_orient的点乘积小于0， 不算障碍
        if ( obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > cfg_.obstacles.costmap_obstacles_behind_robot_dist  )
             continue;
        // 点障碍
        obstacles_.push_back(ObstaclePtr(new PointObstacle(obs)));
      }   }   }   }

这里是参数include_costmap_obstacles唯一出现的地方。 参数costmap_obstacles_behind_robot_dist默认是1.
函数根据costmap中的障碍物信息更新障碍物容器。在这里，可以找到前方距离障碍物的最短距离。然后根据该距离值调整max_vel_x，这样优化时最大线速度的约束会随着障碍物的远近而调整，在一定程度上会改善避障效果。

自定义障碍

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
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles()
{
  // Add custom obstacles obtained via message
  boost::mutex::scoped_lock l(custom_obst_mutex_);
  // 类型 costmap_converter::ObstacleArrayMsg
  if (!custom_obstacle_msg_.obstacles.empty())
  {
    // We only use the global header to specify the obstacle coordinate system 
    // instead of individual ones
    Eigen::Affine3d obstacle_to_map_eig;
    try 
    {
      tf::StampedTransform  obstacle_to_map;
      tf_->waitForTransform(global_frame_, ros::Time(0),
            custom_obstacle_msg_.header.frame_id, ros::Time(0),
            custom_obstacle_msg_.header.frame_id, ros::Duration(0.5) );
      tf_->lookupTransform(global_frame_, ros::Time(0),
          custom_obstacle_msg_.header.frame_id, ros::Time(0), 
          custom_obstacle_msg_.header.frame_id, obstacle_to_map);
      tf::transformTFToEigen(obstacle_to_map, obstacle_to_map_eig);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s",ex.what());
      obstacle_to_map_eig.setIdentity();
    }
    for (size_t i=0; i<custom_obstacle_msg_.obstacles.size(); ++i)
    {
      if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 && 
          custom_obstacle_msg_.obstacles.at(i).radius > 0 ) // circle
      {
        Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
                             custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
                             custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
        obstacles_.push_back(ObstaclePtr(new CircularObstacle( (obstacle_to_map_eig * pos).head(2), custom_obstacle_msg_.obstacles.at(i).radius) )  );
      }
      else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 ) // point
      {
        Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
                             custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
                             custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
        obstacles_.push_back(ObstaclePtr(new PointObstacle( (obstacle_to_map_eig * pos).head(2) )));
      }
      else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 2 ) // line
      {
        Eigen::Vector3d line_start( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x,
                        custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y,
                        custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z );
        Eigen::Vector3d line_end( custom_obstacle_msg_.obstacles.at(i).polygon.points.back().x,
                        custom_obstacle_msg_.obstacles.at(i).polygon.points.back().y,
                        custom_obstacle_msg_.obstacles.at(i).polygon.points.back().z );
        obstacles_.push_back(ObstaclePtr(new LineObstacle( (obstacle_to_map_eig * line_start).head(2),
                                             (obstacle_to_map_eig * line_end).head(2) ) )  );
      }
      else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.empty())
      {
        ROS_WARN("Invalid custom obstacle received. List of polygon vertices is empty. Skipping...");
        continue;
      }
      else // polygon
      {
        PolygonObstacle* polyobst = new PolygonObstacle;
        for (size_t j=0; j<custom_obstacle_msg_.obstacles.at(i).polygon.points.size(); ++j)
        {
          Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points[j].x,
                               custom_obstacle_msg_.obstacles.at(i).polygon.points[j].y,
                               custom_obstacle_msg_.obstacles.at(i).polygon.points[j].z );
          polyobst->pushBackVertex( (obstacle_to_map_eig * pos).head(2) );
        }
        polyobst->finalizePolygon();
        obstacles_.push_back(ObstaclePtr(polyobst));
      }
      // Set velocity, if obstacle is moving
      if(!obstacles_.empty())
      {
        obstacles_.back()->setCentroidVelocity(custom_obstacle_msg_.obstacles[i].velocities, custom_obstacle_msg_.obstacles[i].orientation);
        obstacles_.back()->setDynamic();
      }
    }
  }
}

消息costmap_converter/ObstacleArrayMsg定义：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
std_msgs/Header header    # frame_id 为map
costmap_converter/ObstacleMsg[] obstacles

# costmap_converter/ObstacleMsg 的成员如下
std_msgs/Header header    # frame_id 为map
# Obstacle footprint (polygon descriptions)
geometry_msgs/Polygon polygon
# radius for circular/point obstacles
float64 radius
# Obstacle ID
# Specify IDs in order to provide (temporal) relationships
# between obstacles among multiple messages.
int64 id
# Individual orientation (centroid)
geometry_msgs/Quaternion orientation
# Individual velocities (centroid)
geometry_msgs/TwistWithCovariance velocities

类Obstacle定义在obstacle.h，派生类有PointObstacle, CircularObstacle, LineObstacle,PolygonObstacle

1
2
typedef boost::shared_ptr<Obstacle> ObstaclePtr;
typedef std::vector<ObstaclePtr>  ObstContainer;