Skip to content
Snippets Groups Projects
Commit b7012e88 authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Delete old files

parent 4c068a33
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 2590 deletions
Makefile deleted 100755 → 0
+ 0
− 1
View file @ 4c068a33
include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
cfg/ackermann_local_planner_alg_config.cfg deleted 100755 → 0
+ 0
− 76
View file @ 4c068a33
#! /usr/bin/env python
#* 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 the Willow Garage 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:
PACKAGE='iri_ackermann_local_planner'
import roslib; roslib.load_manifest(PACKAGE)
import sys
from math import pi
from dynamic_reconfigure.parameter_generator import *
gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
gen.add("ack_vel_max", double_t, 0, "maximum translational speed", 0.3, 0, 20)
gen.add("ack_vel_min", double_t, 0, "minimum translational speed", -0.3, -20, 0)
gen.add("ack_acc_max", double_t, 0, "maximum translational acceleration", 1.0, 0, 20)
gen.add("ack_steer_angle_max", double_t, 0, "maximum steer angle", 0.45, 0, 0.54)
gen.add("ack_steer_angle_min", double_t, 0, "minimum steer angle", -0.45, -0.54, 0)
gen.add("ack_steer_speed_max", double_t, 0, "maximum steer speed", 1.0, 0, 2)
gen.add("ack_steer_speed_min", double_t, 0, "minimum steer speed", -1.0, -2, 0)
gen.add("ack_steer_acc_max", double_t, 0, "maximum steer acceleration", 0.36, 0, 5)
gen.add("ack_axis_distance", double_t, 0, "distance between axes", 1.65, 0, 2)
gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 10, 0, 10)
gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0, 5)
gen.add("pdist_scale", double_t, 0, "The weight for the path distance part of the cost function", 0.6, 0, 5)
gen.add("gdist_scale", double_t, 0, "The weight for the goal distance part of the cost function", 0.8, 0, 5)
gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01,0, 5)
gen.add("hdiff_scale", double_t, 0, "The weight for the heading distance part of the cost function", 1.0 ,0, 500)
gen.add("heading_points", int_t, 0, "The number of points to check the heading", 8, 1, 64)
gen.add("vx_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 20, 1, 300)
gen.add("vtheta_samples", int_t, 0, "The number of samples to use when exploring the theta velocity space", 20, 1, 300)
gen.add("simple_attractor", bool_t, 0, "Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation", False)
gen.add("angular_sim_granularity", double_t, 0, "The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things", 0.025, 0, pi/2)
gen.add("xy_goal_tolerance", double_t, 0, "Minimum distance to end the current goal", 0.5, 0, 1)
gen.add("heading_goal_tolerance", double_t, 0, "Minimum angle to end the current goal", 0.1, 0, pi/2)
gen.add("restore_defaults", bool_t, 0, "Retore to the default configuration", False)
exit(gen.generate(PACKAGE, "AckermannLocalPlannerAlgorithm", "AckermannLocalPlanner"))
include/costmap_model.h deleted 100755 → 0
+ 0
− 98
View file @ 4c068a33
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
#define TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
#include <world_model.h>
// For obstacle data access
#include <costmap_2d/costmap_2d.h>
namespace iri_ackermann_local_planner {
/**
* @class CostmapModel
* @brief A class that implements the WorldModel interface to provide grid
* based collision checks for the trajectory controller using the costmap.
*/
class CostmapModel : public WorldModel {
public:
/**
* @brief Constructor for the CostmapModel
* @param costmap The costmap that should be used
* @return
*/
CostmapModel(const costmap_2d::Costmap2D& costmap);
/**
* @brief Destructor for the world model
*/
virtual ~CostmapModel(){}
/**
* @brief Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
private:
/**
* @brief Rasterizes a line in the costmap grid and checks for collisions
* @param x0 The x position of the first cell in grid coordinates
* @param y0 The y position of the first cell in grid coordinates
* @param x1 The x position of the second cell in grid coordinates
* @param y1 The y position of the second cell in grid coordinates
* @return A positive cost for a legal line... negative otherwise
*/
double lineCost(int x0, int x1, int y0, int y1);
/**
* @brief Checks the cost of a point in the costmap
* @param x The x position of the point in cell coordinates
* @param y The y position of the point in cell coordinates
* @return A positive cost for a legal point... negative otherwise
*/
double pointCost(int x, int y);
const costmap_2d::Costmap2D& costmap_; ///< @brief Allows access of costmap obstacle information
};
};
#endif
include/goal_functions.h deleted 100755 → 0
+ 0
− 143
View file @ 4c068a33
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, 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
*********************************************************************/
#ifndef BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
#define BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <tf/transform_listener.h>
#include <string>
#include <cmath>
#include <angles/angles.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_ros.h>
namespace iri_ackermann_local_planner {
/**
* @brief Compute the distance between two points
* @param x1 The first x point
* @param y1 The first y point
* @param x2 The second x point
* @param y2 The second y point
*/
double distance(double x1, double y1, double x2, double y2);
/**
* @brief Check if the goal position has been achieved
* @param global_pose The pose of the robot in the global frame
* @param goal_x The desired x value for the goal
* @param goal_y The desired y value for the goal
* @param xy_goal_tolerance The tolerance on the position
* @return True if achieved, false otherwise
*/
bool goalPositionReached(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y, double xy_goal_tolerance);
/**
* @brief Check if the goal orientation has been achieved
* @param global_pose The pose of the robot in the global frame
* @param goal_x The desired x value for the goal
* @param goal_y The desired y value for the goal
* @param yaw_goal_tolerance The tolerance on the position
* @return True if achieved, false otherwise
*/
bool goalOrientationReached(const tf::Stamped<tf::Pose>& global_pose, double goal_th, double yaw_goal_tolerance);
/**
* @brief Publish a plan for visualization purposes
* @param path The plan to publish
* @param pub The published to use
* @param r,g,b,a The color and alpha value to use when publishing
*/
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, double r, double g, double b, double a);
/**
* @brief Trim off parts of the global plan that are far enough behind the robot
* @param global_pose The pose of the robot in the global frame
* @param plan The plan to be pruned
* @param global_plan The plan to be pruned in the frame of the planner
*/
void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan);
/**
* @brief Transforms the global plan of the robot from the planner frame to the local frame
* @param tf A reference to a transform listener
* @param global_plan The plan to be transformed
* @param costmap A reference to the costmap being used so the window size for transforming can be computed
* @param global_frame The frame to transform the plan to
* @param transformed_plan Populated with the transformed plan
*/
bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan);
/**
* @brief Check if the goal pose has been achieved
* @param tf A reference to a transform listener
* @param global_plan The plan being followed
* @param costmap_ros A reference to the costmap object being used by the planner
* @param global_frame The global frame of the local planner
* @param base_odom The current odometry information for the robot
* @param rot_stopped_vel The rotational velocity below which the robot is considered stopped
* @param trans_stopped_vel The translational velocity below which the robot is considered stopped
* @param xy_goal_tolerance The translational tolerance on reaching the goal
* @param yaw_goal_tolerance The rotational tolerance on reaching the goal
* @return True if achieved, false otherwise
*/
bool isGoalReached(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2DROS& costmap_ros, const std::string& global_frame,
const nav_msgs::Odometry& base_odom, double rot_stopped_vel, double trans_stopped_vel,
double xy_goal_tolerance, double yaw_goal_tolerance);
/**
* @brief Check whether the robot is stopped or not
* @param base_odom The current odometry information for the robot
* @param rot_stopped_velocity The rotational velocity below which the robot is considered stopped
* @param trans_stopped_velocity The translational velocity below which the robot is considered stopped
* @return True if the robot is stopped, false otherwise
*/
bool stopped(const nav_msgs::Odometry& base_odom,
const double& rot_stopped_velocity, const double& trans_stopped_velocity);
};
#endif
include/map_cell.h deleted 100755 → 0
+ 0
− 73
View file @ 4c068a33
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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.
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_MAP_CELL_H_
#define TRAJECTORY_ROLLOUT_MAP_CELL_H_
#include <trajectory_inc.h>
namespace iri_ackermann_local_planner {
/**
* @class MapCell
* @brief Stores path distance and goal distance information used for scoring trajectories
*/
class MapCell{
public:
/**
* @brief Default constructor
*/
MapCell();
/**
* @brief Copy constructor
* @param mc The MapCell to be copied
*/
MapCell(const MapCell& mc);
unsigned int cx, cy; ///< @brief Cell index in the grid map
double path_dist; ///< @brief Distance to planner's path
double goal_dist; ///< @brief Distance to local goal
double occ_dist; ///< @brief Distance to obstacles
int occ_state; ///< @brief Occupancy state (-1 = free, 0 = unknown, 1 = occupied)
bool path_mark, goal_mark; ///< @brief Marks fir computing path/goal distances
bool within_robot; ///< @brief Mark for cells within the robot footprint
};
};
#endif
include/map_grid.h deleted 100755 → 0
+ 0
− 220
View file @ 4c068a33
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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.
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_MAP_GRID_H_
#define TRAJECTORY_ROLLOUT_MAP_GRID_H_
#include <vector>
#include <iostream>
#include <trajectory_inc.h>
#include <ros/console.h>
#include <ros/ros.h>
#include <map_cell.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/PoseStamped.h>
namespace iri_ackermann_local_planner{
/**
* @class MapGrid
* @brief A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controller.
*/
class MapGrid{
public:
/**
* @brief Creates a 0x0 map by default
*/
MapGrid();
/**
* @brief Creates a map of size_x by size_y
* @param size_x The width of the map
* @param size_y The height of the map
*/
MapGrid(unsigned int size_x, unsigned int size_y);
/**
* @brief Creates a map of size_x by size_y with the desired scale and origin
* @param size_x The width of the map
* @param size_y The height of the map
* @param scale The resolution of each MapCell
* @param x The x coordinate of the origin of the map
* @param y The y coordinate of the origin of the map
*/
MapGrid(unsigned int size_x, unsigned int size_y, double scale, double x, double y);
/**
* @brief Returns a map cell accessed by (col, row)
* @param x The x coordinate of the cell
* @param y The y coordinate of the cell
* @return A reference to the desired cell
*/
inline MapCell& operator() (unsigned int x, unsigned int y){
return map_[size_x_ * y + x];
}
/**
* @brief Returns a map cell accessed by (col, row)
* @param x The x coordinate of the cell
* @param y The y coordinate of the cell
* @return A copy of the desired cell
*/
inline MapCell operator() (unsigned int x, unsigned int y) const {
return map_[size_x_ * y + x];
}
inline MapCell& getCell(unsigned int x, unsigned int y){
return map_[size_x_ * y + x];
}
/**
* @brief Destructor for a MapGrid
*/
~MapGrid(){}
/**
* @brief Copy constructor for a MapGrid
* @param mg The MapGrid to copy
*/
MapGrid(const MapGrid& mg);
/**
* @brief Assignment operator for a MapGrid
* @param mg The MapGrid to assign from
*/
MapGrid& operator= (const MapGrid& mg);
/**
* @brief reset path distance fields for all cells
*/
void resetPathDist();
/**
* @brief check if we need to resize
* @param size_x The desired width
* @param size_y The desired height
* @param o_x the desired x coordinate of the origin
* @param o_y the desired y coordinate of the origin
*/
void sizeCheck(unsigned int size_x, unsigned int size_y, double o_x, double o_y);
/**
* @brief Utility to share initialization code across constructors
*/
void commonInit();
/**
* @brief Returns a 1D index into the MapCell array for a 2D index
* @param x The desired x coordinate
* @param y The desired y coordinate
* @return The associated 1D index
*/
size_t getIndex(int x, int y);
/**
* @brief Used to update the distance of a cell in path distance computation
* @param current_cell The cell we're currently in
* @param check_cell The cell to be updated
*/
inline void updatePathCell(MapCell* current_cell, MapCell* check_cell,
std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
//mark the cell as visisted
check_cell->path_mark = true;
//if the cell is an obstacle set the max path distance
unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
if(!getCell(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || cost == costmap_2d::NO_INFORMATION)){
check_cell->path_dist = map_.size();
return;
}
double new_path_dist = current_cell->path_dist + 1;
if(new_path_dist < check_cell->path_dist)
check_cell->path_dist = new_path_dist;
dist_queue.push(check_cell);
}
/**
* @brief Used to update the distance of a cell in goal distance computation
* @param current_cell The cell we're currently in
* @param check_cell The cell to be updated
*/
inline void updateGoalCell(MapCell* current_cell, MapCell* check_cell,
std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap ){
///mark the cell as visited
check_cell->goal_mark = true;
//if the cell is an obstacle set the max goal distance
unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
if(!getCell(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || cost == costmap_2d::NO_INFORMATION)){
check_cell->goal_dist = map_.size();
return;
}
double new_goal_dist = current_cell->goal_dist + 1;
if(new_goal_dist < check_cell->goal_dist)
check_cell->goal_dist = new_goal_dist;
dist_queue.push(check_cell);
}
/**
* @brief Compute the distance from each cell in the local map grid to the planned path
* @param dist_queue A queue of the initial cells on the path
*/
void computePathDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
/**
* @brief Compute the distance from each cell in the local map grid to the local goal point
* @param goal_queue A queue containing the local goal cell
*/
void computeGoalDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
/**
* @brief Update what cells are considered path based on the global plan
*/
void setPathCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan);
unsigned int size_x_, size_y_; ///< @brief The dimensions of the grid
std::vector<MapCell> map_; ///< @brief Storage for the MapCells
double scale; ///< @brief grid scale in meters/cell
double goal_x_, goal_y_; /**< @brief The goal distance was last computed from */
double origin_x, origin_y; ///< @brief lower left corner of grid in world space
};
};
#endif
include/map_grid_cost_point.h deleted 100755 → 0
+ 0
− 60
View file @ 4c068a33
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Eric Perko
* 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 Eric Perko 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.
*********************************************************************/
#ifndef MAP_GRID_COST_POINT_H_
#define MAP_GRID_COST_POINT_H_
#include <pcl/ros/register_point_struct.h>
namespace iri_ackermann_local_planner {
struct MapGridCostPoint {
float x;
float y;
float z;
float path_cost;
float goal_cost;
float occ_cost;
float total_cost;
};
}
POINT_CLOUD_REGISTER_POINT_STRUCT(
iri_ackermann_local_planner::MapGridCostPoint,
(float, x, x)
(float, y, y)
(float, z, z)
(float, path_cost, path_cost)
(float, goal_cost, goal_cost)
(float, occ_cost, occ_cost)
(float, total_cost, total_cost));
#endif
include/map_grid_visualizer.h deleted 100755 → 0
+ 0
− 81
View file @ 4c068a33
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Eric Perko
* 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 Eric Perko 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.
*********************************************************************/
#ifndef MAP_GRID_VISUALIZER_H_
#define MAP_GRID_VISUALIZER_H_
#include <ros/ros.h>
#include <map_grid.h>
#include <costmap_2d/costmap_2d.h>
#include <map_grid_cost_point.h>
#include <pcl_ros/publisher.h>
namespace iri_ackermann_local_planner {
class MapGridVisualizer {
public:
/**
* @brief Default constructor
*/
MapGridVisualizer();
/**
* @brief Initializes the MapGridVisualizer
* @param name The name to be appended to ~/ in order to get the proper namespace for parameters
* @param costmap The costmap instance to use to get the size of the map to generate a point cloud for
* @param cost_function The function to use to compute the cost values to be inserted into each point in the output PointCloud as well as whether to include a given point or not
*/
void initialize(const std::string& name,const costmap_2d::Costmap2D * costmap, boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function);
/**
* Destructor for the visualizer
*/
~MapGridVisualizer() {}
/**
* @brief Build and publish a PointCloud if the publish_cost_grid_pc parameter was true. Only include points for which the cost_function at (cx,cy) returns true.
*/
void publishCostCloud();
private:
std::string name_; ///< @brief The name to get parameters relative to.
std::string frame_id_; ///< @brief The frame to assign to the output PointCloud
boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function_; ///< @brief The function to be used to generate the cost components for the output PointCloud
bool publish_cost_grid_pc_; ///< @brief Whether or not to build and publsih a PointCloud
const costmap_2d::Costmap2D *costmap_p_; ///< @brief The costmap to use to infer the size of the underlying map grid
ros::NodeHandle ns_nh_;
pcl::PointCloud<MapGridCostPoint> cost_cloud_;
pcl_ros::Publisher<MapGridCostPoint> pub_;
};
};
#endif
include/planar_laser_scan.h deleted 100755 → 0
+ 0
− 57
View file @ 4c068a33
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
#define TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
#include <geometry_msgs/Point32.h>
#include <sensor_msgs/PointCloud.h>
namespace iri_ackermann_local_planner {
/**
* @class PlanarLaserScan
* @brief Stores a scan from a planar laser that can be used to clear freespace
*/
class PlanarLaserScan {
public:
PlanarLaserScan() {}
geometry_msgs::Point32 origin;
sensor_msgs::PointCloud cloud;
double angle_min, angle_max, angle_increment;
};
};
#endif
include/point_grid.h deleted 100755 → 0
+ 0
− 356
View file @ 4c068a33
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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
*********************************************************************/
#ifndef POINT_GRID_H_
#define POINT_GRID_H_
#include <vector>
#include <list>
#include <cfloat>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Point32.h>
#include <costmap_2d/observation.h>
#include <world_model.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
namespace iri_ackermann_local_planner {
/**
* @class PointGrid
* @brief A class that implements the WorldModel interface to provide
* free-space collision checks for the trajectory controller. This class
* stores points binned into a grid and performs point-in-polygon checks when
* necessary to determine the legality of a footprint at a given
* position/orientation.
*/
class PointGrid : public WorldModel {
public:
/**
* @brief Constuctor for a grid that stores points in the plane
* @param width The width in meters of the grid
* @param height The height in meters of the gird
* @param resolution The resolution of the grid in meters/cell
* @param origin The origin of the bottom left corner of the grid
* @param max_z The maximum height for an obstacle to be added to the grid
* @param obstacle_range The maximum distance for obstacles to be added to the grid
* @param min_separation The minimum distance between points in the grid
*/
PointGrid(double width, double height, double resolution, geometry_msgs::Point origin,
double max_z, double obstacle_range, double min_separation);
/**
* @brief Destructor for a point grid
*/
virtual ~PointGrid(){}
/**
* @brief Returns the points that lie within the cells contained in the specified range. Some of these points may be outside the range itself.
* @param lower_left The lower left corner of the range search
* @param upper_right The upper right corner of the range search
* @param points A vector of pointers to lists of the relevant points
*/
void getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, std::vector< std::list<pcl::PointXYZ>* >& points);
/**
* @brief Checks if any points in the grid lie inside a convex footprint
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
/**
* @brief Inserts observations from sensors into the point grid
* @param footprint The footprint of the robot in its current location
* @param observations The observations from various sensors
* @param laser_scans The laser scans used to clear freespace (the point grid only uses the first scan which is assumed to be the base laser)
*/
void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
/**
* @brief Convert from world coordinates to grid coordinates
* @param pt A point in world space
* @param gx The x coordinate of the corresponding grid cell to be set by the function
* @param gy The y coordinate of the corresponding grid cell to be set by the function
* @return True if the conversion was successful, false otherwise
*/
inline bool gridCoords(geometry_msgs::Point pt, unsigned int& gx, unsigned int& gy) const {
if(pt.x < origin_.x || pt.y < origin_.y){
gx = 0;
gy = 0;
return false;
}
gx = (int) ((pt.x - origin_.x)/resolution_);
gy = (int) ((pt.y - origin_.y)/resolution_);
if(gx >= width_ || gy >= height_){
gx = 0;
gy = 0;
return false;
}
return true;
}
/**
* @brief Get the bounds in world coordinates of a cell in the point grid, assumes a legal cell when called
* @param gx The x coordinate of the grid cell
* @param gy The y coordinate of the grid cell
* @param lower_left The lower left bounds of the cell in world coordinates to be filled in
* @param upper_right The upper right bounds of the cell in world coordinates to be filled in
*/
inline void getCellBounds(unsigned int gx, unsigned int gy, geometry_msgs::Point& lower_left, geometry_msgs::Point& upper_right) const {
lower_left.x = gx * resolution_ + origin_.x;
lower_left.y = gy * resolution_ + origin_.y;
upper_right.x = lower_left.x + resolution_;
upper_right.y = lower_left.y + resolution_;
}
/**
* @brief Compute the squared distance between two points
* @param pt1 The first point
* @param pt2 The second point
* @return The squared distance between the two points
*/
inline double sq_distance(pcl::PointXYZ& pt1, pcl::PointXYZ& pt2){
return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
}
/**
* @brief Convert from world coordinates to grid coordinates
* @param pt A point in world space
* @param gx The x coordinate of the corresponding grid cell to be set by the function
* @param gy The y coordinate of the corresponding grid cell to be set by the function
* @return True if the conversion was successful, false otherwise
*/
inline bool gridCoords(pcl::PointXYZ pt, unsigned int& gx, unsigned int& gy) const {
if(pt.x < origin_.x || pt.y < origin_.y){
gx = 0;
gy = 0;
return false;
}
gx = (int) ((pt.x - origin_.x)/resolution_);
gy = (int) ((pt.y - origin_.y)/resolution_);
if(gx >= width_ || gy >= height_){
gx = 0;
gy = 0;
return false;
}
return true;
}
/**
* @brief Converts cell coordinates to an index value that can be used to look up the correct grid cell
* @param gx The x coordinate of the cell
* @param gy The y coordinate of the cell
* @return The index of the cell in the stored cell list
*/
inline unsigned int gridIndex(unsigned int gx, unsigned int gy) const {
/*
* (0, 0) ---------------------- (width, 0)
* | |
* | |
* | |
* | |
* | |
* (0, height) ----------------- (width, height)
*/
return(gx + gy * width_);
}
/**
* @brief Check the orientation of a pt c with respect to the vector a->b
* @param a The start point of the vector
* @param b The end point of the vector
* @param c The point to compute orientation for
* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
*/
inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b, const pcl::PointXYZ& c){
double acx = a.x - c.x;
double bcx = b.x - c.x;
double acy = a.y - c.y;
double bcy = b.y - c.y;
return acx * bcy - acy * bcx;
}
/**
* @brief Check the orientation of a pt c with respect to the vector a->b
* @param a The start point of the vector
* @param b The end point of the vector
* @param c The point to compute orientation for
* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
*/
inline double orient(const geometry_msgs::Point32& a, const geometry_msgs::Point32& b, const pcl::PointXYZ& c){
double acx = a.x - c.x;
double bcx = b.x - c.x;
double acy = a.y - c.y;
double bcy = b.y - c.y;
return acx * bcy - acy * bcx;
}
/**
* @brief Check the orientation of a pt c with respect to the vector a->b
* @param a The start point of the vector
* @param b The end point of the vector
* @param c The point to compute orientation for
* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
*/
inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b,
const geometry_msgs::Point& c){
double acx = a.x - c.x;
double bcx = b.x - c.x;
double acy = a.y - c.y;
double bcy = b.y - c.y;
return acx * bcy - acy * bcx;
}
/**
* @brief Check the orientation of a pt c with respect to the vector a->b
* @param a The start point of the vector
* @param b The end point of the vector
* @param c The point to compute orientation for
* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
*/
inline double orient(const pcl::PointXYZ& a, const pcl::PointXYZ& b, const pcl::PointXYZ& c){
double acx = a.x - c.x;
double bcx = b.x - c.x;
double acy = a.y - c.y;
double bcy = b.y - c.y;
return acx * bcy - acy * bcx;
}
/**
* @brief Check if two line segmenst intersect
* @param v1 The first point of the first segment
* @param v2 The second point of the first segment
* @param u1 The first point of the second segment
* @param u2 The second point of the second segment
* @return True if the segments intersect, false otherwise
*/
inline bool segIntersect(const pcl::PointXYZ& v1, const pcl::PointXYZ& v2,
const pcl::PointXYZ& u1, const pcl::PointXYZ& u2){
return (orient(v1, v2, u1) * orient(v1, v2, u2) < 0) && (orient(u1, u2, v1) * orient(u1, u2, v2) < 0);
}
/**
* @brief Find the intersection point of two lines
* @param v1 The first point of the first segment
* @param v2 The second point of the first segment
* @param u1 The first point of the second segment
* @param u2 The second point of the second segment
* @param result The point to be filled in
*/
void intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2,
const geometry_msgs::Point& u1, const geometry_msgs::Point& u2,
geometry_msgs::Point& result);
/**
* @brief Check if a point is in a polygon
* @param pt The point to be checked
* @param poly The polygon to check against
* @return True if the point is in the polygon, false otherwise
*/
bool ptInPolygon(const pcl::PointXYZ& pt, const std::vector<geometry_msgs::Point>& poly);
/**
* @brief Insert a point into the point grid
* @param pt The point to be inserted
*/
void insert(pcl::PointXYZ pt);
/**
* @brief Find the distance between a point and its nearest neighbor in the grid
* @param pt The point used for comparison
* @return The distance between the point passed in and its nearest neighbor in the point grid
*/
double nearestNeighborDistance(pcl::PointXYZ& pt);
/**
* @brief Find the distance between a point and its nearest neighbor in a cell
* @param pt The point used for comparison
* @param gx The x coordinate of the cell
* @param gy The y coordinate of the cell
* @return The distance between the point passed in and its nearest neighbor in the cell
*/
double getNearestInCell(pcl::PointXYZ& pt, unsigned int gx, unsigned int gy);
/**
* @brief Removes points from the grid that lie within the polygon
* @param poly A specification of the polygon to clear from the grid
*/
void removePointsInPolygon(const std::vector<geometry_msgs::Point> poly);
/**
* @brief Removes points from the grid that lie within a laser scan
* @param laser_scan A specification of the laser scan to use for clearing
*/
void removePointsInScanBoundry(const PlanarLaserScan& laser_scan);
/**
* @brief Checks to see if a point is within a laser scan specification
* @param pt The point to check
* @param laser_scan The specification of the scan to check against
* @return True if the point is contained within the scan, false otherwise
*/
bool ptInScan(const pcl::PointXYZ& pt, const PlanarLaserScan& laser_scan);
/**
* @brief Get the points in the point grid
* @param cloud The point cloud to insert the points into
*/
void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
private:
double resolution_; ///< @brief The resolution of the grid in meters/cell
geometry_msgs::Point origin_; ///< @brief The origin point of the grid
unsigned int width_; ///< @brief The width of the grid in cells
unsigned int height_; ///< @brief The height of the grid in cells
std::vector< std::list<pcl::PointXYZ> > cells_; ///< @brief Storage for the cells in the grid
double max_z_; ///< @brief The height cutoff for adding points as obstacles
double sq_obstacle_range_; ///< @brief The square distance at which we no longer add obstacles to the grid
double sq_min_separation_; ///< @brief The minimum square distance required between points in the grid
std::vector< std::list<pcl::PointXYZ>* > points_; ///< @brief The lists of points returned by a range search, made a member to save on memory allocation
};
};
#endif
include/trajectory.h deleted 100755 → 0
+ 0
− 116
View file @ 4c068a33
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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.
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#include <vector>
namespace iri_ackermann_local_planner {
/**
* @class Trajectory
* @brief Holds a trajectory generated by an x, y, and theta velocity
*/
class Trajectory {
public:
/**
* @brief Default constructor
*/
Trajectory();
/**
* @brief Constructs a trajectory
* @param xv The x velocity used to seed the trajectory
* @param yv The y velocity used to seed the trajectory
* @param thetav The theta velocity used to seed the trajectory
* @param num_pts The expected number of points for a trajectory
*/
Trajectory(double xv, double yv, double thetav, unsigned int num_pts);
double xv_, yv_, thetav_; ///< @brief The x, y, and theta velocities of the trajectory
double cost_; ///< @brief The cost/score of the trajectory
/**
* @brief Get a point within the trajectory
* @param index The index of the point to get
* @param x Will be set to the x position of the point
* @param y Will be set to the y position of the point
* @param th Will be set to the theta position of the point
*/
void getPoint(unsigned int index, double& x, double& y, double& th);
/**
* @brief Set a point within the trajectory
* @param index The index of the point to set
* @param x The x position
* @param y The y position
* @param th The theta position
*/
void setPoint(unsigned int index, double x, double y, double th);
/**
* @brief Add a point to the end of a trajectory
* @param x The x position
* @param y The y position
* @param th The theta position
*/
void addPoint(double x, double y, double th);
/**
* @brief Get the last point of the trajectory
* @param x Will be set to the x position of the point
* @param y Will be set to the y position of the point
* @param th Will be set to the theta position of the point
*/
void getEndpoint(double& x, double& y, double& th);
/**
* @brief Clear the trajectory's points
*/
void resetPoints();
/**
* @brief Return the number of points in the trajectory
* @return The number of points in the trajectory
*/
unsigned int getPointsSize();
private:
std::vector<double> x_pts_; ///< @brief The x points in the trajectory
std::vector<double> y_pts_; ///< @brief The y points in the trajectory
std::vector<double> th_pts_; ///< @brief The theta points in the trajectory
};
};
#endif
include/trajectory_inc.h deleted 100755 → 0
+ 0
− 47
View file @ 4c068a33
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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.
*********************************************************************/
#ifndef TRAJECTORY_INC_H_
#define TRAJECTORY_INC_H_
#include <limits>
#ifndef DBL_MAX /* Max decimal value of a double */
#define DBL_MAX std::numeric_limits<double>::max() // 1.7976931348623157e+308
#endif
#ifndef DBL_MIN //Min decimal value of a double
#define DBL_MIN std::numeric_limits<double>::min() // 2.2250738585072014e-308
#endif
#endif
include/trajectory_planner.h deleted 100755 → 0
+ 0
− 322
View file @ 4c068a33
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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.
*
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
#include <vector>
#include <string>
#include <sstream>
#include <math.h>
#include <ros/console.h>
#include <angles/angles.h>
//for creating a local cost grid
#include <map_cell.h>
#include <map_grid.h>
//for obstacle data access
#include <costmap_2d/costmap_2d.h>
#include <world_model.h>
#include <trajectory.h>
//we'll take in a path as a vector of poses
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Twist.h>
#include <iri_ackermann_local_planner/Position2DInt.h>
//for computing path distance
#include <queue>
//for some datatypes
#include <tf/transform_datatypes.h>
#include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
#include <boost/algorithm/string.hpp>
namespace iri_ackermann_local_planner {
/**
* @class TrajectoryPlanner
* @brief Computes control velocities for a robot given a costmap, a plan, and the robot's position in the world.
*/
class TrajectoryPlanner{
friend class TrajectoryPlannerTest; //Need this for gtest to work
public:
/**
* @brief Constructs a trajectory controller
* @param world_model The WorldModel the trajectory controller uses to check for collisions
* @param costmap A reference to the Costmap the controller should use
* @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @param acc_lim_x The acceleration limit of the robot in the x direction
* @param acc_lim_y The acceleration limit of the robot in the y direction
* @param acc_lim_theta The acceleration limit of the robot in the theta direction
* @param sim_time The number of seconds to "roll-out" each trajectory
* @param sim_granularity The distance between simulation points should be small enough that the robot doesn't hit things
* @param vx_samples The number of trajectories to sample in the x dimension
* @param vtheta_samples The number of trajectories to sample in the theta dimension
* @param pdist_scale A scaling factor for how close the robot should stay to the path
* @param gdist_scale A scaling factor for how aggresively the robot should pursue a local goal
* @param occdist_scale A scaling factor for how much the robot should prefer to stay away from obstacles
* @param heading_lookahead How far the robot should look ahead of itself when differentiating between different rotational velocities
* @param oscillation_reset_dist The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
* @param escape_reset_dist The distance the robot must travel before it can exit escape mode
* @param escape_reset_theta The distance the robot must rotate before it can exit escape mode
* @param holonomic_robot Set this to true if the robot being controlled can take y velocities and false otherwise
* @param max_vel_x The maximum x velocity the controller will explore
* @param min_vel_x The minimum x velocity the controller will explore
* @param max_vel_th The maximum rotational velocity the controller will explore
* @param min_vel_th The minimum rotational velocity the controller will explore
* @param min_in_place_vel_th The absolute value of the minimum in-place rotational velocity the controller will explore
* @param backup_vel The velocity to use while backing up
* @param dwa Set this to true to use the Dynamic Window Approach, false to use acceleration limits
* @param heading_scoring Set this to true to score trajectories based on the robot's heading after 1 timestep
* @param heading_scoring_timestep How far to look ahead in time when we score heading based trajectories
* @param simple_attractor Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation
* @param y_vels A vector of the y velocities the controller will explore
* @param angular_sim_granularity The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things
*/
TrajectoryPlanner(WorldModel& world_model,
const costmap_2d::Costmap2D& costmap,
std::vector<geometry_msgs::Point> footprint_spec,
double max_acc = 1.0, double max_vel = 0.3, double min_vel = -0.3,
double max_steer_acc = 1.0, double max_steer_vel = 0.5, double min_steer_vel = -0.5,
double max_steer_angle = 0.35, double min_steer_angle = -0.35, double axis_distance = 1.65,
double sim_time = 10.0, double sim_granularity = 0.025,
int vx_samples = 20, int vtheta_samples = 20,
double pdist_scale = 0.6, double gdist_scale = 0.8, double occdist_scale = 0.01, double hdiff_scale = 1.0,
bool simple_attractor = false, double angular_sim_granularity = 0.025,int heading_points = 8, double xy_goal_tol = 0.5);
/**
* @brief Destructs a trajectory controller
*/
~TrajectoryPlanner();
/**
* @brief Reconfigures the trajectory planner
*/
void reconfigure(AckermannLocalPlannerConfig &cfg);
/**
* @brief Given the current position, orientation, and velocity of the robot, return a trajectory to follow
* @param global_pose The current pose of the robot in world space
* @param global_vel The current velocity of the robot in world space
* @param drive_velocities Will be set to velocities to send to the robot base
* @return The selected path or trajectory
*/
Trajectory findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities,geometry_msgs::Twist &ackermann_state);
/**
* @brief Update the plan that the controller is following
* @param new_plan A new plan for the controller to follow
* @param compute_dists Wheter or not to compute path/goal distances when a plan is updated
*/
void updatePlan(const std::vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists = false);
/**
* @brief Accessor for the goal the robot is currently pursuing in world corrdinates
* @param x Will be set to the x position of the local goal
* @param y Will be set to the y position of the local goal
*/
void getLocalGoal(double& x, double& y);
/**
* @brief Generate and score a single trajectory
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @return True if the trajectory is legal, false otherwise
*/
bool checkTrajectory(double x, double y, double theta, double vx, double vy,
double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
/**
* @brief Generate and score a single trajectory
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @return The score (as double)
*/
double scoreTrajectory(double x, double y, double theta, double vx, double vy,
double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
/**
* @brief Compute the components and total cost for a map grid cell
* @param cx The x coordinate of the cell in the map grid
* @param cy The y coordinate of the cell in the map grid
* @param path_cost Will be set to the path distance component of the cost function
* @param goal_cost Will be set to the goal distance component of the cost function
* @param occ_cost Will be set to the costmap value of the cell
* @param total_cost Will be set to the value of the overall cost function, taking into account the scaling parameters
* @return True if the cell is traversible and therefore a legal location for the robot to move to
*/
bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost);
private:
/**
* @brief Create the trajectories we wish to explore, score them, and return the best option
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param acc_x The x acceleration limit of the robot
* @param acc_y The y acceleration limit of the robot
* @param acc_theta The theta acceleration limit of the robot
* @return
*/
Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta,
double acc_x, double acc_y, double acc_theta);
/**
* @brief Generate and score a single trajectory
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param vx The x velocity of the robot
* @param vy The y velocity of the robot
* @param vtheta The theta velocity of the robot
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @param acc_x The x acceleration limit of the robot
* @param acc_y The y acceleration limit of the robot
* @param acc_theta The theta acceleration limit of the robot
* @param impossible_cost The cost value of a cell in the local map grid that is considered impassable
* @param traj Will be set to the generated trajectory with its associated score
*/
void generateTrajectory(double x, double y, double theta, double vx, double vy,
double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y,
double acc_theta, double impossible_cost, Trajectory& traj);
/**
* @brief Checks the legality of the robot footprint at a position and orientation using the world model
* @param x_i The x position of the robot
* @param y_i The y position of the robot
* @param theta_i The orientation of the robot
* @return
*/
double footprintCost(double x_i, double y_i, double theta_i);
/**
* @brief Used to get the cells that make up the footprint of the robot
* @param x_i The x position of the robot
* @param y_i The y position of the robot
* @param theta_i The orientation of the robot
* @param fill If true: returns all cells in the footprint of the robot. If false: returns only the cells that make up the outline of the footprint.
* @return The cells that make up either the outline or entire footprint of the robot depending on fill
*/
std::vector<iri_ackermann_local_planner::Position2DInt> getFootprintCells(double x_i, double y_i, double theta_i, bool fill);
/**
* @brief Use Bresenham's algorithm to trace a line between two points in a grid
* @param x0 The x coordinate of the first point
* @param x1 The x coordinate of the second point
* @param y0 The y coordinate of the first point
* @param y1 The y coordinate of the second point
* @param pts Will be filled with the cells that lie on the line in the grid
*/
void getLineCells(int x0, int x1, int y0, int y1, std::vector<iri_ackermann_local_planner::Position2DInt>& pts);
/**
* @brief Fill the outline of a polygon, in this case the robot footprint, in a grid
* @param footprint The list of cells making up the footprint in the grid, will be modified to include all cells inside the footprint
*/
void getFillCells(std::vector<iri_ackermann_local_planner::Position2DInt>& footprint);
MapGrid map_; ///< @brief The local map grid where we propagate goal and path distance
const costmap_2d::Costmap2D& costmap_; ///< @brief Provides access to cost map information
WorldModel& world_model_; ///< @brief The world model that the controller uses for collision detection
std::vector<geometry_msgs::Point> footprint_spec_; ///< @brief The footprint specification of the robot
std::vector<geometry_msgs::PoseStamped> global_plan_; ///< @brief The global path for the robot to follow
double goal_x_,goal_y_; ///< @brief Storage for the local goal the robot is pursuing
double sim_time_; ///< @brief The number of seconds each trajectory is "rolled-out"
double sim_granularity_; ///< @brief The distance between simulation points
double angular_sim_granularity_; ///< @brief The distance between angular simulation points
int vx_samples_; ///< @brief The number of samples we'll take in the x dimenstion of the control space
int vtheta_samples_; ///< @brief The number of samples we'll take in the theta dimension of the control space
double pdist_scale_, gdist_scale_, occdist_scale_,hdiff_scale_; ///< @brief Scaling factors for the controller's cost function
Trajectory traj_one, traj_two; ///< @brief Used for scoring trajectories
/* ackerman parameters */
double steering_speed_;
/* ackerman reconfigure parameters */
double ack_acc_max_;
double ack_vel_min_;
double ack_vel_max_;
double ack_steer_acc_max_;
double ack_steer_speed_max_;
double ack_steer_speed_min_;
double ack_steer_angle_max_;
double ack_steer_angle_min_;
double ack_axis_distance_;
bool simple_attractor_; ///< @brief Enables simple attraction to a goal point
int heading_points_;
double xy_goal_tol_;
boost::mutex configuration_mutex_;
double lineCost(int x0, int x1, int y0, int y1);
double pointCost(int x, int y);
double headingDiff(int cell_x, int cell_y, double x, double y, double heading);
};
};
#endif
include/trajectory_planner_ros.h deleted 100755 → 0
+ 0
− 207
View file @ 4c068a33
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_publisher.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <world_model.h>
#include <point_grid.h>
#include <costmap_model.h>
#include <voxel_grid_model.h>
#include <trajectory_planner.h>
#include <map_grid_visualizer.h>
#include <planar_laser_scan.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <tf/transform_listener.h>
#include <boost/thread.hpp>
#include <string>
#include <angles/angles.h>
#include <nav_core/base_local_planner.h>
#include <dynamic_reconfigure/server.h>
#include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
namespace iri_ackermann_local_planner {
/**
* @class TrajectoryPlannerROS
* @brief A ROS wrapper for the trajectory controller that queries the param server to construct a controller
*/
class TrajectoryPlannerROS : public nav_core::BaseLocalPlanner {
public:
/**
* @brief Default constructor for the ros wrapper
*/
TrajectoryPlannerROS();
/**
* @brief Constructs the ros wrapper
* @param name The name to give this instance of the trajectory planner
* @param tf A pointer to a transform listener
* @param costmap The cost map to use for assigning costs to trajectories
*/
TrajectoryPlannerROS(std::string name, tf::TransformListener* tf,
costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Constructs the ros wrapper
* @param name The name to give this instance of the trajectory planner
* @param tf A pointer to a transform listener
* @param costmap The cost map to use for assigning costs to trajectories
*/
void initialize(std::string name, tf::TransformListener* tf,
costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Destructor for the wrapper
*/
~TrajectoryPlannerROS();
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
*/
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
/**
* @brief Set the plan that the controller is following
* @param orig_global_plan The plan to pass to the controller
* @return True if the plan was updated successfully, false otherwise
*/
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
/**
* @brief Check if the goal pose has been achieved
* @return True if achieved, false otherwise
*/
bool isGoalReached();
/**
* @brief Generate and score a single trajectory
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @param update_map Whether or not to update the map for the planner
* when computing the legality of the trajectory, this is useful to set
* to false if you're going to be doing a lot of trajectory checking over
* a short period of time
* @return True if the trajectory is legal, false otherwise
*/
bool checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);
/**
* @brief Generate and score a single trajectory
* @param vx_samp The x velocity used to seed the trajectory
* @param vy_samp The y velocity used to seed the trajectory
* @param vtheta_samp The theta velocity used to seed the trajectory
* @param update_map Whether or not to update the map for the planner
* when computing the legality of the trajectory, this is useful to set
* to false if you're going to be doing a lot of trajectory checking over
* a short period of time
* @return score of trajectory (double)
*/
double scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);
private:
/**
* @brief Callback for dynamic_reconfigure
*/
void reconfigureCB(AckermannLocalPlannerConfig &config, uint32_t level);
/**
* @brief Stop the robot taking into account acceleration limits
* @param global_pose The pose of the robot in the global frame
* @param robot_vel The velocity of the robot
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
double sign(double x){
return x < 0.0 ? -1.0 : 1.0;
}
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
costmap_2d::Costmap2DROS* costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will use
costmap_2d::Costmap2D costmap_; ///< @brief The costmap the controller will use
MapGridVisualizer map_viz_; ///< @brief The map grid visualizer for outputting the potential field generated by the cost function
tf::TransformListener* tf_; ///< @brief Used for transforming point clouds
std::string global_frame_; ///< @brief The frame in which the controller will run
double max_sensor_range_; ///< @brief Keep track of the effective maximum range of our sensors
nav_msgs::Odometry base_odom_; ///< @brief Used to get the velocity of the robot
std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
double rot_stopped_velocity_, trans_stopped_velocity_;
double xy_goal_tolerance_, yaw_goal_tolerance_;
double inflation_radius_;
std::vector<geometry_msgs::PoseStamped> global_plan_;
bool prune_plan_;
ros::Publisher g_plan_pub_, l_plan_pub_;
ros::Subscriber odom_sub_;
boost::recursive_mutex odom_lock_;
bool initialized_;
bool rotating_to_goal_;
bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
dynamic_reconfigure::Server<AckermannLocalPlannerConfig> *dsrv_;
iri_ackermann_local_planner::AckermannLocalPlannerConfig default_config_;
bool setup_;
std::vector < std::vector < geometry_msgs::PoseStamped > > subPathList;
std::vector < geometry_msgs::PoseStamped > subPath;
unsigned int subPathIndex;
geometry_msgs::Twist ackermann_state_;
};
};
#endif
include/voxel_grid_model.h deleted 100755 → 0
+ 0
− 176
View file @ 4c068a33
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
#define TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
#include <vector>
#include <list>
#include <cfloat>
#include <geometry_msgs/Point32.h>
#include <costmap_2d/observation.h>
#include <world_model.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
//voxel grid stuff
#include <voxel_grid/voxel_grid.h>
namespace iri_ackermann_local_planner {
/**
* @class VoxelGridModel
* @brief A class that implements the WorldModel interface to provide grid
* based collision checks for the trajectory controller using a 3D voxel grid.
*/
class VoxelGridModel : public WorldModel {
public:
/**
* @brief Constructor for the VoxelGridModel
* @param size_x The x size of the map
* @param size_y The y size of the map
* @param size_z The z size of the map... up to 32 cells
* @param xy_resolution The horizontal resolution of the map in meters/cell
* @param z_resolution The vertical resolution of the map in meters/cell
* @param origin_x The x value of the origin of the map
* @param origin_y The y value of the origin of the map
* @param origin_z The z value of the origin of the map
* @param max_z The maximum height for an obstacle to be added to the grid
* @param obstacle_range The maximum distance for obstacles to be added to the grid
*/
VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution,
double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range);
/**
* @brief Destructor for the world model
*/
virtual ~VoxelGridModel(){}
/**
* @brief Checks if any obstacles in the voxel grid lie inside a convex footprint that is rasterized into the grid
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
/**
* @brief The costmap already keeps track of world observations, so for this world model this method does nothing
* @param footprint The footprint of the robot in its current location
* @param observations The observations from various sensors
* @param laser_scan The scans used to clear freespace
*/
void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
private:
/**
* @brief Rasterizes a line in the costmap grid and checks for collisions
* @param x0 The x position of the first cell in grid coordinates
* @param y0 The y position of the first cell in grid coordinates
* @param x1 The x position of the second cell in grid coordinates
* @param y1 The y position of the second cell in grid coordinates
* @return A positive cost for a legal line... negative otherwise
*/
double lineCost(int x0, int x1, int y0, int y1);
/**
* @brief Checks the cost of a point in the costmap
* @param x The x position of the point in cell coordinates
* @param y The y position of the point in cell coordinates
* @return A positive cost for a legal point... negative otherwise
*/
double pointCost(int x, int y);
void removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range);
inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz){
if(wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
return false;
mx = (int) ((wx - origin_x_) / xy_resolution_);
my = (int) ((wy - origin_y_) / xy_resolution_);
mz = (int) ((wz - origin_z_) / z_resolution_);
return true;
}
inline bool worldToMap2D(double wx, double wy, unsigned int& mx, unsigned int& my){
if(wx < origin_x_ || wy < origin_y_)
return false;
mx = (int) ((wx - origin_x_) / xy_resolution_);
my = (int) ((wy - origin_y_) / xy_resolution_);
return true;
}
inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz){
//returns the center point of the cell
wx = origin_x_ + (mx + 0.5) * xy_resolution_;
wy = origin_y_ + (my + 0.5) * xy_resolution_;
wz = origin_z_ + (mz + 0.5) * z_resolution_;
}
inline void mapToWorld2D(unsigned int mx, unsigned int my, double& wx, double& wy){
//returns the center point of the cell
wx = origin_x_ + (mx + 0.5) * xy_resolution_;
wy = origin_y_ + (my + 0.5) * xy_resolution_;
}
inline double dist(double x0, double y0, double z0, double x1, double y1, double z1){
return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
}
inline void insert(pcl::PointXYZ pt){
unsigned int cell_x, cell_y, cell_z;
if(!worldToMap3D(pt.x, pt.y, pt.z, cell_x, cell_y, cell_z))
return;
obstacle_grid_.markVoxel(cell_x, cell_y, cell_z);
}
voxel_grid::VoxelGrid obstacle_grid_;
double xy_resolution_;
double z_resolution_;
double origin_x_;
double origin_y_;
double origin_z_;
double max_z_; ///< @brief The height cutoff for adding points as obstacles
double sq_obstacle_range_; ///< @brief The square distance at which we no longer add obstacles to the grid
};
};
#endif
include/world_model.h deleted 100755 → 0
+ 0
− 74
View file @ 4c068a33
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#define TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#include <vector>
#include <costmap_2d/observation.h>
#include <geometry_msgs/Point.h>
#include <planar_laser_scan.h>
namespace iri_ackermann_local_planner {
/**
* @class WorldModel
* @brief An interface the trajectory controller uses to interact with the
* world regardless of the underlying world model.
*/
class WorldModel{
public:
/**
* @brief Subclass will implement this method to check a footprint at a given position and orientation for legality in the world
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return Positive if all the points lie outside the footprint, negative otherwise
*/
virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius) = 0;
/**
* @brief Subclass will implement a destructor
*/
virtual ~WorldModel(){}
protected:
WorldModel(){}
};
};
#endif
manifest.xml deleted 100755 → 0
+ 0
− 35
View file @ 4c068a33
<package>
<description brief="iri_ackermann_local_planner">
iri_ackermann_local_planner
</description>
<author>Sergi Hernandez Juan</author>
<license>LGPL</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/iri_ackermann_local_planner</url>
<depend package="std_msgs" />
<depend package="nav_msgs" />
<depend package="rosconsole" />
<depend package="roscpp" />
<depend package="tf" />
<depend package="roslib" />
<depend package="rospy" />
<depend package="pluginlib" />
<depend package="costmap_2d" />
<depend package="voxel_grid" />
<depend package="angles" />
<depend package="visualization_msgs"/>
<depend package="geometry_msgs"/>
<depend package="nav_core"/>
<depend package="pcl"/>
<depend package="pcl_ros"/>
<depend package="common_rosdeps" />
<rosdep name="eigen" />
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/cfg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -liri_ackermann_local_planner" />
<nav_core plugin="${prefix}/ackermannlp_plugin.xml" />
</export>
</package>
msg/Position2DInt.msg deleted 100755 → 0
+ 0
− 2
View file @ 4c068a33
int64 x
int64 y
\ No newline at end of file
src/costmap_model.cpp deleted 100755 → 0
+ 0
− 197
View file @ 4c068a33
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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 the Willow Garage 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
*********************************************************************/
#include <costmap_model.h>
using namespace std;
using namespace costmap_2d;
namespace iri_ackermann_local_planner {
CostmapModel::CostmapModel(const Costmap2D& ma) : costmap_(ma) {}
double CostmapModel::footprintCost(const geometry_msgs::Point& position, const vector<geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius){
//used to put things into grid coordinates
unsigned int cell_x, cell_y;
//get the cell coord of the center point of the robot
if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
return -1.0;
//if number of points in the footprint is less than 3, we'll just assume a circular robot
if(footprint.size() < 3){
unsigned char cost = costmap_.getCost(cell_x, cell_y);
//if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION)
return -1.0;
return cost;
}
//now we really have to lay down the footprint in the costmap grid
unsigned int x0, x1, y0, y1;
double line_cost = 0.0;
double footprint_cost = 0.0;
//we need to rasterize each line in the footprint
for(unsigned int i = 0; i < footprint.size() - 1; ++i){
//get the cell coord of the first point
if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
return -1.0;
//get the cell coord of the second point
if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
return -1.0;
line_cost = lineCost(x0, x1, y0, y1);
footprint_cost = std::max(line_cost, footprint_cost);
//if there is an obstacle that hits the line... we know that we can return false right away
if(line_cost < 0)
return -1.0;
}
//we also need to connect the first point in the footprint to the last point
//get the cell coord of the last point
if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
return -1.0;
//get the cell coord of the first point
if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
return -1.0;
line_cost = lineCost(x0, x1, y0, y1);
footprint_cost = std::max(line_cost, footprint_cost);
if(line_cost < 0)
return -1.0;
//if all line costs are legal... then we can return that the footprint is legal
return footprint_cost;
}
//calculate the cost of a ray-traced line
double CostmapModel::lineCost(int x0, int x1,
int y0, int y1){
//Bresenham Ray-Tracing
int deltax = abs(x1 - x0); // The difference between the x's
int deltay = abs(y1 - y0); // The difference between the y's
int x = x0; // Start x off at the first pixel
int y = y0; // Start y off at the first pixel
int xinc1, xinc2, yinc1, yinc2;
int den, num, numadd, numpixels;
double line_cost = 0.0;
double point_cost = -1.0;
if (x1 >= x0) // The x-values are increasing
{
xinc1 = 1;
xinc2 = 1;
}
else // The x-values are decreasing
{
xinc1 = -1;
xinc2 = -1;
}
if (y1 >= y0) // The y-values are increasing
{
yinc1 = 1;
yinc2 = 1;
}
else // The y-values are decreasing
{
yinc1 = -1;
yinc2 = -1;
}
if (deltax >= deltay) // There is at least one x-value for every y-value
{
xinc1 = 0; // Don't change the x when numerator >= denominator
yinc2 = 0; // Don't change the y for every iteration
den = deltax;
num = deltax / 2;
numadd = deltay;
numpixels = deltax; // There are more x-values than y-values
}
else // There is at least one y-value for every x-value
{
xinc2 = 0; // Don't change the x for every iteration
yinc1 = 0; // Don't change the y when numerator >= denominator
den = deltay;
num = deltay / 2;
numadd = deltax;
numpixels = deltay; // There are more y-values than x-values
}
for (int curpixel = 0; curpixel <= numpixels; curpixel++)
{
point_cost = pointCost(x, y); //Score the current point
if(point_cost < 0)
return -1;
if(line_cost < point_cost)
line_cost = point_cost;
num += numadd; // Increase the numerator by the top of the fraction
if (num >= den) // Check if numerator >= denominator
{
num -= den; // Calculate the new numerator value
x += xinc1; // Change the x as appropriate
y += yinc1; // Change the y as appropriate
}
x += xinc2; // Change the x as appropriate
y += yinc2; // Change the y as appropriate
}
return line_cost;
}
double CostmapModel::pointCost(int x, int y){
unsigned char cost = costmap_.getCost(x, y);
//if the cell is in an obstacle the path is invalid
//if(cost == LETHAL_OBSTACLE){
if(cost == LETHAL_OBSTACLE || cost == NO_INFORMATION){
return -1;
}
return cost;
}
};
src/goal_functions.cpp deleted 100755 → 0
+ 0
− 249
View file @ 4c068a33
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, 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
*********************************************************************/
#include <goal_functions.h>
namespace iri_ackermann_local_planner {
double distance(double x1, double y1, double x2, double y2){
return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
}
bool goalPositionReached(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y, double xy_goal_tolerance){
double dist = distance(global_pose.getOrigin().x(), global_pose.getOrigin().y(), goal_x, goal_y);
return fabs(dist) <= xy_goal_tolerance;
}
bool goalOrientationReached(const tf::Stamped<tf::Pose>& global_pose, double goal_th, double yaw_goal_tolerance){
double yaw = tf::getYaw(global_pose.getRotation());
return fabs(angles::shortest_angular_distance(yaw, goal_th)) <= yaw_goal_tolerance;
}
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, double r, double g, double b, double a){
//given an empty path we won't do anything
if(path.empty())
return;
//create a path message
nav_msgs::Path gui_path;
gui_path.poses.resize(path.size());
gui_path.header.frame_id = path[0].header.frame_id;
gui_path.header.stamp = path[0].header.stamp;
// 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];
}
pub.publish(gui_path);
}
void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
ROS_ASSERT(global_plan.size() >= plan.size());
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
while(it != plan.end()){
const geometry_msgs::PoseStamped& w = *it;
// Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
double x_diff = global_pose.getOrigin().x() - w.pose.position.x;
double y_diff = global_pose.getOrigin().y() - w.pose.position.y;
double distance_sq = x_diff * x_diff + y_diff * y_diff;
if(distance_sq < 1){
ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.getOrigin().x(), global_pose.getOrigin().y(), w.pose.position.x, w.pose.position.y);
break;
}
it = plan.erase(it);
global_it = global_plan.erase(global_it);
}
}
bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
transformed_plan.clear();
try{
if (!global_plan.size() > 0)
{
ROS_ERROR("Recieved plan with zero length");
return false;
}
tf::StampedTransform transform;
tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, transform);
//let's get the pose of the robot in the frame of the plan
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = costmap.getBaseFrameID();
robot_pose.stamp_ = ros::Time();
tf.transformPose(plan_pose.header.frame_id, robot_pose, robot_pose);
//we'll keep points on the plan that are within the window that we're looking at
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = DBL_MAX;
//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size() && sq_dist > sq_dist_threshold){
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
++i;
}
//make sure not to count the first point that is too far away
if(i > 0)
--i;
tf::Stamped<tf::Pose> tf_pose;
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist < sq_dist_threshold){
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
const geometry_msgs::PoseStamped& pose = global_plan[i];
poseStampedMsgToTF(pose, tf_pose);
tf_pose.setData(transform * tf_pose);
tf_pose.stamp_ = transform.stamp_;
tf_pose.frame_id_ = global_frame;
poseStampedTFToMsg(tf_pose, newer_pose);
transformed_plan.push_back(newer_pose);
++i;
}
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
bool isGoalReached(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2DROS& costmap_ros, const std::string& global_frame,
const nav_msgs::Odometry& base_odom, double rot_stopped_vel, double trans_stopped_vel,
double xy_goal_tolerance, double yaw_goal_tolerance){
const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
tf::Stamped<tf::Pose> goal_pose;
try{
if (!global_plan.size() > 0)
{
ROS_ERROR("Recieved plan with zero length");
return false;
}
tf::StampedTransform transform;
tf.lookupTransform(global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, transform);
poseStampedMsgToTF(plan_goal_pose, goal_pose);
goal_pose.setData(transform * goal_pose);
goal_pose.stamp_ = transform.stamp_;
goal_pose.frame_id_ = global_frame;
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
//we assume the global goal is the last point in the global plan
double goal_x = goal_pose.getOrigin().getX();
double goal_y = goal_pose.getOrigin().getY();
double yaw = tf::getYaw(goal_pose.getRotation());
double goal_th = yaw;
tf::Stamped<tf::Pose> global_pose;
if(!costmap_ros.getRobotPose(global_pose))
return false;
//check to see if we've reached the goal position
if(goalPositionReached(global_pose, goal_x, goal_y, xy_goal_tolerance)){
//check to see if the goal orientation has been reached
if(goalOrientationReached(global_pose, goal_th, yaw_goal_tolerance)){
//make sure that we're actually stopped before returning success
if(stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
return true;
}
}
return false;
}
bool stopped(const nav_msgs::Odometry& base_odom,
const double& rot_stopped_velocity, const double& trans_stopped_velocity){
return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity
&& fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
&& fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
}
};
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