move_base使用自己编写的全局规划器

其他 2021-03-24 04:08:44 阅读次数: 0

编写全局规划器

首先在工作目录创建功能包

catkin_create_pkg relaxed_astar nav_core roscpp rospy std_msgs

在功能包下编写对应的头文件和源文件
具体的规划器代码根据自己的实际需求进行编写，本文为简化版，结尾处附有github 源代码链接。
RAstar_ros.h

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h> 
#include <string>

/** include ros libraries**********************/
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <move_base_msgs/MoveBaseActionGoal.h>
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/GetPlan.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
/** ********************************************/ 

#include <boost/foreach.hpp>
//#define forEach BOOST_FOREACH
/** for global path planner interface */
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <angles/angles.h>
//#include <pcl_conversions/pcl_conversions.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
#include <set>
using namespace std;
using std::string;

#ifndef RASTAR_ROS_CPP
#define RASTAR_ROS_CPP

namespace RAstar_planner {
    
    
  
class RAstarPlannerROS : public nav_core::BaseGlobalPlanner {
    
    
public:
  
  RAstarPlannerROS (ros::NodeHandle &); //this constructor is may be not needed
  RAstarPlannerROS ();
  RAstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

  /** overriden classes from interface nav_core::BaseGlobalPlanner **/
  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
  bool makePlan(const geometry_msgs::PoseStamped& start, 
		const geometry_msgs::PoseStamped& goal, 
		std::vector<geometry_msgs::PoseStamped>& plan
	       );
};
};
  • 需要包含路径规划器所需的核心ROS库。路径规划器需要使用<costmap_2d/costmap_2d_ros.h>和<costmap_2d/costmap_2d.h>, 而costmap_2d::Costmap2D类作为输入的地图类。当定义为插件时，路径规划器类将自动访问此地图。没有必要订阅costmap2d来获取ROS的代价地图。
  • <nav_core/base_global_planner.h> 用于导入接口nav_core::BaseGlobalPlanner，这是插件必须要继承的父类。

然后在cpp文件中完善initialize 和 makePlan函数。
RAstar_ros.cpp

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h> 
#include <fstream>
#include <iostream>
#include <iomanip>
#include <string>

#include "RAstar_ros.h"

#include <pluginlib/class_list_macros.h>
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(RAstar_planner::RAstarPlannerROS, nav_core::BaseGlobalPlanner)


int value;
int mapSize;
bool* OGM;
static const float INFINIT_COST = INT_MAX; //!< cost of non connected nodes
float infinity = std::numeric_limits< float >::infinity();
float tBreak;  // coefficient for breaking ties
ofstream MyExcelFile ("RA_result.xlsx", ios::trunc);


