Initial commit

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(iri_opendrive_global_planner)
+
+ADD_DEFINITIONS(-D_HAVE_XSD)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    costmap_2d
+    dynamic_reconfigure
+    geometry_msgs
+    nav_core
+    nav_msgs
+    pluginlib
+    roscpp
+    tf2_geometry_msgs
+    tf2_ros
+)
+
+find_package(iriutils REQUIRED)
+FIND_PACKAGE(iri_autonomous_driving_tools REQUIRED)
+find_package(iri_opendrive_road_map REQUIRED)
+
+generate_dynamic_reconfigure_options(
+  cfg/OpendriveGlobalPlanner.cfg
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    costmap_2d
+    dynamic_reconfigure
+    geometry_msgs
+    nav_core
+    nav_msgs
+    pluginlib
+    roscpp
+    tf2_ros
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${iriutils_INCLUDE_DIRS}
+  ${iri_autonomous_driving_tools_INCLUDE_DIR}
+  ${iri_opendrive_road_map_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME}
+  src/opendrive_planner.cpp
+)
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iri_autonomous_driving_tools_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iri_opendrive_road_map_LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE)
+
+install(FILES opendrive_gp_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

cfg/OpendriveGlobalPlanner.cfg

+#!/usr/bin/env python
+PACKAGE = "iri_opendrive_global_planner"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+gen.add("opendrive_file", str_t, 0, "Opendrive map file", "")
+
+exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner"))

include/iri_opendrive_global_planner/opendrive_planner.h

+#ifndef _OPENDRIVE_PLANNER_H
+#define _OPENDRIVE_PLANNER_H
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+#include <ros/ros.h>
+#include <costmap_2d/costmap_2d.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Point.h>
+#include <nav_msgs/Path.h>
+#include <vector>
+#include <nav_core/base_global_planner.h>
+#include <nav_msgs/GetPlan.h>
+#include <dynamic_reconfigure/server.h>
+#include <iri_opendrive_global_planner/OpendriveGlobalPlannerConfig.h>
+
+#include "road_map.h"
+
+namespace iri_opendrive_global_planner {
+
+/**
+ * @class PlannerCore
+ * @brief Provides a ROS wrapper for the global_planner planner which runs a fast, interpolated navigation function on a costmap.
+ */
+
+class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner 
+{
+  public:
+    /**
+     * @brief  Default constructor for the PlannerCore object
+     */
+    OpendriveGlobalPlanner();
+    /**
+     * @brief  Constructor for the PlannerCore object
+     * @param  name The name of this planner
+     * @param  costmap A pointer to the costmap to use
+     * @param  frame_id Frame of the costmap
+     */
+    OpendriveGlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
+    /**
+     * @brief  Default deconstructor for the PlannerCore object
+     */
+    ~OpendriveGlobalPlanner();
+    /**
+     * @brief  Initialization function for the PlannerCore object
+     * @param  name The name of this planner
+     * @param  costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
+     */
+    void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
+
+    void initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
+
+    /**
+     * @brief Given a goal pose in the world, compute a plan
+     * @param start The start pose
+     * @param goal The goal pose
+     * @param plan The plan... filled by the planner
+     * @return True if a valid plan was found, false otherwise
+     */
+    bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan);
+
+    /**
+     * @brief Given a goal pose in the world, compute a plan
+     * @param start The start pose
+     * @param goal The goal pose
+     * @param tolerance The tolerance on the goal point for the planner
+     * @param plan The plan... filled by the planner
+     * @return True if a valid plan was found, false otherwise
+     */
+    bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance,std::vector<geometry_msgs::PoseStamped>& plan);
+
+    /**
+     * @brief  Publish a path for visualization purposes
+     */
+    void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
+
+    bool makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp);
+
+  protected:
+
+    /**
+     * @brief Store a copy of the current costmap in \a costmap.  Called by makePlan.
+     */
+    costmap_2d::Costmap2D* costmap_;
+    std::string frame_id_;
+    ros::Publisher plan_pub_;
+    bool initialized_;
+
+    CRoadMap roadmap;        
+
+  private:
+    void mapToWorld(double mx, double my, double& wx, double& wy);
+    bool worldToMap(double wx, double wy, double& mx, double& my);
+    void clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my);
+
+    boost::mutex mutex_;
+    ros::ServiceServer make_plan_srv_;
+
+    dynamic_reconfigure::Server<iri_opendrive_global_planner::OpendriveGlobalPlannerConfig> *dsrv_;
+    void reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig &new_config, uint32_t level);
+    iri_opendrive_global_planner::OpendriveGlobalPlannerConfig config;
+
+};
+
+} //end namespace iri_opendrive_global_planner
+
+#endif

