2021SC@SDUSC
ROS源代码阅读（7）

导航与定位是机器人研究中的重要部分。 一般机器人在陌生的环境下需要使用激光传感器（或者深度传感器转换成激光数据），先进行地图建模，然后在根据建立的地图进行导航、定位。在ROS中也有很多完善的包可以直接使用。 在ROS中，进行导航需要使用到的三个包是： （1） move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置； （2） gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图； （3） amcl：根据已经有的地图进行定位。

在这里主要看move_base的代码
入口为文件move_base_node.cpp，声明了move_base::MoveBase对象move_base，传入参数为tf2_ros::Buffer& tf 。声明的时候，便进入了构造函数。但是在看构造函数前，我们先来看一下在move_base这个命名空间中都有哪些数据。
进入入口的命名空间，命名空间为move_base。接下来挨个介绍一下命名空间move_base的内容：
声明server端，消息类型是move_base_msgs::MoveBaseAction

typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer; 

枚举movebase状态表示

enum MoveBaseState { 
     PLANNING,
     CONTROLLING,
     CLEARING
   };

枚举，触发恢复模式

enum RecoveryTrigger
  {
    PLANNING_R,
    CONTROLLING_R,
    OSCILLATION_R
  };

MoveBase类，使用actionlib：：ActionServer接口，该接口将robot移动到目标位置

class MoveBase{ }

然后对于movebase类，主要看文件move_base.cpp中定义

入口是构造函数MoveBase::MoveBase。
首先，初始化了一堆参数：

tf_(tf), //tf2_ros::Buffer& 引用？取址？
    as_(NULL), //MoveBaseActionServer* 指针
    planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),//costmap的实例化指针
    bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"), //nav_core::BaseGlobalPlanner类型的插件
    blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),//nav_core::BaseLocalPlanner类型的插件
    recovery_loader_("nav_core", "nav_core::RecoveryBehavior"), //nav_core::RecoveryBehavior类型的插件
    planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),//三种规划器，看触发哪种
    runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), //配置参数
    new_global_plan_(false)  //配置参数

创建move_base action,绑定回调函数。定义一个名为move_base的SimpleActionServer。该服务器的Callback为moveBase::executeCb

 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

这个时候调用回调函数MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)，具体如下：
如果目标非法，则直接返回：

if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
         as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
         return;
     }

其中，isQuaternionValid(const geometry_msgs::Quaternion& q)函数如下，主要用于检查四元数的合法性：

bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){
    //1、首先检查四元数是否元素完整
    if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
        ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal");
        return false;
    }
    tf2::Quaternion tf_q(q.x, q.y, q.z, q.w);
    //2、检查四元数是否趋近于0
    if(tf_q.length2() < 1e-6){
        ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");
        return false;
    }
    //3、对四元数规范化，转化为vector
    tf_q.normalize();
    tf2::Vector3 up(0, 0, 1);
    double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
    if(fabs(dot - 1) > 1e-3){
        ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
        return false;
    }

    return true;
}

回到回调函数继续看

将目标的坐标系统一转换为全局坐标系：

geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);

其中，函数MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg)如下：

geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){
    std::string global_frame = planner_costmap_ros_->getGlobalFrameID();
    geometry_msgs::PoseStamped goal_pose, global_pose;
    goal_pose = goal_pose_msg;

    //tf一下
    goal_pose.header.stamp = ros::Time();

    try{
        tf_.transform(goal_pose_msg, global_pose, global_frame);
    }
    catch(tf2::TransformException& ex){
        ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
                 goal_pose.header.frame_id.c_str(), global_frame.c_str(), ex.what());
        return goal_pose_msg;
    }

    return global_pose;
}

设置目标点并唤醒路径规划线程：

boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
     planner_goal_ = goal;
     runPlanner_ = true;
     planner_cond_.notify_one();
     lock.unlock();

然后发布goal，设置控制频率：

current_goal_pub_.publish(goal);
     std::vector<geometry_msgs::PoseStamped> global_plan;
     ros::Rate r(controller_frequency_);

开启costmap更新：

if(shutdown_costmaps_){
         ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
         planner_costmap_ros_->start();
         controller_costmap_ros_->start();
     }

开启循环，循环判断是否有新的goal抢占

