自定义插件未集成到ROS工具链

924453613

已创建插件
RAstar_ros.h

/* iPath: A C++ Library of Intelligent Global Path Planners for Mobile Robots
 * with ROS Integration. Website: http://www.iroboapp.org/index.php?title=IPath
 * Contact:
 *
 * Copyright (c) 2014
 * Owners: Al-Imam University/King AbdulAziz Center for Science and Technology
 * (KACST)/Prince Sultan University All rights reserved.
 *
 * License Type: GNU GPL
 *
 *   This program is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

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

#include <string>

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

#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"
/** ********************************************/

#include <boost/foreach.hpp>
//#define forEach BOOST_FOREACH

/** for global path planner interface */
#include <angles/angles.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_core/base_global_planner.h>

//#include <pcl_conversions/pcl_conversions.h>
#include <base_local_planner/costmap_model.h>
#include <base_local_planner/world_model.h>

#include <set>
using namespace std;
using std::string;

#ifndef RASTAR_ROS_CPP
#define RASTAR_ROS_CPP

/**
 * @struct cells
 * @brief A struct that represents a cell and its fCost.
 */
struct cells {
  int currentCell;
  float fCost;
};

// STEP1-2.your class should inherits from the interface
// "nav_core::BaseGlobalPlanner"
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);

  ros::NodeHandle ROSNodeHandle;

  /** 公有继承 nav_core::BaseGlobalPlanner  接口 **/
  /** 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);

  void getCorrdinate(float& x, float& y);
  int convertToCellIndex(float x, float y);
  void convertToCoordinate(int index, float& x, float& y);
  bool isCellInsideMap(float x, float y);
  void mapToWorld(double mx, double my, double& wx, double& wy);
  vector<int> RAstarPlanner(int startCell, int goalCell);
  vector<int> findPath(int startCell, int goalCell, float g_score[]);
  vector<int> constructPath(int startCell, int goalCell, float g_score[]);
  float calculateHCost(int cellID, int goalCell) {
    int x1 = getCellRowID(goalCell);
    int y1 = getCellColID(goalCell);
    int x2 = getCellRowID(cellID);
    int y2 = getCellColID(cellID);
    return abs(x1 - x2) + abs(y1 - y2);
    // return min(abs(x1-x2),abs(y1-y2))*sqrt(2) +
    // max(abs(x1-x2),abs(y1-y2))-min(abs(x1-x2),abs(y1-y2));
  }
  void addNeighborCellToOpenList(multiset<cells>& OPL, int neighborCell,
                                 int goalCell, float g_score[]);
  vector<int> findFreeNeighborCell(int CellID);
  bool isStartAndGoalCellsValid(int startCell, int goalCell);
  float getMoveCost(int CellID1, int CellID2);
  float getMoveCost(int i1, int j1, int i2, int j2);
  bool isFree(int CellID);  // returns true if the cell is Free
  bool isFree(int i, int j);

  int getCellIndex(int i,
                   int j)  // get the index of the cell to be used in Path
  {
    return (i * width) + j;
  }
  int getCellRowID(int index)  // get the row ID from cell index
  {
    return index / width;
  }
  int getCellColID(int index)  // get colunm ID from cell index
  {
    return index % width;
  }

  float originX;
  float originY;
  float resolution;
  costmap_2d::Costmap2DROS* costmap_ros_;
  double step_size_, min_dist_from_robot_;
  costmap_2d::Costmap2D* costmap_;
  // base_local_planner::WorldModel* world_model_;
  bool initialized_;
  int width;
  int height;
};

};  // namespace RAstar_planner
#endif

已创建基类并注册插件
RAstar_ros.cpp
d0b76a5d-ffbc-4426-b40b-fe45b1c98d5f-图片.png

/* iPath: A C++ Library of Intelligent Global Path Planners for Mobile Robots
 * with ROS Integration. Website: http://www.iroboapp.org/index.php?title=IPath
 * Contact:
 *
 * Copyright (c) 2014
 * Owners: Al-Imam University/King AbdulAziz Center for Science and Technology
 * (KACST)/Prince Sultan University All rights reserved.
 *
 * License Type: GNU GPL
 *
 *   This program is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

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

#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>

// STEP1-1.include core ROS libraries -> nav_core roscpp rospy std_msgs
#include "RAstar_ros.h"
// STEP1-4.include <pluginlib/class_list_macros.h>
#include <pluginlib/class_list_macros.h>
// register this planner as a BaseGlobalPlanner plugin
/*注册一个插件
  STEP1-5.register this planner as a BaseGlobalPlanner plugin by adding
  PLUGINLIB_EXPORT_CLASS(RAstar_planner::RAstarPlannerROS,nav_core::BaseGlobalPlanner)*/
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);