opendrive_gp_plugin.xml

+<library path="lib/libiri_opendrive_global_planner">
+  <class name="iri_opendrive_global_planner/OpendriveGlobalPlanner" type="iri_opendrive_global_planner::OpendriveGlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
+    <description>
+      A implementation of a global planner based on an OpenDrive map.
+    </description>
+  </class>
+</library>

package.xml

+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>iri_opendrive_global_planner</name>
+  <version>1.16.7</version>
+  <description>
+     A path planner library and node.
+  </description>
+  <license>LGPL</license>
+
+  <maintainer email="shernand@iri.upc.edi">Sergi Hernandez Juan</maintainer>
+  <author>Sergi Hernandez Juan</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>angles</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+
+  <depend>costmap_2d</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_core</depend>
+  <depend>nav_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>tf2_ros</depend>
+
+  <export>
+      <nav_core plugin="${prefix}/opendrive_gp_plugin.xml" />
+  </export>
+
+</package>

src/opendrive_planner.cpp

+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+#include <iri_opendrive_global_planner/opendrive_planner.h>
+#include <pluginlib/class_list_macros.h>
+#include <costmap_2d/cost_values.h>
+#include <costmap_2d/costmap_2d.h>
+#include <tf/transform_datatypes.h>
+
+#include "exceptions.h"
+
+//register this planner as a BaseOpendriveGlobalPlanner plugin
+PLUGINLIB_EXPORT_CLASS(iri_opendrive_global_planner::OpendriveGlobalPlanner, nav_core::BaseGlobalPlanner)
+
+namespace iri_opendrive_global_planner {
+
+OpendriveGlobalPlanner::OpendriveGlobalPlanner() :
+  costmap_(NULL), initialized_(false) {
+}
+
+OpendriveGlobalPlanner::OpendriveGlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
+  OpendriveGlobalPlanner() 
+{
+  //initialize the planner
+  initialize(name, costmap, frame_id);
+}
+
+OpendriveGlobalPlanner::~OpendriveGlobalPlanner() 
+{
+  if (dsrv_)
+    delete dsrv_;
+}
+
+void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 
+{
+  initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
+}
+
+void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) 
+{
+  if (!initialized_) 
+  {
+    ros::NodeHandle private_nh("~/" + name);
+    costmap_ = costmap;
+    frame_id_ = frame_id;
+
+    try{
+      std::string opendrive_file;
+      private_nh.param("opendrive_file", opendrive_file,std::string(""));
+      if(opendrive_file!="")
+        this->roadmap.load(opendrive_file);
+    }catch(CException &e){
+      ROS_ERROR_STREAM(e.what());
+    }
+
+    plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
+    make_plan_srv_ = private_nh.advertiseService("make_plan", &OpendriveGlobalPlanner::makePlanService, this);
+
+    dsrv_ = new dynamic_reconfigure::Server<iri_opendrive_global_planner::OpendriveGlobalPlannerConfig>(ros::NodeHandle("~/" + name));
+    dynamic_reconfigure::Server<iri_opendrive_global_planner::OpendriveGlobalPlannerConfig>::CallbackType cb = boost::bind(&OpendriveGlobalPlanner::reconfigureCB, this, _1, _2);
+    dsrv_->setCallback(cb);
+
+    initialized_ = true;
+  } else
+    ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
+}
+
+void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig& new_config, uint32_t level) 
+{
+  try{
+    if(new_config.opendrive_file!=this->config.opendrive_file)
+      this->roadmap.load(new_config.opendrive_file);
+    this->config=new_config;
+  }catch(CException &e){
+    ROS_ERROR_STREAM(e.what());
+  }
+}
+
+void OpendriveGlobalPlanner::clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my) 
+{
+  if (!initialized_) 
+  {
+    ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
+    return;
+  }
+
+  //set the associated costs in the cost map to be free
+  costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);
+}
+
+bool OpendriveGlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) 
+{
+  makePlan(req.start, req.goal, resp.plan.poses);
+
+  resp.plan.header.stamp = ros::Time::now();
+  resp.plan.header.frame_id = frame_id_;
+
+  return true;
+}
+
+void OpendriveGlobalPlanner::mapToWorld(double mx, double my, double& wx, double& wy) 
+{
+  wx = costmap_->getOriginX() + mx * costmap_->getResolution();
+  wy = costmap_->getOriginY() + my * costmap_->getResolution();
+}
+
+bool OpendriveGlobalPlanner::worldToMap(double wx, double wy, double& mx, double& my) 
+{
+  double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
+  double resolution = costmap_->getResolution();
+
+  if (wx < origin_x || wy < origin_y)
+    return false;
+
+  mx = (wx - origin_x) / resolution;
+  my = (wy - origin_y) / resolution;
+
+  if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY())
+    return true;
+
+  return false;
+}
+
+bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan) 
+{
+  return makePlan(start, goal, 0.0, plan);
+}
+
+bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) 
+{
+  double roll, pitch, yaw;
+  std::vector<unsigned int> path;
+  std::vector<double> x,y;
+
+  boost::mutex::scoped_lock lock(mutex_);
+  if (!initialized_) {
+    ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
+    return false;
+  }
+
+  //clear the plan, just in case
+  plan.clear();
+
+  ros::NodeHandle n;
+  std::string global_frame = frame_id_;
+
+  //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
+  if (goal.header.frame_id != global_frame) 
+  {
+    ROS_ERROR("The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
+    return false;
+  }
+
+  if (start.header.frame_id != global_frame) 
+  {
+    ROS_ERROR("The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
+    return false;
+  }
+
+/*
+  double wx = start.pose.position.x;
+  double wy = start.pose.position.y;
+
+  unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
+  double start_x, start_y, goal_x, goal_y;
+
+  if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) 
+  {
+    ROS_WARN("The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
+    return false;
+  }
+  worldToMap(wx, wy, start_x, start_y);
+
+  //clear the starting cell within the costmap because we know it can't be an obstacle
+  clearRobotCell(start, start_x_i, start_y_i);
+
+  int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
+*/
+  try{
+    ROS_WARN("Make Plan");
+    tf::Quaternion q_start(start.pose.orientation.x,start.pose.orientation.y,start.pose.orientation.z,start.pose.orientation.w);
+    tf::Matrix3x3 m_start(q_start);
+    m_start.getRPY(roll, pitch, yaw);
+    ROS_WARN_STREAM("start position: x: " << start.pose.position.x << ", y: " << start.pose.position.y << ", yaw: " << yaw);
+    this->roadmap.set_start_point(start.pose.position.x,start.pose.position.y,yaw);
+    tf::Quaternion q_goal(goal.pose.orientation.x,goal.pose.orientation.y,goal.pose.orientation.z,goal.pose.orientation.w);
+    tf::Matrix3x3 m_goal(q_goal);
+    m_goal.getRPY(roll, pitch, yaw);
+    ROS_WARN_STREAM("start position: x: " << goal.pose.position.x << ", y: " << goal.pose.position.y << ", yaw: " << yaw);
+    this->roadmap.set_target_point(goal.pose.position.x,goal.pose.position.y,yaw);
+    this->roadmap.find_shortest_path();
+    this->roadmap.get_path(path);
+    for(unsigned int i=0;i<path.size();i++)
+      std::cout << path[i] << ",";
+    std::cout << std::endl;
+    this->roadmap.get_trajectory(x,y);
+    plan.resize(x.size());
+    ros::Time plan_time=ros::Time::now();
+    for(unsigned int i=0;i<x.size();i++)
+    {
+      plan[i].header.frame_id=frame_id_;
+      plan[i].header.stamp=plan_time;
+      plan[i].pose.position.x=x[i];
+      plan[i].pose.position.y=y[i];
+      plan[i].pose.position.y=y[i];
+      plan[i].pose.orientation.x=0.0;
+      plan[i].pose.orientation.y=0.0;
+      plan[i].pose.orientation.z=0.0;
+      plan[i].pose.orientation.w=1.0;
+    }
+  }catch(CException &e){
+    ROS_ERROR_STREAM(e.what());
+    return false;
+  }
+  //make sure to resize the underlying array that Navfn uses
+
+  //publish the plan for visualization purposes
+  publishPlan(plan);
+  return !plan.empty();
+}
+
+void OpendriveGlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) 
+{
+  if (!initialized_) 
+  {
+    ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
+    return;
+  }
+
+  //create a message for the plan
+  nav_msgs::Path gui_path;
+  gui_path.poses.resize(path.size());
+
+  gui_path.header.frame_id = frame_id_;
+  gui_path.header.stamp = ros::Time::now();
+
+  // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+  for (unsigned int i = 0; i < path.size(); i++) 
+  {
+    gui_path.poses[i] = path[i];
+  }
+
+  plan_pub_.publish(gui_path);
+}
+
+} //end namespace iri_opendrive_global_planner