Commit b03fa0ad authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Initial commit

parents
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})
#!/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"))
#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
<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>
<?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>
/*********************************************************************
*
* 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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment