ros知识点总结--头文件

自定义头文件步骤： 下面的.h 和.cpp文件没必要细看，就是c++中写头文件的方法。
ROS中自己编写头文件，在其他cpp文件中调用，和一般的c++文件编写，基本差不多，但是也是需要配置CMakeLists.txt文件的几处地方，下面给出我原来写的一个简单A*算法的例子

atsar.h —一般创建在功能包下的include/功能包名/astar.h

//astar.h
#include "ros/ros.h"
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <stack>
#include "nav_msgs/OccupancyGrid.h"
    int toIndex(int x,int y,int width)
    {
        return width * y + x ;
    }
    struct Node{
        int x;
        int y;
        float cost;
        int G;//起点当当前点的代价
        int F;//终点到当前点的代价
        int pre_index;
        Node(int x,int y, int g,int f,int pre)
        {
            this->x = x;
            this->y = y;
            this->G = g;
            this->F = f;
            this->cost = g+f;
            this->pre_index = pre;
        }
    
    };
    class compare {
        public:
            bool operator()(const Node& a, const Node& b) const {
                return a.cost < b.cost;
        }
    };
    class astar{
        private:
            std::multiset<Node,compare> open_;//自动排序
            std::vector<int> close_;//大小等于地图数组大小,0---代表没有使用,i代表父节点,及open_出来的全标记为1;
            int width;
            int height;
            std::vector<int8_t> map_data;
            bool add(int x,int y,Node parent,int end_x,int end_y,int cost);
        public:
            astar(nav_msgs::OccupancyGrid data);
            nav_msgs::Path astar_path_planning(int begin_x,int begin_y,int end_x,int end_y,bool &symbol);//标志位为false,则规划不出一条可行路径.
    };

astar.cpp —创建在src/astar.cpp

#include "multi_robot/astar.h"
    astar::astar(nav_msgs::OccupancyGrid data)
    {
        map_data = data.data;
        width = data.info.width;
        height = data.info.height;
        close_ = std::vector<int>(data.data.size(),-1);//初始化为-1;
    }
    nav_msgs::Path astar::astar_path_planning(int begin_x,int begin_y,int end_x,int end_y,bool &symbol)
    {
        open_.insert(Node(begin_x,begin_y,0,0,0));
        while(open_.size()>0&&symbol == false)//
        {
            std::cout<<"main"<<open_.size()<<std::endl;
            Node top = *open_.begin();
            open_.erase(open_.begin());
            close_[toIndex(top.x,top.y,width)] = top.pre_index;
            symbol = add(top.x+1,top.y,top,end_x,end_y,10);
            if(symbol==true)
                break;
            symbol = add(top.x-1,top.y,top,end_x,end_y,10);
            if(symbol==true)
                break;
            symbol = add(top.x,top.y+1,top,end_x,end_y,10);
            if(symbol==true)
                break;
            symbol = add(top.x,top.y-1,top,end_x,end_y,10);
            if(symbol==true)
                break;
            symbol = add(top.x+1,top.y+1,top,end_x,end_y,14);
            if(symbol==true)
                break;
                symbol = add(top.x+1,top.y-1,top,end_x,end_y,14);
            if(symbol==true)
                break;
                symbol = add(top.x-1,top.y+1,top,end_x,end_y,14);
            if(symbol==true)
                break;
                symbol = add(top.x-1,top.y-1,top,end_x,end_y,14);
            if(symbol==true)
                break;
            
        }
        nav_msgs::Path path;
        if(symbol == false)
            return path;
        for(int i =0;i<=8;i++)
            std::cout<<close_[i]<<std::endl;
        std::stack<int> st;
        st.push(toIndex(end_x,end_y,width));
        int i = close_[toIndex(end_x,end_y,width)];

        while(i !=toIndex(begin_x,begin_y,width))
        {
            
            st.push(i);
            i = close_[i];
        }
        st.push(i);
        geometry_msgs::PoseStamped this_pose_stamped;
        while(st.size() != 0)
        {
            this_pose_stamped.pose.position.x = st.top()%width;
            this_pose_stamped.pose.position.y = st.top()/width;
            this_pose_stamped.pose.position.z = 0;
            this_pose_stamped.header.frame_id = "/map";
            this_pose_stamped.header.stamp = ros::Time::now();
            path.poses.push_back(this_pose_stamped);
            st.pop();
        }
        return path;
    }
    bool astar::add(int x,int y,Node parent,int end_x,int end_y,int cost)
    {
         std::cout<<"add"<<std::endl;
        if(x<0||x>=width||y<0||y>=height)
            return false;
        if(map_data[toIndex(x,y,width)] != 0)//表示占有
            return false;
        if(close_[toIndex(x,y,width)] !=-1)
        {
            return false;
            //open_.insert(Node(x,y, parent.G+10,10*(abs(end_x-x)+abs(end_y-y)) ,toIndex(parent.x,parent.y,width)));
        }
        bool sym=false;
        if(open_.size() != 0)
        {
            std::multiset<Node,compare>::iterator it = open_.begin();
        
            while(it != open_.end())
            {
                if((*it).x == x && (*it).y ==y)
                {
                    sym =true;
                    if((*it).F>parent.G+10)
                    {
                        open_.erase(it);
                        open_.insert(Node(x,y, parent.G+cost,10*(abs(end_x-x)+abs(end_y-y)) ,toIndex(parent.x,parent.y,width)));
                        break;
                    }
                
                }
                it++;
            }
        }
        if(sym==false)
            open_.insert(Node(x,y, parent.G+cost,10*(abs(end_x-x)+abs(end_y-y)) ,toIndex(parent.x,parent.y,width)));
        if(x==end_x&&y==end_y)
        {
            close_[toIndex(x,y,width)] = toIndex(parent.x,parent.y,width);
            return true;
        }    
        return false;
    }

CMakeLists.txt文件配置

include_directories(
 include    //------放开include
  ${catkin_INCLUDE_DIRS}
)
add_library(//每一个头文件配置一个add_library
  Astar    //类似可执行文件名，这里自己起，后面配置对应即可
  src/astar.cpp
  include/multi_robot/astar.h //其中multi_robot是功能包名，也可以使用${PROJECT_NAME}，
)
add_dependencies(Astar ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})//Astar就是前面的可执行文件名

target_link_libraries(Astar //Astar就是前面的可执行文件名
  ${catkin_LIBRARIES}
)

//在某个cpp文件中使用该头文件时 就和平时一样头文件包含即可

#include "multi_robot/astar.h"

在该文件配置时，和以往比，唯一区别：

target_link_libraries(sub_pose
  Astar   //---多一步添加头文件的可执行文件
  ${catkin_LIBRARIES}
)