ROS源代码阅读（7）——导航（Navigation）

源代码阅读（7）——导航（Navigation）"/>

ROS源代码阅读（7）——导航（Navigation）

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、检查四元数是否趋近于0if(tf_q.length2() < 1e-6){ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");return false;}//3、对四元数规范化，转化为vectortf_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.设置状态为PLANNINGrecovery_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 downresetState();//通知ActionServer已成功抢占ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");as_->setPreempted();//we'll actually return from execute after preemptingreturn;}}//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 cyclerecovery_index_ = 0;state_ = PLANNING;//17. 设置目标点并唤醒路径规划线程lock.lock();planner_goal_ = goal;runPlanner_ = true;planner_cond_.notify_one();lock.unlock();//publish the goal point to the visualizerROS_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 simulationros::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 wayros::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 timeif(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 outmove_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 availableros::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、多线程、智能指针有必要深入学一下，对内存和任务管理很有帮助。