int clock_gettime(clockid_t clk_id, struct timespect* tp);

timespec diff(timespec start, timespec end) {
  timespec temp;
  if ((end.tv_nsec - start.tv_nsec) < 0) {
    temp.tv_sec = end.tv_sec - start.tv_sec - 1;
    temp.tv_nsec = 1000000000 + end.tv_nsec - start.tv_nsec;
  } else {
    temp.tv_sec = end.tv_sec - start.tv_sec;
    temp.tv_nsec = end.tv_nsec - start.tv_nsec;
  }
  return temp;
}

inline vector<int> findFreeNeighborCell(int CellID);

namespace RAstar_planner {

// Default Constructor
RAstarPlannerROS::RAstarPlannerROS() {}
RAstarPlannerROS::RAstarPlannerROS(ros::NodeHandle& nh) { ROSNodeHandle = nh; }
// STEP1-3.overriden the methods "initialize" and "makePlan"
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");
}
// STEP1-3.1Here we override the "makePlan" method,in which you should write or
// call your path planning algorithm
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 relaxed Astar 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;

        // your path should be stored in the "plan" vector
        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;
  }
}
void RAstarPlannerROS::getCorrdinate(float& x, float& y) {
  x = x - originX;
  y = y - originY;
}

int RAstarPlannerROS::convertToCellIndex(float x, float y) {
  int cellIndex;

  float newX = x / resolution;
  float newY = y / resolution;

  cellIndex = getCellIndex(newY, newX);

  return cellIndex;
}

void RAstarPlannerROS::convertToCoordinate(int index, float& x, float& y) {
  x = getCellColID(index) * resolution;

  y = getCellRowID(index) * resolution;

  x = x + originX;
  y = y + originY;
}

bool RAstarPlannerROS::isCellInsideMap(float x, float y) {
  bool valid = true;

  if (x > (width * resolution) || y > (height * resolution)) valid = false;

  return valid;
}

void RAstarPlannerROS::mapToWorld(double mx, double my, double& wx,
                                  double& wy) {
  costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
  wx = costmap->getOriginX() + mx * resolution;
  wy = costmap->getOriginY() + my * resolution;
}

vector<int> RAstarPlannerROS::RAstarPlanner(int startCell, int goalCell) {
  vector<int> bestPath;

  // float g_score [mapSize][2];
  float g_score[mapSize];

  for (uint i = 0; i < mapSize; i++) g_score[i] = infinity;

  timespec time1, time2;
  /* take current time here */
  clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);

  bestPath = findPath(startCell, goalCell, g_score);

  clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);

  cout << "time to generate best global path by Relaxed A* = "
       << (diff(time1, time2).tv_sec) * 1e3 +
              (diff(time1, time2).tv_nsec) * 1e-6
       << " microseconds" << endl;

  MyExcelFile << "\t"
              << (diff(time1, time2).tv_sec) * 1e3 +
                     (diff(time1, time2).tv_nsec) * 1e-6;

  return bestPath;
}

