美文网首页ROS自主导航机器人
48.在ROS中实现local planner(1)- 实现一个

48.在ROS中实现local planner(1)- 实现一个

作者: PIBOT导航机器人 | 来源:发表于2023-06-27 23:06 被阅读0次

    有了之前45.在ROS中实现global planner(1)- 实现一个可以用模板global planner的经验, 现在再去创建一个local planner的包就容易多了

    1. 创建包

    • 创建
    cd ~/pibot_ros/ros_ws/src  # 这里可以使用自己的ros workspace
    catkin_create_pkg sample_local_planner
    
    • 添加类
      我们需要实现一个从nav_core::BaseLocalPlanner继承的类, nav_core::BaseLocalPlanner接口类定义在这里base_local_planner.h#L50)可以看到

    • 修改编译
      修改CMakeLists.txt,添加相关编译参数和选项

    • 添加bgp_plugin.xml文件
      指定导出的类名称

    <library path="lib/libsample_local_planner">
      <class name="sample_local_planner/LocalPlanner" type="sample_local_planner::LocalPlanner" base_class_type="nav_core::BaseLocalPlanner">
        <description>
          A sample implementation of a grid local planner 
        </description>
      </class>
    </library>
    

    目录结构这样

    ❯ tree sample_local_planner
    sample_local_planner
    ├── bgp_plugin.xml
    ├── CMakeLists.txt
    ├── include
    │   └── sample_local_planner
    │       └── planner_node.h
    ├── package.xml
    └── src
        └── planner_node.cpp
    
    PLUGINLIB_EXPORT_CLASS(sample_local_planner::LocalPlanner, nav_core::BaseLocalPlanner)
    

    2. 接口实现

    2.1 接口

    base_local_planner.h#L50)可以看到接口类

    namespace nav_core {
      class BaseLocalPlanner{
        public:
          virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
          virtual bool isGoalReached() = 0;
          virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
          virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
      };
    };  // namespace nav_core
    
    

    通过命名大概就知道其定义,

    • initialize
      初始化接口,给我们传相关功能接口的,如tfcostmap
    • setPlan
      规划控制接口,给我们提供一个plan,这个应该是global planner的输出,通过move_base转了一手给到我们,后面可以看下move_base源码
    • computeVelocityCommands
      计算速度,传入的参数是一个引用,应该是输出函数,我们把计算好的速度填进去就可以
    • isGoalReached
      获取是否以及到达目标点

    2.2 不同ros版本接口差异

    BaseLocalPlannerros kinetic 中的initialize接口稍有差异 见base_local_planner.h#L78

    
    // kinetic
    virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
    
    // melodic&noetic
    virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
    

    后面我们以melodic&noetic实现

    2.3 实现

    主要代码如下,stopwatch_为计时器,我们在setPlan调用后,设置变量,computeVelocityCommands接口中设置固定的速度,在时间到达后,输出0,同时isGoalReached接口返回true

    void LocalPlanner::initialize(std::string name, tf::TransformListener *tf,
                                      costmap_2d::Costmap2DROS *costmap_ros)
        {
            ROS_INFO("LocalPlanner initialize");
        }
    
        bool LocalPlanner::computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
        {
            ROS_INFO("LocalPlanner computeVelocityCommands");
            if (start_flag_) {
                cmd_vel.linear.x = 0.2;
                cmd_vel.linear.y = 0;
                cmd_vel.angular.z = 0.8;
            } else {
                cmd_vel.linear.x = 0;
                cmd_vel.linear.y = 0;
                cmd_vel.angular.z = 0;
            }
            
            return true;
        }
    
        bool LocalPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan)
        {
            ROS_INFO("LocalPlanner setPlan");
    
            if (!start_flag_) {
                start_flag_ = true;
                stopwatch_.reset();
            }
    
            return true;
        }
    
        bool LocalPlanner::isGoalReached()
        {
            if (stopwatch_.elapsed(std::chrono::seconds(2)))
            {
                ROS_INFO("LocalPlanner GoalReached");
                return true;
            }
    
            return false;
        }
    

    通过查看move_base源码,上面几个接口是在同一个线程被调用,所有后续不需要考虑资源竞争,即变量无需加锁

    3. 测试

    3.1 编译

    cd ~/pibot_ros/ros_ws
    catkin_make
    

    3.2 测试

    修改~/pibot_ros/src/pibot_simulator/move_base_params.yaml

    # base_local_planner: "dwa_local_planner/DWAPlannerROS"
    base_local_planner: sample_local_planner/LocalPlanner
    

    dwa_local_planner/DWAPlannerROS ----> sample_local_planner/LocalPlanner

    • 启动模拟器
    pibot_simulator
    
    • 查看当前的local_planner
    ❯ rosparam get /move_base/base_local_planner
    sample_local_planner/LocalPlanner  # 输出sample_local_planner/LocalPlanner表示插件已经被正确加载
    
    • 启动rviz发送点位,选点导航测试
    pibot_view
    

    3.3 测试结果

    [ INFO] [1676647988.863610652]: make plan start:[0.000000 0.000000], goal:[-2.986773 4.282055]
    [ INFO] [1676647989.063781836]: LocalPlanner setPlan
    [ INFO] [1676647989.064015702]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647989.263707871]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647989.463771479]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647989.663754028]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647989.863583610]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647989.864067517]: make plan start:[0.000000 0.000000], goal:[-2.986773 4.282055]
    [ INFO] [1676647990.063701815]: LocalPlanner setPlan
    [ INFO] [1676647990.063874092]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647990.263710418]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647990.463773749]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647990.663630163]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647990.863635728]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647990.864087581]: make plan start:[0.000000 0.000000], goal:[-2.986773 4.282055]
    [ INFO] [1676647991.063713670]: LocalPlanner setPlan
    [ INFO] [1676647991.063894899]: LocalPlanner computeVelocityCommands
    [ INFO] [1676647991.263639509]: LocalPlanner GoalReached
    
    

    通过日志可以看出

    • 在全局规划(make plan start是我们前面文章新增的astar planner输出)后LocalPlanner的接口setPlan被调用
    • computeVelocityCommands函数没0.2s被调用一次, 期间机器人也在做圆周运动
    • 全局规划再次被调用(move_bsae里配置了规划频率1hz,这里可以看到间隔1s全局规划一次),重复前面的
    • 直到超时GoalReached返回true完成

    4. 总结

    本文简单实现了一个local planner的插件,显然实际没啥用,不过可以作为一个模板,基于该模板实现自己的算法。后面我们将基于该模板实现可用的局部规划控制。

    本文代码见sample_local_planner

    相关文章

      网友评论

        本文标题:48.在ROS中实现local planner(1)- 实现一个

        本文链接:https://www.haomeiwen.com/subject/wuznydtx.html