本文共 8892 字，大约阅读时间需要 29 分钟。

         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:

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:109Thread 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:109Thread 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::unique_lock
     
      &, boost::date_time::subsecond_duration
      
        const&) ()   from /opt/ros/kinetic/lib/libroscpp.so#20x00007f717e52438e in ros::TimerManager
       
        ::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:109Thread 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:109Thread 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
        
         
          
            > >::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:109Thread 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:109Thread 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:109Thread 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:109Thread 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:109Thread 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:109Thread 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 每个线程的职责分析

每个线程的职责如下:

转载地址：https://blog.csdn.net/lclfans1983/article/details/106692440 如侵犯您的版权，请留言回复原文章的地址，我们会给您删除此文章，给您带来不便请您谅解！