沉默杀手HomeArchivesTagsCategories
(二) 算法的初始化
|路径规划TEB算法|
Word count: 780|Reading time: 4 min

MoveBase中加载局部路径规划器

1
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);

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
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
{
  if(!initialized_)
  {
    // create Node Handle with name of plugin (as used in move_base for loading)
    ros::NodeHandle nh("~/" + name);
	        
    // get parameters of TebConfig via the nodehandle and override the default config
    cfg_.loadRosParamFromNodeHandle(nh);  // TebConfig  cfg_;
    // nh.param("trapped_threshold", trapped_threshold_, 20);
    nh.param("filter_back_velocity", filter_back_velocity_, true); 
    // nh.param("setting_acc", setting_acc_, 1.5);     

    obstacles_.reserve(500);    // ObstContainer  obstacles_;
        
    // 在TebVisualization构造函数里,注册话题 global_plan, local_plan,teb_poses, teb_markers, teb_feedback
    visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_)); 
        
    //创建robot footprint/contour model for optimization，对5种类型分别处理
    // 点类型最简单，返回新建的PointRobotFootprint共享指针
    // 对于polygon类型，只对 Point2dContainer vertices_;赋值
    RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh);
    
    // 根据参数，选择创建哪种 planner
    if (cfg_.hcp.enable_homotopy_class_planning)
    {
      planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies enabled.");
    }
    else
    {
      planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies disabled.");
    }
    // init other variables
    tf_ = tf;   // 在 move_base_node.cpp中最初定义
    // 这个就是 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
    costmap_ros_ = costmap_ros;
    // locking should be done in MoveBase
    costmap_ = costmap_ros_->getCostmap();

    // 创建 CostmapModel 共享指针
    costmap_model_ = boost::make_shared<base_local_planner::CostmapModel>(*costmap_);

    // 就是return global_frame, 实际是 "map"
    global_frame_ = costmap_ros_->getGlobalFrameID();
    cfg_.map_frame = global_frame_; // 更新cf_的成员

    // 代价地图的local frame, 实际是 "base_footprint"
    robot_base_frame_ = costmap_ros_->getBaseFrameID();

    //Initialize a costmap to polygon converter
    if (!cfg_.obstacles.costmap_converter_plugin.empty())
    {
      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();
      }
    }
    else 
      ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.");
  
    // 类型是 vector<geometry_msgs::Point>
    footprint_spec_ = costmap_ros_->getRobotFootprint();
    costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
    // 更新cf_的成员
    cfg_.robot.robot_inscribed_radius =  robot_inscribed_radius_;
    cfg_.robot.robot_circumscribed_radius = robot_circumscribed_radius; 
    
    // init the odom helper to receive the robot's velocity from odom messages
    odom_helper_.setOdomTopic(cfg_.odom_topic);

    // setup dynamic reconfigure
    dynamic_recfg_ = boost::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(nh);
    dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(&TebLocalPlannerROS::reconfigureCB, this, _1, _2);
    dynamic_recfg_->setCallback(cb);
    
    // validate optimization footprint and costmap footprint
    // 对于point类型，第1个参数是0，要求最好第1项+第3项 > 第2项
    // 否则 Infeasible optimziation results might occur frequently
    validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, 
          cfg_.obstacles.min_obstacle_dist);

    // custom obstacles的回调函数，话题 /move_base/TebLocalPlannerROS/obstacles
    custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);

    // custom via-points的回调，话题 /move_base/TebLocalPlannerROS/via_points
    via_points_sub_ = nh.subscribe("via_points", 1, &TebLocalPlannerROS::customViaPointsCB, this);
    // queue_follow 所用，话题 /rectangle_distance
    front_dist_sub_ = nh.subscribe("/rectangle_distance", 1, &TebLocalPlannerROS::frontDistanceCB, this);
    // 新添加
    footprint_sub_ = nh.subscribe("/footprint", 10, &TebLocalPlannerROS::footprintCB, this);

    
    ros::NodeHandle nh_move_base("~");
    double controller_frequency = 5;
    // 实际配置是 30
    nh_move_base.param("controller_frequency", controller_frequency, controller_frequency);
    // initialize failure detector，Detect if the robot got stucked
    failure_detector_.setBufferLength(std::round(cfg_.recovery.oscillation_filter_duration*controller_frequency));
    // 初始化，返回一个很大的数
    min_distance_custom_obstacle = std::numeric_limits<double>::max();
    
    // set initialized flag
    initialized_ = true;
    ROS_DEBUG("teb_local_planner plugin initialized.");
  }
  else
    ROS_WARN("teb_local_planner has already been initialized, doing nothing.");
}

关于RobotFootprintModelPtr，看TEB中的footprint模型

calculateMinAndMaxDistances函数和RobotFootprintModelPtr说明同时使用了两种footprint模型