namespace RAstar_planner
{
    
    

//Default Constructor
RAstarPlannerROS::RAstarPlannerROS()
{
    
    

}
RAstarPlannerROS::RAstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{
    
    
  initialize(name, costmap_ros);
}

void RAstarPlannerROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
{
    
    

  if (!initialized_)
  {
    
    
    costmap_ros_ = costmap_ros;
    costmap_ = costmap_ros_->getCostmap();

    ros::NodeHandle private_nh("~/" + name);

    originX = costmap_->getOriginX();
    originY = costmap_->getOriginY();



	width = costmap_->getSizeInCellsX();
	height = costmap_->getSizeInCellsY();
	resolution = costmap_->getResolution();
	mapSize = width*height;
	tBreak = 1+1/(mapSize); 
	value =0;


	OGM = new bool [mapSize]; 
    for (unsigned int iy = 0; iy < costmap_->getSizeInCellsY(); iy++)
    {
    
    
      for (unsigned int ix = 0; ix < costmap_->getSizeInCellsX(); ix++)
      {
    
    
        unsigned int cost = static_cast<int>(costmap_->getCost(ix, iy));
        //cout<<cost;
        if (cost == 0)
          OGM[iy*width+ix]=true;
        else
          OGM[iy*width+ix]=false;
      }
    }


	MyExcelFile << "StartID\tStartX\tStartY\tGoalID\tGoalX\tGoalY\tPlannertime(ms)\tpathLength\tnumberOfCells\t" << endl;

    ROS_INFO("RAstar planner initialized successfully");
    initialized_ = true;
  }
  else
    ROS_WARN("This planner has already been initialized... doing nothing");
}

bool RAstarPlannerROS::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                             std::vector<geometry_msgs::PoseStamped>& plan)
{
    
    

  if (!initialized_)
  {
    
    
    ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
    return false;
  }

  ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y,
            goal.pose.position.x, goal.pose.position.y);

  plan.clear();

  if (goal.header.frame_id != costmap_ros_->getGlobalFrameID())
  {
    
    
    ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",
              costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
    return false;
  }

  tf::Stamped < tf::Pose > goal_tf;
  tf::Stamped < tf::Pose > start_tf;

  poseStampedMsgToTF(goal, goal_tf);
  poseStampedMsgToTF(start, start_tf);

  // convert the start and goal positions

  float startX = start.pose.position.x;
  float startY = start.pose.position.y;

  float goalX = goal.pose.position.x;
  float goalY = goal.pose.position.y;

  getCorrdinate(startX, startY);
  getCorrdinate(goalX, goalY);

  int startCell;
  int goalCell;

  if (isCellInsideMap(startX, startY) && isCellInsideMap(goalX, goalY))
  {
    
    
    startCell = convertToCellIndex(startX, startY);

    goalCell = convertToCellIndex(goalX, goalY);

MyExcelFile << startCell <<"\t"<< start.pose.position.x <<"\t"<< start.pose.position.y <<"\t"<< goalCell <<"\t"<< goal.pose.position.x <<"\t"<< goal.pose.position.y;

  }
  else
  {
    
    
    ROS_WARN("the start or goal is out of the map");
    return false;
  }

  /

  // call global planner

  if (isStartAndGoalCellsValid(startCell, goalCell)){
    
    

        vector<int> bestPath;
	bestPath.clear();

    bestPath = RAstarPlanner(startCell, goalCell);

//if the global planner find a path
    if ( bestPath.size()>0)
    {
    
    

// convert the path

      for (int i = 0; i < bestPath.size(); i++)
      {
    
    

        float x = 0.0;
        float y = 0.0;

        int index = bestPath[i];

        convertToCoordinate(index, x, y);

        geometry_msgs::PoseStamped pose = goal;

        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.position.z = 0.0;

        pose.pose.orientation.x = 0.0;
        pose.pose.orientation.y = 0.0;
        pose.pose.orientation.z = 0.0;
        pose.pose.orientation.w = 1.0;

        plan.push_back(pose);
      }


	float path_length = 0.0;
	
	std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
	
	geometry_msgs::PoseStamped last_pose;
	last_pose = *it;
	it++;
	for (; it!=plan.end(); ++it) {
    
    
	   path_length += hypot(  (*it).pose.position.x - last_pose.pose.position.x, 
		                 (*it).pose.position.y - last_pose.pose.position.y );
	   last_pose = *it;
	}
	cout <<"The global path length: "<< path_length<< " meters"<<endl;
	MyExcelFile << "\t" <<path_length <<"\t"<< plan.size() <<endl;
      //publish the plan

      return true;

    }

    else
    {
    
    
      ROS_WARN("The planner failed to find a path, choose other goal position");
      return false;
    }

  }

  else
  {
    
    
    ROS_WARN("Not valid start or goal");
    return false;
  }

}
}

  • 构造函数GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)用于初始化代价地图，即将用于规划的代价地图（costmap_ros）以及规划器的名称（name）。

  • 默认构造函数GlobalPlanner()即使用默认值作为初始化。方法initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)是BaseGlobalPlanner的初始化函数，用于初始化costmap，参数为用于规划的代价地图（costmap_ros）和规划器的名称(name)。

  • 接着，bool makePlan（start，goal，plan）的方法必须要重写。最终规划将存储在方法的参数std::vector<geometry_msgs::PoseStamped>& plan 中。

  • .注册规划器作为BaseGlobalPlanner插件：这是通过指令PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner,
    nav_core::BaseGlobalPlanner)完成的。为此，必须包含库 #include
    <pluginlib/class_list_macros.h>

  • makePlan()方法实现：在开始和目标参数用来获取初始位置和目标位置。在该说明性实现中，以起始位置（plan.push_back(start))）作为开始的规划变量。然后，在for循环中，将在规划中静态插入20个新的虚拟位置，然后将目标位置作为最后一个位置插入规划。然后，此规划路径将发送到move_base全局规划模块，该模块将通过ROS话题nav_msgs/Path进行发布，然后将由本地规划模块接收。

