ROS中编写自己全局路径规划插件实现固定路线规划（2）

---

前言

在ROS中编写自己全局路径规划插件实现固定路线规划（1）中实现了自定义全局路径插件的注册，本文进行具体的实现。

实现固定路线发布：
   1.确定固定路线
   2.固定路线发布
   3.全局路径插件接收路线并发布
其中，前两个步骤的实现可以控制机器人机器人按照我们期望的轨迹走一遍，然后将路径保存下来，之后再将路径发布，具体可参考读取机器人移动轨迹并在RVIZ界面中显示，这里主要介绍第三步的实现。

---

一、全局路径插件接收路线并发布

假定：固定路线发布的话题名为：/exist_path。
全局路径规划插件需要将路径点一个接一个的获取，并发布。

二、具体实现

1.global_path_planner.h

private:
   void receive_path_callback(const nav_msgs::Path::ConstPtr &msg);
   void reach_goal(const move_base_msgs::MoveBaseActionResult::ConstPtr &msg);
   void pub_path(std::vector<geometry_msgs::PoseStamped> &plan);

函数声明，receive_path_callback()路径接收函数；reach_goal()到达目标点；pub_path()路径发布

private:
        bool initialized_;  //初始化标志位
        std::string map_frame_;  //地图坐标系名
        double path_length_;   //路径长度
        int direction_;       //方向
        int start_index_;   //起始索引
        int goal_index_;   // 目标索引
        
        //ros
        ros::NodeHandle nh_;
        ros::Publisher sub_path_pub_;  
        ros::Subscriber path_sub_;
        ros::Subscriber res_sub_;
        nav_msgs::Path record_path_;   //存储路径
        geometry_msgs::PoseStamped goal_cache_;

成员变量，包含初始化标志、地图坐标系名称、路径长度、索引等。

inline double distance(const geometry_msgs::PoseStamped &from, geometry_msgs::PoseStamped &to) {
   
    double delta_x = from.pose.position.x - to.pose.position.x;
    double delta_y = from.pose.position.y - to.pose.position.y;
    return delta_x * delta_x + delta_y * delta_y;
}

功能函数： 两点间距计算