while(n.ok())
    {

        //7. 修改循环频率
        if(c_freq_change_)
        {
            ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
            r = ros::Rate(controller_frequency_);
            c_freq_change_ = false;
        }

        //8. 如果获得一个抢占式目标
        if(as_->isPreemptRequested()){ //action的抢断函数
            if(as_->isNewGoalAvailable()){
                //如果有新的目标，会接受的，但不会关闭其他进程
                move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();

                //9.如果目标无效,则返回
                if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
                    as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
                    return;
                }
                //10.将目标转换为全局坐标系
                goal = goalToGlobalFrame(new_goal.target_pose);

                //we'll make sure that we reset our state for the next execution cycle
                //11.设置状态为PLANNING
                recovery_index_ = 0;
                state_ = PLANNING;

                //we have a new goal so make sure the planner is awake
                //12. 设置目标点并唤醒路径规划线程
                lock.lock();
                planner_goal_ = goal;
                runPlanner_ = true;
                planner_cond_.notify_one();
                lock.unlock();

                //13. 把goal发布给可视化工具
                ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
                current_goal_pub_.publish(goal);

                //make sure to reset our timeouts and counters
                //14. 重置规划时间
                last_valid_control_ = ros::Time::now();
                last_valid_plan_ = ros::Time::now();
                last_oscillation_reset_ = ros::Time::now();
                planning_retries_ = 0;
            }
            else {

                //14.重置状态,设置为抢占式任务
                //if we've been preempted explicitly we need to shut things down
                resetState();

                //通知ActionServer已成功抢占
                ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
                as_->setPreempted();

                //we'll actually return from execute after preempting
                return;
            }
        }

        //we also want to check if we've changed global frames because we need to transform our goal pose
        //15.如果目标点的坐标系和全局地图的坐标系不相同
        if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
            //16,转换目标点坐标系
            goal = goalToGlobalFrame(goal);

            //we want to go back to the planning state for the next execution cycle
            recovery_index_ = 0;
            state_ = PLANNING;

            //17. 设置目标点并唤醒路径规划线程
            lock.lock();
            planner_goal_ = goal;
            runPlanner_ = true;
            planner_cond_.notify_one();
            lock.unlock();

            //publish the goal point to the visualizer
            ROS_DEBUG_NAMED("move_base","The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
            current_goal_pub_.publish(goal);

            //18.重置规划器相关时间标志位
            last_valid_control_ = ros::Time::now();
            last_valid_plan_ = ros::Time::now();
            last_oscillation_reset_ = ros::Time::now();
            planning_retries_ = 0;
        }

        //for timing that gives real time even in simulation
        ros::WallTime start = ros::WallTime::now();

        //19. 到达目标点的真正工作，控制机器人进行跟随
        bool done = executeCycle(goal, global_plan);

        //20.如果完成任务则返回
        if(done)
            return;

        //check if execution of the goal has completed in some way

        ros::WallDuration t_diff = ros::WallTime::now() - start;
        ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());
        //21. 执行休眠动作
        r.sleep();
        //make sure to sleep for the remainder of our cycle time
        if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
            ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
    }

其中，done的值即为MoveBase::executeCycle（）函数的返回值，这个值非常重要，直接判断了是否到达目标点；MoveBase::executeCycle（）函数是控制机器人进行跟随的函数，如下：

获取机器人当前位置：

geometry_msgs::PoseStamped global_pose;
    getRobotPose(global_pose, planner_costmap_ros_);
    const geometry_msgs::PoseStamped& current_position = global_pose;

    //push the feedback out
    move_base_msgs::MoveBaseFeedback feedback;
    feedback.base_position = current_position;
    as_->publishFeedback(feedback);

其中，getRobotPose（）函数如下，需要对准坐标系和时间戳：

bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap)
{
    tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
    geometry_msgs::PoseStamped robot_pose;
    tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
    robot_pose.header.frame_id = robot_base_frame_;
    robot_pose.header.stamp = ros::Time(); // latest available
    ros::Time current_time = ros::Time::now();  // save time for checking tf delay later

    // 转换到统一的全局坐标
    try
    {
        tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID());
    }
    catch (tf2::LookupException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ConnectivityException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ExtrapolationException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
        return false;
    }

    // 全局坐标时间戳是否在costmap要求下
    if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
    {
        ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " \
                               "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(),
                          current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
        return false;
    }

    return true;
}

在这一部分中，需要注意的地方 ：

1、move_base与costmap_2d、全局规划器、局部规划器、恢复器紧密相连，很多地方都是调用了这几个包的函数。
2、需要着重关注 头文件 中写的那些枚举常量，以及各个标志位，很多都是正文中的判断条件。
3、多线程、智能指针有必要深入学一下，对内存和任务管理很有帮助。