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 每个线程的职责分析

每个线程的职责如下:

以上7个线程是ROS框架使用的线程，对所有的ROS node都一样。

下面开始就是move base自己的线程了。