现在，全局规划类已经完成，进行第二步，即为全局规划类创建插件，将其集成到move_base包的全局规划模块nav_core::BaseGlobalPlanner中。

要编译上面创建的全局规划库，必须将其添加到CMakeLists.txt中。添加代码：

add_library(global_planner_lib src/path_planner/global_planner/global_planner.cpp)

回到工作目录catkin_make进行编译，完成后可以在devel/lib下有一个librelaxed_astar_lib。
继续编写插件描述文件

插件描述文件

relaxed_astar_planner_plugin.xml

<library path="lib/librelaxed_astar_lib">
  <class name="RAstar_planner/RAstarPlannerROS" type="RAstar_planner::RAstarPlannerROS" base_class_type="nav_core::BaseGlobalPlanner">
    <description>This is Relaxed Astar global planner plugin by iroboapp project.</description>
  </class>
</library>

注册插件到ROS包系统

为了让pluginlib在所有ROS程序包上查询系统上的所有可用插件，每个程序包都必须明确指定其导出的插件，哪些程序包库包含这些插件。插件提供者必须在其导出标记块中的package.xml中指向其插件描述文件。
在package.xml的文件中的 export 标签里加入下面这句

	<nav_core plugin="${prefix}/relaxed_astar_planner_plugin.xml"/>

注意：为了使上述导出命令正常工作，提供程序包必须直接依赖于包含插件接口的程序包，这在全局规划程序的情况下是nav_core。因此，其package.xml中必须包含以下内容：

<build_depend>nav_core</build_depend>
<run_depend>nav_core</run_depend>

这将告诉编译器关于nav_core包的依赖性
回到工作目录在进行编译一次catkin_make，然后查询插件：

rospack plugins --attrib=plugin nav_core

可以看到 relaxed_astar的插件和其位置。在这里插入图片描述

到此，插件编写完成，下面就是运用自己得全局规划器在move_base中。

打开自己的move_base.launch配置文件，在move_base启动节点后添加

 <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
 <param name="base_global_planner" value="RAstar_planner/RAstarPlannerROS"/>

保存并关闭move_base.launch.xml。

  • 需要注意的是，规划器的名称是RAstar_planner/RAstarPlannerROS，跟指定在relaxed_astar_planner_plugin.xml文件名称一致。

至此，完成配置。就可以运行了。

参考：
http://wiki.ros.org/navigation/Tutorials/Writing%20A%20Global%20Path%20Planner%20As%20Plugin%20in%20ROS
https://www.youtube.com/watch?v=We1gGDXAO_o
https://www.youtube.com/watch?v=t4A_niNlDdg
https://github.com/coins-lab/relaxed_astar

猜你喜欢

转载自blog.csdn.net/qq_34935373/article/details/109487241
今日推荐
周排行
win7之session理解
如何快速上手 AB Testing ？阿里技术专家秘方公开
数据库自助查询平台
【CCF 2017-12-2】游戏问题
Fire!(BFS)
修复GRUB引导故障
老倪祖膏药分销系统
MySQL慢查询日志-捕获慢SQL
批归一化(Batch Normalization)
ssm整合问题-------一
每日归档
更多
2025-03-02(0)
2025-03-01(0)
2025-02-28(0)
2025-02-27(0)
2025-02-26(0)
2025-02-25(0)
2025-02-24(0)
2025-02-23(0)
2025-02-22(0)
2025-02-21(0)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022