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;
}

功能函数： 两点间距计算

inline int positive_modulo(int i,int n){
    return ( i%n + n)% n;
}

功能函数：求模计算

inline int min_distance_index(const geometry_msgs::PoseStamped &from, nav_msgs::Path &path) {
    int index = -1;
    double min_value = std::numeric_limits<double>::max();
    double dis;
    for (size_t i = 0; i < path.poses.size(); i++) {
        dis = distance(from, path.poses.at(i));
        if (dis < min_value) {
            min_value = dis;
            index = static_cast<int>(i);
        }
    }
    return index;
}

起始时，机器人位置和路径上最近距离点的索引获取

inline int min_distance_index_inner(const geometry_msgs::PoseStamped &from,nav_msgs::Path path,size_t from_index,size_t to_index)
{
    int index = static_cast<int>(from_index);
    double min_value = std::numeric_limits<double>::max();
    double dis;
    for(int i = static_cast<int>(from_index);positive_modulo(i,static_cast<int>(path.poses.size()))!=to_index;i = i+direction_)
    {
        auto tmp_index = static_cast<size_t>(positive_modulo(i,static_cast<int>(path.poses.size())));
        dis = distance(from,path.poses.at(tmp_index));
        if(dis < min_value)
        {
            min_value = dis;
            index = static_cast<int>(tmp_index);
        }
    }
    return index;
}

机器人出发后，机器人当前位置和路径点最近距离索引

2.global_path_planner.cpp

    GlobalPathPlanner::GlobalPathPlanner():initialized_(false),start_index_(-1),goal_index_(-1) {
    }

构造函数，初始化列表对变量初始化。

void GlobalPathPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
{
    if(!initialized_)
    {
    	//参数
        ros::NodeHandle private_nh("~/" +name);
        private_nh.param<std::string>("map_frame",map_frame_,std::string("map"));
        private_nh.param<double>("path_length",path_length_,0.5);
        private_nh.param<int>("direction",direction_,1);
        
        //函数定义
        sub_path_pub_ = private_nh.advertise<nav_msgs::Path>("sub_path",1,true);
        res_sub_ = nh_.subscribe<move_base_msgs::MoveBaseActionResult>("/move_base/result",1,&GlobalPathPlanner::reach_goal,this);
        path_sub_ = nh_.subscribe<nav_msgs::Path>("exist_path",10,&GlobalPathPlanner::receive_path_callback,this);
        initialized_ = true;
    }
}

初始化函数实现：定义变量的初始值，以及发布和接收函数定义

void GlobalPathPlanner::reach_goal(const move_base_msgs::MoveBaseActionResult::ConstPtr &msg)
{
    if(msg->status.status == 3)
    {
        ROS_INFO("goal reach !");
    }
}

达到终点函数接收

void GlobalPathPlanner::receive_path_callback(const nav_msgs::Path::ConstPtr &msg)
{
    ROS_INFO("Recv Path ~");
    record_path_ = *msg;
}

接收固定路径信息，保存在变量record_path_中，用于后期发布。

void GlobalPathPlanner::pub_path(std::vector<geometry_msgs::PoseStamped> &plan)
{
    nav_msgs::Path sub_path;
    sub_path.header.frame_id= map_frame_;
    sub_path.header.stamp = ros::Time::now();
    sub_path.poses = plan;
    sub_path_pub_.publish(sub_path);
}

发布路径函数实现

    bool GlobalPathPlanner::makePlan(const geometry_msgs::PoseStamped &start,
                      const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan)
    {
    	//检查路径和坐标系是否合法
        if(record_path_.poses.size() <=1)
        {
            ROS_INFO_STREAM("PATH TOO SHORT ~");
            return false;
        }
        else if(goal.header.frame_id != map_frame_)
        {
            ROS_ERROR("The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", map_frame_.c_str(), goal.header.frame_id.c_str());
            return false;
        }
        else if(start.header.frame_id != map_frame_)
        {
            ROS_ERROR("The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", map_frame_.c_str(), start.header.frame_id.c_str());
            return false;
        }
		//初始化路径
        plan.clear();
        //初始 索引获取
        if(start_index_ == -1 || distance(goal,goal_cache_)>= 0.01)
        {
            start_index_ = min_distance_index(start,record_path_);
            goal_cache_ = goal;
        }
        //移动过程 索引获取
        else
        {
            start_index_ = min_distance_index_inner(start,record_path_, static_cast<size_t>(start_index_), static_cast<size_t>(goal_index_));

        }
        //更新索引
        int next_index = start_index_;
        double sum = 0;
        int from_index, to_index;
        for(;;)
        {
            from_index = next_index;
            to_index = next_index + 1;
            if( to_index >= record_path_.poses.size())
            {
                break;
            }
            sum += std::sqrt(distance(record_path_.poses.at(static_cast<unsigned long>(from_index)),record_path_.poses.at(static_cast<unsigned long>(to_index))));
            geometry_msgs::PoseStamped org = record_path_.poses.at(static_cast<unsigned long>(to_index));
            org.pose.orientation = tf::createQuaternionMsgFromYaw(tf::getYaw(org.pose.orientation));
            plan.push_back(org);
            next_index = to_index;
            if(sum >= path_length_)
            {
                goal_index_ = to_index;
                break;
            }
        }
        //发布路径
        pub_path(plan);
        return true;
    }

路径规划make_plan()函数实现

三、结果

放张导航图吧~~~
注意：这里仅仅是全局路径插件的实现，并没有运动控制的实现。

完整代码：gitee链接

---

总结

本文实现了简单自定义固定路线的发布实现，用于机器人后期沿着固定线路进行运动。