/*******************************************************************************/
// Function Name: findPath
// Inputs: the map layout, the start and the goal Cells and a boolean to
// indicate if we will use break ties or not Output: the best path Description:
// it is used to generate the robot free path
/*********************************************************************************/
vector<int> RAstarPlannerROS::findPath(int startCell, int goalCell,
                                       float g_score[]) {
  value++;
  vector<int> bestPath;
  vector<int> emptyPath;
  cells CP;

  multiset<cells> OPL;
  int currentCell;

  // calculate g_score and f_score of the start position
  g_score[startCell] = 0;
  CP.currentCell = startCell;
  CP.fCost = g_score[startCell] + calculateHCost(startCell, goalCell);

  // add the start cell to the open list
  OPL.insert(CP);
  currentCell = startCell;

  // while the open list is not empty continuie the search or g_score(goalCell)
  // is equal to infinity
  while (!OPL.empty() && g_score[goalCell] == infinity) {
    // choose the cell that has the lowest cost fCost in the open set which is
    // the begin of the multiset
    currentCell = OPL.begin()->currentCell;
    // remove the currentCell from the openList
    OPL.erase(OPL.begin());
    // search the neighbors of the current Cell
    vector<int> neighborCells;
    neighborCells = findFreeNeighborCell(currentCell);
    for (uint i = 0; i < neighborCells.size();
         i++)  // for each neighbor v of current cell
    {
      // if the g_score of the neighbor is equal to INF: unvisited cell
      if (g_score[neighborCells[i]] == infinity) {
        g_score[neighborCells[i]] =
            g_score[currentCell] + getMoveCost(currentCell, neighborCells[i]);
        addNeighborCellToOpenList(OPL, neighborCells[i], goalCell, g_score);
      }  // end if
    }    // end for
  }      // end while

  if (g_score[goalCell] !=
      infinity)  // if g_score(goalcell)==INF : construct path
  {
    bestPath = constructPath(startCell, goalCell, g_score);
    return bestPath;
  } else {
    cout << "Failure to find a path !" << endl;
    return emptyPath;
  }
}

/*******************************************************************************/
// Function Name: constructPath
// Inputs: the start and the goal Cells
// Output: the best path
// Description: it is used to construct the robot path
/*********************************************************************************/
vector<int> RAstarPlannerROS::constructPath(int startCell, int goalCell,
                                            float g_score[]) {
  vector<int> bestPath;
  vector<int> path;

  path.insert(path.begin() + bestPath.size(), goalCell);
  int currentCell = goalCell;

  while (currentCell != startCell) {
    vector<int> neighborCells;
    neighborCells = findFreeNeighborCell(currentCell);

    vector<float> gScoresNeighbors;
    for (uint i = 0; i < neighborCells.size(); i++)
      gScoresNeighbors.push_back(g_score[neighborCells[i]]);

    int posMinGScore =
        distance(gScoresNeighbors.begin(),
                 min_element(gScoresNeighbors.begin(), gScoresNeighbors.end()));
    currentCell = neighborCells[posMinGScore];

    // insert the neighbor in the path
    path.insert(path.begin() + path.size(), currentCell);
  }
  for (uint i = 0; i < path.size(); i++)
    bestPath.insert(bestPath.begin() + bestPath.size(),
                    path[path.size() - (i + 1)]);

  return bestPath;
}

