当前位置： 首页 > news >正文

幸运28网站建设,手机网页视频如何下载,网页与网站的区别与联系是什么,做网站的优惠广告有了之前45.在ROS中实现global planner#xff08;1#xff09;- 实现一个可以用模板的global planner的经验, 现在再去创建一个local planner的包就容易多了

1. 创建包 创建 cd ~/pibot_ros/ros_ws/src # 这里可以使用自己的ros workspace catkin_create_pkg sample_loc…有了之前45.在ROS中实现global planner1- 实现一个可以用模板的global planner的经验, 现在再去创建一个local planner的包就容易多了

2. 创建包 创建 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 pathlib/libsample_local_plannerclass namesample_local_planner/LocalPlanner typesample_local_planner::LocalPlanner base_class_typenav_core::BaseLocalPlannerdescriptionA 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导出类 参考navigation里面, 添加宏导出该类 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::vectorgeometry_msgs::PoseStamped plan) 0;virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) 0;}; }; // namespace nav_core 通过命名大概就知道其定义 initialize 初始化接口给我们传相关功能接口的如tf costmapsetPlan 规划控制接口给我们提供一个plan这个应该是global planner的输出通过move_base转了一手给到我们后面可以看下move_base源码computeVelocityCommands 计算速度传入的参数是一个引用应该是输出函数我们把计算好的速度填进去就可以isGoalReached 获取是否以及到达目标点 2.2 不同ros版本接口差异 BaseLocalPlanner在ros kinetic 中的initialize接口稍有差异 见base_local_planner.h#L78 // kinetic virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) 0;// melodicnoetic virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) 0;后面我们以melodicnoetic实现 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 (startflag) {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::vectorgeometry_msgs::PoseStamped orig_global_plan){ROS_INFO(LocalPlanner setPlan);if (!startflag) {startflag 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_make3.2 测试 修改/pibot_ros/src/pibot_simulator/move_base_params.yaml

   base_local_planner: dwa_local_planner/DWAPlannerROS

   base_local_planner: sample_local_planner/LocalPlannerdwa_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_view3.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完成

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