ROS Navigation源代码剖析(1)-move_base 线程框架

         Navigation是机器人最基本的功能之一，ROS为我们提供了一整套Navigation的解决方案，包括全局与局部的路径规划、代价地图、异常行为恢复、地图服务器等等，这些开源工具包极大地方便了移动机器人导航功能的开发和部署。

         ROS的导航功能是如何实现的？如果想基于ROS开发自己的导航功能包应该如何做呢？

本系列文章会从源代码分析的角度深入剖析ROS Navigation功能包，来加深对ROS 导航的理解和认识。基于的ROS版本是 ROS kinetic，Global planner使用 global_planner/GlobalPlanner， Local planner使用 teb_local_planner/TebLocalPlannerROS

1 Github url

Ros 导航部分的代码可以从如下github下载

Github: https://github.com/ros-planning/navigation

2 目录结构

整个代码的目录结构如下图所示：

 

3 Move_base的线程stack

ROS Navigation框架的核心是move base进程，像全局与局部的路径规划、代价地图、异常行为恢复等功能都是通过move base进程在工作。所以我们的源代码剖析就从move base进程开始。

Move base进程启动之后，使用gdb可以查看整个进程的stack信息。通过stack信息可以便于我们从更高的层次理解它的软件架构。

3.1 线程stack信息

如下所示，move base启动之后，会有11个线程。红色部分标识出了每个线程的历程函数。通过这个历程函数就可以方便的找到对应的源代码。

Thread 11 (Thread 0x7f714fda8700 (LWP 4171)):
#0  0x00007f717d79830d in nanosleep () at ../sysdeps/unix/syscall-template.S:84
#1 0x00007f717e24896b in ros::ros_wallsleep(unsigned int, unsigned int) () from /opt/ros/kinetic/lib/librostime.so
#2 0x00007f717e24942e in ros::Duration::sleep() const () from /opt/ros/kinetic/lib/librostime.so
#3  0x00007f717e2483c0 in ros::Rate::sleep() () from /opt/ros/kinetic/lib/librostime.so
#4  0x00007f717d47412f in costmap_2d::Costmap2DROS::mapUpdateLoop(double) () from /opt/ros/kinetic/lib/libcostmap_2d.so
#5  0x00007f717bf375d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#6  0x00007f717bd106ba in start_thread (arg=0x7f714fda8700) at pthread_create.c:333
#7  0x00007f717d7d341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Thread 10 (Thread 0x7f716c898700 (LWP 3908)):
#0  0x00007f717d79830d in nanosleep () at ../sysdeps/unix/syscall-template.S:84
#1  0x00007f717e24896b in ros::ros_wallsleep(unsigned int, unsigned int) () from /opt/ros/kinetic/lib/librostime.so
#2 0x00007f717e24942e in ros::Duration::sleep() const () from /opt/ros/kinetic/lib/librostime.so
#3  0x00007f717e2483c0 in ros::Rate::sleep() () from /opt/ros/kinetic/lib/librostime.so
#4  0x00007f717d47412f in costmap_2d::Costmap2DROS::mapUpdateLoop(double) () from /opt/ros/kinetic/lib/libcostmap_2d.so
#5  0x00007f717bf375d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#6  0x00007f717bd106ba in start_thread (arg=0x7f716c898700) at pthread_create.c:333
#7  0x00007f717d7d341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Thread 9 (Thread 0x7f716d1dd700 (LWP 3762)):
#0pthread_cond_timedwait@@GLIBC_2.3.2 () at ../sysdeps/unix/sysv/linux/x86_64/pthread_cond_timedwait.S:225
#10x00007f717e5237ef in bool boost::condition_variable::timed_wait<boost::date_time::subsecond_duration<boost::posix_time::time_duration,1000l>>(boost::unique_lock<boost::mutex>&, boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000l> const&) ()
   from /opt/ros/kinetic/lib/libroscpp.so
#20x00007f717e52438e in ros::TimerManager<ros::Time,ros::Duration, ros::TimerEvent>::threadFunc() ()
   from /opt/ros/kinetic/lib/libroscpp.so