/*******************************************************************************/
// Function Name: calculateHCost
// Inputs:the cellID and the goalCell
// Output: the distance between the current cell and the goal cell
// Description: it is used to calculate the hCost
/*********************************************************************************/
/*
float RAstarPlannerROS::calculateHCost(int cellID, int goalCell)
{
  int x1=getCellRowID(goalCell);
  int y1=getCellColID(goalCell);
  int x2=getCellRowID(cellID);
  int y2=getCellColID(cellID);

  //if(getNeighborNumber()==4)
    //The diagonal shortcut distance between two grid points (x1,y1) and (x2,y2)
is:
    //  return min(abs(x1-x2),abs(y1-y2))*sqrt(2) +
max(abs(x1-x2),abs(y1-y2))-min(abs(x1-x2),abs(y1-y2));

  //else
    //manhatten distance for 8 neighbor
    return abs(x1-x2)+abs(y1-y2);
}
*/
/*******************************************************************************/
// Function Name: addNeighborCellToOpenList
// Inputs: the open list, the neighbors Cell, the g_score matrix, the goal cell
// Output:
// Description: it is used to add a neighbor Cell to the open list
/*********************************************************************************/
void RAstarPlannerROS::addNeighborCellToOpenList(multiset<cells>& OPL,
                                                 int neighborCell, int goalCell,
                                                 float g_score[]) {
  cells CP;
  CP.currentCell = neighborCell;  // insert the neighbor cell
  CP.fCost = g_score[neighborCell] + calculateHCost(neighborCell, goalCell);
  OPL.insert(CP);
  // multiset<cells>::iterator it = OPL.lower_bound(CP);
  // multiset<cells>::iterator it = OPL.upper_bound(CP);
  // OPL.insert( it, CP  );
}

/*******************************************************************************
 * Function Name: findFreeNeighborCell
 * Inputs: the row and columun of the current Cell
 * Output: a vector of free neighbor cells of the current cell
 * Description:it is used to find the free neighbors Cells of a the current Cell
 *in the grid Check Status: Checked by Anis, Imen and Sahar
 *********************************************************************************/

vector<int> RAstarPlannerROS::findFreeNeighborCell(int CellID) {
  int rowID = getCellRowID(CellID);
  int colID = getCellColID(CellID);
  int neighborIndex;
  vector<int> freeNeighborCells;

  for (int i = -1; i <= 1; i++)
    for (int j = -1; j <= 1; j++) {
      // check whether the index is valid
      if ((rowID + i >= 0) && (rowID + i < height) && (colID + j >= 0) &&
          (colID + j < width) && (!(i == 0 && j == 0))) {
        neighborIndex = getCellIndex(rowID + i, colID + j);
        if (isFree(neighborIndex)) freeNeighborCells.push_back(neighborIndex);
      }
    }
  return freeNeighborCells;
}

/*******************************************************************************/
// Function Name: isStartAndGoalCellsValid
// Inputs: the start and Goal cells
// Output: true if the start and the goal cells are valid
// Description: check if the start and goal cells are valid
/*********************************************************************************/
bool RAstarPlannerROS::isStartAndGoalCellsValid(int startCell, int goalCell) {
  bool isvalid = true;
  bool isFreeStartCell = isFree(startCell);
  bool isFreeGoalCell = isFree(goalCell);
  if (startCell == goalCell) {
    // cout << "The Start and the Goal cells are the same..." << endl;
    isvalid = false;
  } else {
    if (!isFreeStartCell && !isFreeGoalCell) {
      // cout << "The start and the goal cells are obstacle positions..." <<
      // endl;
      isvalid = false;
    } else {
      if (!isFreeStartCell) {
        // cout << "The start is an obstacle..." << endl;
        isvalid = false;
      } else {
        if (!isFreeGoalCell) {
          // cout << "The goal cell is an obstacle..." << endl;
          isvalid = false;
        } else {
          if (findFreeNeighborCell(goalCell).size() == 0) {
            // cout << "The goal cell is encountred by obstacles... "<< endl;
            isvalid = false;
          } else {
            if (findFreeNeighborCell(startCell).size() == 0) {
              // cout << "The start cell is encountred by obstacles... "<< endl;
              isvalid = false;
            }
          }
        }
      }
    }
  }
  return isvalid;
}

