沉默杀手HomeArchivesTagsCategories
move_base分析（四） planThread线程
|路径规划move_base分析|
Word count: 755|Reading time: 3 min

MoveBase::planThread()

三个函数唤醒了planThread线程: MoveBase::wakePlanner MoveBase::executeCb MoveBase::executeCycle

MoveBase::planThread()

第一部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
ros::NodeHandle n;
ros::Timer timer;
bool wait_for_wake = false;   // 先不唤醒线程
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
while(n.ok() )
{
    // check if we should run the planner (the mutex is locked)
    while(wait_for_wake || !runPlanner_){
    //if we should not be running the planner then suspend this thread
        log.debug("[move_base_plan_thread] %s","Planner thread is suspending");
        planner_cond_.wait(lock);
        wait_for_wake = false;
    }
    // 开启线程
    ros::Time start_time = ros::Time::now();
    //time to plan! get a copy of the goal and unlock the mutex
    geometry_msgs::PoseStamped temp_goal = planner_goal_;
    lock.unlock();
    ROS_DEBUG_NAMED("move_base_plan_thread",  "Planning...");
    // run planner
    planner_plan_->clear();
    bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
    ......
}

前面就是定义一些变量和unique_lock,然后进入一个大while循环,嵌套一个while循环,由于runPlanner_在构造函数里声明为false,所以条件为真，直接锁住了这个线程，wait_for_wake仍为false

在move_base还未完全启动时，线程planThread就开始运行了，然后才会从地图数据加载代价地图。所以在启动时，不要在线程里访问一些未初始化的变量。

线程一直运行在while循环

1
2
3
while(wait_for_wake || !runPlanner_ )
{
}

我在move_base里定义了一个发布者pub，如果在这个while循环里执行pub.publish(msg)，move_base就会出错退出。可以使用智能指针，其实Publisher该有个函数，比如isValid来验证是否可用。我发现用getTopic()函数也可以，如果结果不是空，说明发布者和roscore的通信成功了，此时可以调用publish函数了。

在上面的三个函数唤醒线程后，就往下运行，获得临时目标点，clear planner后，进入makePlan

第二部分

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
if(gotPlan)
{
    ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
    //pointer swap the plans under mutex (the controller will pull from latest_plan_)
    std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;
    
    lock.lock();
    // 交换变量，赋值给 latest_plan_
    planner_plan_ = latest_plan_;
    latest_plan_ = temp_plan;
    last_valid_plan_ = ros::Time::now();
    planning_retries_ = 0;
    new_global_plan_ = true;
    
    //make sure we only start the controller if we still haven't reached the goal
    if(runPlanner_)
        state_ = CONTROLLING;
    if(planner_frequency_ <= 0)
        runPlanner_ = false;
    lock.unlock();
}
// 如果没找到路径，而且还在 PLANNING 状态(机器人不动)
else if(state_==PLANNING)
{
    ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
    //check if make a plan for over time limit or maximum number of retries
    lock.lock();
    // lock是用于这个非原子操作
    planning_retries_++;
    // 如果参数 max_planning_retries还是默认值 -1，那转为uint32_t就是一个很大的数，就是不考虑重试次数
    if(runPlanner_ &&
            (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_)) )
    {
        // 进入 CLEARING 状态
        state_ = CLEARING;
        runPlanner_ = false;  // proper solution for issue #523
        publishZeroVelocity();      // 车不准动
        recovery_trigger_ = PLANNING_R;
    }
    lock.unlock();
}
//if can't get new plan, robot will move with last global plan
// 其实跟上面的情况一样
else if(state_ == CONTROLLING)
{
    movebase_log.warn("Robot in controlling, but can't get new global plan ");
    publishZeroVelocity();
    state_ = CLEARING;
    recovery_trigger_ = PLANNING_R;
}
//take the mutex for the next iteration
lock.lock();
if(planner_frequency_ > 0)
{
    ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
    if (sleep_time > ros::Duration(0.0)  )
    {
        wait_for_wake = true;
        // wakePlanner的函数只有一句: planner_cond_.notify_one();
        timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
    }
}

最后的部分就是根据全局路径规划频率，调用makePlan的地方。