#3  0x00007f717bf375d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#4  0x00007f717bd106ba in start_thread (arg=0x7f716d1dd700) at pthread_create.c:333
#5  0x00007f717d7d341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Thread 8 (Thread 0x7f716dffb700 (LWP 3015)):
#0pthread_cond_wait@@GLIBC_2.3.2 () at ../sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:185
#10x00007f717ea89895 in move_base::MoveBase::planThread() () from /opt/ros/kinetic/lib/libmove_base.so
#2  0x00007f717bf375d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#3  0x00007f717bd106ba in start_thread (arg=0x7f716dffb700) at pthread_create.c:333
#4  0x00007f717d7d341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Thread 7 (Thread 0x7f716e7fc700 (LWP 2907)):
#0pthread_cond_timedwait@@GLIBC_2.3.2 () at ../sysdeps/unix/sysv/linux/x86_64/pthread_cond_timedwait.S:225
#10x00007f717eae3df9in actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction_<std::allocator<void> > >::executeLoop() ()from /opt/ros/kinetic/lib/libmove_base.so
#2  0x00007f717bf375d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#3  0x00007f717bd106ba in start_thread (arg=0x7f716e7fc700) at pthread_create.c:333
#4  0x00007f717d7d341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Thread 6 (Thread 0x7f716effd700 (LWP 2861)):
#0pthread_cond_timedwait@@GLIBC_2.3.2 () at ../sysdeps/unix/sysv/linux/x86_64/pthread_cond_timedwait.S:225
#1  0x00007f717e54b4f7 in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/kinetic/lib/libroscpp.so
#2  0x00007f717b59bc42 in tf2_ros::TransformListener::dedicatedListenerThread() () from /opt/ros/kinetic/lib/libtf2_ros.so
#3  0x00007f717bf375d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#4  0x00007f717bd106ba in start_thread (arg=0x7f716effd700) at pthread_create.c:333
#5  0x00007f717d7d341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Thread 5 (Thread 0x7f716f7fe700 (LWP 2758)):
#0pthread_cond_timedwait@@GLIBC_2.3.2 () at ../sysdeps/unix/sysv/linux/x86_64/pthread_cond_timedwait.S:225
#10x00007f717e54b4f7 in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/kinetic/lib/libroscpp.so
#20x00007f717e58c9a4 in ros::internalCallbackQueueThreadFunc() () from /opt/ros/kinetic/lib/libroscpp.so
#3  0x00007f717bf375d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#4  0x00007f717bd106ba in start_thread (arg=0x7f716f7fe700) at pthread_create.c:333
#5  0x00007f717d7d341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Thread 4 (Thread 0x7f716ffff700 (LWP 2682)):
#0pthread_cond_wait@@GLIBC_2.3.2 () at ../sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:185
#10x00007f717e588db3 in ros::ROSOutAppender::logThread() () from /opt/ros/kinetic/lib/libroscpp.so
#2  0x00007f717bf375d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#3  0x00007f717bd106ba in start_thread (arg=0x7f716ffff700) at pthread_create.c:333
#4  0x00007f717d7d341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Thread 3 (Thread 0x7f7174d45700 (LWP 2681)):
#0  0x00007f717d7c774d in poll () at ../sysdeps/unix/syscall-template.S:84
#10x00007f717c58c8bc in XmlRpc::XmlRpcDispatch::work(double)() from /opt/ros/kinetic/lib/libxmlrpcpp.so
#20x00007f717e518fae in ros::XMLRPCManager::serverThreadFunc()() from /opt/ros/kinetic/lib/libroscpp.so
#3  0x00007f717bf375d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#4  0x00007f717bd106ba in start_thread (arg=0x7f7174d45700) at pthread_create.c:333
#5  0x00007f717d7d341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Thread 2 (Thread 0x7f7175546700 (LWP 2680)):
#0  0x00007f717d7d3a13 in epoll_wait () at ../sysdeps/unix/syscall-template.S:84
#1  0x00007f717e525a87 in ros::poll_sockets(int, pollfd*, unsigned long, int) () from /opt/ros/kinetic/lib/libroscpp.so
#2  0x00007f717e5af236 in ros::PollSet::update(int) () from /opt/ros/kinetic/lib/libroscpp.so
#30x00007f717e534ac5 ros::PollManager::threadFunc() () from /opt/ros/kinetic/lib/libroscpp.so
#4  0x00007f717bf375d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#5  0x00007f717bd106ba in start_thread (arg=0x7f7175546700) at pthread_create.c:333
#6  0x00007f717d7d341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Thread 1 (Thread 0x7f717eee9800 (LWP 2624)):
#0pthread_cond_timedwait@@GLIBC_2.3.2 () at ../sysdeps/unix/sysv/linux/x86_64/pthread_cond_timedwait.S:225
#1  0x00007f717e54b4f7 in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/kinetic/lib/libroscpp.so
#2  0x00007f717e5a7e39 in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*) () from /opt/ros/kinetic/lib/libroscpp.so
#3  0x00007f717e58ce9b in ros::spin() () from /opt/ros/kinetic/lib/libroscpp.so
#4  0x00000000004052b2 in main ()

3.2 每个线程的职责分析

每个线程的职责如下:

线程名	所在的动态库	功能描述
ros::SingleThreadedSpinner ::spin(ros::CallbackQueue*)	/opt/ros/kinetic/lib/move_base/move_base /opt/ros/kinetic/lib/libroscpp.so	主线程。启动进程，并进入循环状态
ros::PollManager::threadFunc()	/opt/ros/kinetic/lib/libroscpp.so	Poll Manager线程
ros::XMLRPCManager:: serverThreadFunc()	/opt/ros/kinetic/lib/libroscpp.so	XMLRPC线程
ros::ROSOutAppender:: logThread()	/opt/ros/kinetic/lib/libroscpp.so	log线程
ros:: internalCallbackQueueThreadFunc()	/opt/ros/kinetic/lib/libroscpp.so	回调队列线程
tf2_ros::TransformListener:: dedicatedListenerThread()	/opt/ros/kinetic/lib/libtf2_ros.so	TF线程
ros::TimerManager<ros::Time,ros::Duration, ros::TimerEvent>::threadFunc() ()	/opt/ros/kinetic/lib/libroscpp.so	TimerManager线程
以上7个线程是ROS框架使用的线程，对所有的ROS node都一样。 下面开始就是move base自己的线程了。
actionlib::SimpleActionServer <move_base_msgs::MoveBaseAction_ <std::allocator<void> > >::executeLoop()	/opt/ros/kinetic/lib/libmove_base.so	action server线程。 是整个导航调度的控制线程。接收外部的导航目标指令，获取全局规划路径，调用局部路径规划，计算发布运动指令。直至整个导航成功或者失败。
move_base::MoveBase::planThread()	/opt/ros/kinetic/lib/libmove_base.so	global planner线程，负责全局的路径规划。生成的全局路径给action server线程使用
costmap_2d::Costmap2DROS::mapUpdateLoop(double)	/opt/ros/kinetic/lib/libcostmap_2d.so	global cost map更新线程
costmap_2d::Costmap2DROS::mapUpdateLoop(double)	/opt/ros/kinetic/lib/libcostmap_2d.so	local cost map更新线程