float RAstarPlannerROS::getMoveCost(int i1, int j1, int i2, int j2) {
  float moveCost = INFINIT_COST;  // start cost with maximum value. Change it to
                                  // real cost of cells are connected
  // if cell2(i2,j2) exists in the diagonal of cell1(i1,j1)
  if ((j2 == j1 + 1 && i2 == i1 + 1) || (i2 == i1 - 1 && j2 == j1 + 1) ||
      (i2 == i1 - 1 && j2 == j1 - 1) || (j2 == j1 - 1 && i2 == i1 + 1)) {
    // moveCost = DIAGONAL_MOVE_COST;
    moveCost = 1.4;
  }
  // if cell 2(i2,j2) exists in the horizontal or vertical line with
  // cell1(i1,j1)
  else {
    if ((j2 == j1 && i2 == i1 - 1) || (i2 == i1 && j2 == j1 - 1) ||
        (i2 == i1 + 1 && j2 == j1) || (i1 == i2 && j2 == j1 + 1)) {
      // moveCost = MOVE_COST;
      moveCost = 1;
    }
  }
  return moveCost;
}

float RAstarPlannerROS::getMoveCost(int CellID1, int CellID2) {
  int i1 = 0, i2 = 0, j1 = 0, j2 = 0;

  i1 = getCellRowID(CellID1);
  j1 = getCellColID(CellID1);
  i2 = getCellRowID(CellID2);
  j2 = getCellColID(CellID2);

  return getMoveCost(i1, j1, i2, j2);
}

// verify if the cell(i,j) is free
bool RAstarPlannerROS::isFree(int i, int j) {
  int CellID = getCellIndex(i, j);
  return OGM[CellID];
}

// verify if the cell(i,j) is free
bool RAstarPlannerROS::isFree(int CellID) { return OGM[CellID]; }
};  // namespace RAstar_planner

bool operator<(cells const& c1, cells const& c2) { return c1.fCost < c2.fCost; }

已构建插件库并导出插件
CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(relaxed_astar)
find_package(catkin REQUIRED COMPONENTS
  nav_core
  roscpp
  rospy
  std_msgs
  actionlib
  actionlib_msgs
)
catkin_package()
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)
add_library(relaxed_astar_lib
  src/RAstar_ros.cpp
)

package.xml

<?xml version="1.0"?>
<package format="2">
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>nav_core</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>actionlib</build_depend>
  <build_depend>actionlib_msgs</build_depend>
  <build_export_depend>nav_core</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>actionlib</build_export_depend>
  <build_export_depend>actionlib_msgs</build_export_depend>
  <exec_depend>nav_core</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>actionlib</exec_depend>
  <exec_depend>actionlib_msgs</exec_depend>



  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->
    <!--STEP2-3.registering plugin with ROS package system-->
    <nav_core plugin="${prefix}/relaxed_astar_planner_plugin.xml" />
  </export>
</package>

构建插件库成功但未集成到ROS工具链
8b180d20-1e0d-41a7-984d-bf3f6b6234c2-图片.png

relaxed_astar_planner_plugin.xml

<!--增加一个配置文件说明该插件的功能-->
<!--STEP2-2.write all the important information about your plugin-->
<library path="lib/librelaxed_astar_lib">
    <class name="RAstar_planner/RAstarPlannerROS" type="RAstar_planner::RAstarPlannerROS" base_class_type="nav_core::BaseGlobalPlanner">
        <description>This is RA∗ global planner plugin by iroboapp project.</description>
    </class>
</library>

小鱼

@924453613 用rospack之前是否source了devel下的setup.bash

924453613

代码是github上的，还在学习阶段

小鱼

@924453613 用rospack之前是否source了devel下的setup.bash

924453613

@小鱼 rospack之前原来要先source一下，已解决，谢谢鱼总
094cc34f-209f-4099-8d5c-9ef13425b2d7-图片.png

小鱼

@924453613 解决了就好，主要你的提问格式和内容描述的都很好，可以让我很快定位问题，赞一个