DuongTD/base_local_planner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
duongtd
2026-01-16 10:53:00 +07:00
commit 65edc7a386
50 changed files with 7845 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
include/robot_base_local_planner/Position2DInt.h Normal file
View File

@@ -0,0 +1,68 @@
#ifndef ROBOT_BASE_LOCAL_PLANNER_MESSAGE_POSITION2DINT_H
#define ROBOT_BASE_LOCAL_PLANNER_MESSAGE_POSITION2DINT_H
#include <string>
#include <vector>
#include <memory>
namespace robot_base_local_planner
{
template <class ContainerAllocator>
struct Position2DInt_
{
typedef Position2DInt_<ContainerAllocator> Type;
Position2DInt_()
: x(0)
, y(0) {
}
Position2DInt_(const ContainerAllocator& _alloc)
: x(0)
, y(0) {
(void)_alloc;
}
typedef int64_t _x_type;
_x_type x;
typedef int64_t _y_type;
_y_type y;
typedef boost::shared_ptr< ::robot_base_local_planner::Position2DInt_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::robot_base_local_planner::Position2DInt_<ContainerAllocator> const> ConstPtr;
}; // struct Position2DInt_
typedef ::robot_base_local_planner::Position2DInt_<std::allocator<void> > Position2DInt;
typedef boost::shared_ptr< ::robot_base_local_planner::Position2DInt > Position2DIntPtr;
typedef boost::shared_ptr< ::robot_base_local_planner::Position2DInt const> Position2DIntConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_base_local_planner::Position2DInt_<ContainerAllocator1> & lhs, const ::robot_base_local_planner::Position2DInt_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_base_local_planner::Position2DInt_<ContainerAllocator1> & lhs, const ::robot_base_local_planner::Position2DInt_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_base_local_planner
#endif // BASE_LOCAL_PLANNER_MESSAGE_POSITION2DINT_H

102
include/robot_base_local_planner/costmap_model.h Normal file
View File

@@ -0,0 +1,102 @@
/*********************************************************************
*
* 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 ROBOT_TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
#define ROBOT_TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
#include <robot_base_local_planner/world_model.h>
// For obstacle data access
#include <robot_costmap_2d/costmap_2d.h>
namespace robot_base_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 robot_costmap_2d::Costmap2D& costmap);
/**
* @brief Destructor for the world model
*/
virtual ~CostmapModel(){}
using WorldModel::footprintCost;
/**
* @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:
* -1 if footprint covers at least a lethal obstacle cell, or
* -2 if footprint covers at least a no-information cell, or
* -3 if footprint is [partially] outside of the map
*/
virtual double footprintCost(const robot_geometry_msgs::Point& position, const std::vector<robot_geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
/**
* @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) const;
/**
* @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;
private:
const robot_costmap_2d::Costmap2D& costmap_; ///< @brief Allows access of costmap obstacle information
};
}
#endif

87
include/robot_base_local_planner/footprint_helper.h Normal file
View File

@@ -0,0 +1,87 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ROBOT_FOOTPRINT_HELPER_H_
#define ROBOT_FOOTPRINT_HELPER_H_
#include <vector>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_geometry_msgs/Point.h>
#include <Eigen/Core>
#include <robot_base_local_planner/Position2DInt.h>
namespace robot_base_local_planner {
class FootprintHelper {
public:
FootprintHelper();
virtual ~FootprintHelper();
/**
* @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<robot_base_local_planner::Position2DInt> getFootprintCells(
Eigen::Vector3f pos,
std::vector<robot_geometry_msgs::Point> footprint_spec,
const robot_costmap_2d::Costmap2D&,
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<robot_base_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<robot_base_local_planner::Position2DInt>& footprint);
};
} /* namespace robot_base_local_planner */
#endif /* FOOTPRINT_HELPER_H_ */

154
include/robot_base_local_planner/goal_functions.h Normal file
View File

@@ -0,0 +1,154 @@
/*********************************************************************
*
* 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 ROBOT_BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
#define ROBOT_BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
#include <robot/robot.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
#include <robot_geometry_msgs/Point.h>
#include <tf3/buffer_core.h>
#include <string>
#include <cmath>
#include <robot_angles/angles.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <data_convert/data_convert.h>
namespace robot_base_local_planner {
/**
* @brief return squared distance to 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
* @return distance to goal
*/
double getGoalPositionDistance(const robot_geometry_msgs::PoseStamped& global_pose, double goal_x, double goal_y);
/**
* @brief return angle difference to goal to 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
* @return angular difference
*/
double getGoalOrientationAngleDifference(const robot_geometry_msgs::PoseStamped& global_pose, double goal_th);
/**
* @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<robot_geometry_msgs::PoseStamped>& path, const ros::Publisher& pub);
/**
* @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 robot_geometry_msgs::PoseStamped& global_pose, std::vector<robot_geometry_msgs::PoseStamped>& plan, std::vector<robot_geometry_msgs::PoseStamped>& global_plan);
/**
* @brief Transforms the global plan of the robot from the planner frame to the frame of the costmap,
* selects only the (first) part of the plan that is within the costmap area.
* @param tf A reference to a transform listener
* @param global_plan The plan to be transformed
* @param robot_pose The pose of the robot in the global frame (same as costmap)
* @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 tf3::BufferCore& tf,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan,
const robot_geometry_msgs::PoseStamped& global_robot_pose,
const robot_costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<robot_geometry_msgs::PoseStamped>& transformed_plan);
/**
* @brief Returns last pose in plan
* @param tf A reference to a transform listener
* @param global_plan The plan being followed
* @param global_frame The global frame of the local planner
* @param goal_pose the pose to copy into
* @return True if achieved, false otherwise
*/
bool getGoalPose(const tf3::BufferCore& tf,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame,
robot_geometry_msgs::PoseStamped &goal_pose);
/**
* @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_robot 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 tf3::BufferCore& tf,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan,
const robot_costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
robot_geometry_msgs::PoseStamped& global_pose,
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

143
include/robot_base_local_planner/line_iterator.h Normal file
View File

@@ -0,0 +1,143 @@
/*
* Copyright (c) 2012, 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, 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.
*/
#ifndef ROBOT_LINE_ITERATOR_H
#define ROBOT_LINE_ITERATOR_H
#include <stdlib.h>
namespace robot_base_local_planner
{
/** An iterator implementing Bresenham Ray-Tracing. */
class LineIterator
{
public:
LineIterator( int x0, int y0, int x1, int y1 )
: x0_( x0 )
, y0_( y0 )
, x1_( x1 )
, y1_( y1 )
, x_( x0 ) // X and Y start of at first endpoint.
, y_( y0 )
, deltax_( abs( x1 - x0 ))
, deltay_( abs( y1 - y0 ))
, curpixel_( 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
}
}
bool isValid() const
{
return curpixel_ <= numpixels_;
}
void advance()
{
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
curpixel_++;
}
int getX() const { return x_; }
int getY() const { return y_; }
int getX0() const { return x0_; }
int getY0() const { return y0_; }
int getX1() const { return x1_; }
int getY1() const { return y1_; }
private:
int x0_; ///< X coordinate of first end point.
int y0_; ///< Y coordinate of first end point.
int x1_; ///< X coordinate of second end point.
int y1_; ///< Y coordinate of second end point.
int x_; ///< X coordinate of current point.
int y_; ///< Y coordinate of current point.
int deltax_; ///< Difference between Xs of endpoints.
int deltay_; ///< Difference between Ys of endpoints.
int curpixel_; ///< index of current point in line loop.
int xinc1_, xinc2_, yinc1_, yinc2_;
int den_, num_, numadd_, numpixels_;
};
} // end namespace robot_base_local_planner
#endif // LINE_ITERATOR_H

121
include/robot_base_local_planner/local_planner_limits.h Normal file
View File

@@ -0,0 +1,121 @@
/***********************************************************
* 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 __base_local_planner__ROBOT_LOCALPLANNERLIMITS_H__
#define __base_local_planner__ROBOT_LOCALPLANNERLIMITS_H__
#include <Eigen/Core>
namespace robot_base_local_planner
{
class LocalPlannerLimits
{
public:
double max_vel_trans;
double min_vel_trans;
double max_vel_x;
double min_vel_x;
double max_vel_y;
double min_vel_y;
double max_vel_theta;
double min_vel_theta;
double acc_lim_x;
double acc_lim_y;
double acc_lim_theta;
double acc_lim_trans;
bool prune_plan;
double xy_goal_tolerance;
double yaw_goal_tolerance;
double trans_stopped_vel;
double theta_stopped_vel;
bool restore_defaults;
LocalPlannerLimits() {}
LocalPlannerLimits(
double nmax_vel_trans,
double nmin_vel_trans,
double nmax_vel_x,
double nmin_vel_x,
double nmax_vel_y,
double nmin_vel_y,
double nmax_vel_theta,
double nmin_vel_theta,
double nacc_lim_x,
double nacc_lim_y,
double nacc_lim_theta,
double nacc_lim_trans,
double nxy_goal_tolerance,
double nyaw_goal_tolerance,
bool nprune_plan = true,
double ntrans_stopped_vel = 0.1,
double ntheta_stopped_vel = 0.1):
max_vel_trans(nmax_vel_trans),
min_vel_trans(nmin_vel_trans),
max_vel_x(nmax_vel_x),
min_vel_x(nmin_vel_x),
max_vel_y(nmax_vel_y),
min_vel_y(nmin_vel_y),
max_vel_theta(nmax_vel_theta),
min_vel_theta(nmin_vel_theta),
acc_lim_x(nacc_lim_x),
acc_lim_y(nacc_lim_y),
acc_lim_theta(nacc_lim_theta),
acc_lim_trans(nacc_lim_trans),
prune_plan(nprune_plan),
xy_goal_tolerance(nxy_goal_tolerance),
yaw_goal_tolerance(nyaw_goal_tolerance),
trans_stopped_vel(ntrans_stopped_vel),
theta_stopped_vel(ntheta_stopped_vel) {}
~LocalPlannerLimits() {}
/**
* @brief Get the acceleration limits of the robot
* @return The acceleration limits of the robot
*/
Eigen::Vector3f getAccLimits() {
Eigen::Vector3f acc_limits;
acc_limits[0] = acc_lim_x;
acc_limits[1] = acc_lim_y;
acc_limits[2] = acc_lim_theta;
return acc_limits;
}
};
}
#endif // __LOCALPLANNERLIMITS_H__

67
include/robot_base_local_planner/map_cell.h Normal file
View File

@@ -0,0 +1,67 @@
/*********************************************************************
* 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 ROBOT_TRAJECTORY_ROLLOUT_MAP_CELL_H_
#define ROBOT_TRAJECTORY_ROLLOUT_MAP_CELL_H_
#include <robot_base_local_planner/trajectory_inc.h>
namespace robot_base_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 target_dist; ///< @brief Distance to planner's path
bool target_mark; ///< @brief Marks for computing path/goal distances
bool within_robot; ///< @brief Mark for cells within the robot footprint
};
}
#endif

200
include/robot_base_local_planner/map_grid.h Normal file
View File

@@ -0,0 +1,200 @@
/*********************************************************************
* 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 ROBOT_TRAJECTORY_ROLLOUT_MAP_GRID_H_
#define ROBOT_TRAJECTORY_ROLLOUT_MAP_GRID_H_
#include <vector>
#include <iostream>
#include <robot_base_local_planner/trajectory_inc.h>
#include <robot/console.h>
#include <robot_base_local_planner/map_cell.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_sensor_msgs/PointCloud2.h>
namespace robot_base_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 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
*/
void sizeCheck(unsigned int size_x, unsigned int size_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);
/**
* return a value that indicates cell is in obstacle
*/
inline double obstacleCosts() {
return map_.size();
}
/**
* returns a value indicating cell was not reached by wavefront
* propagation of set cells. (is behind walls, regarding the region covered by grid)
*/
inline double unreachableCellCosts() {
return map_.size() + 1;
}
/**
* @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 bool updatePathCell(MapCell* current_cell, MapCell* check_cell,
const robot_costmap_2d::Costmap2D& costmap);
/**
* increase global plan resolution to match that of the costmap by adding points linearly between global plan points
* This is necessary where global planners produce plans with few points.
* @param global_plan_in input
* @param global_plan_output output
* @param resolution desired distance between waypoints
*/
static void adjustPlanResolution(const std::vector<robot_geometry_msgs::PoseStamped>& global_plan_in,
std::vector<robot_geometry_msgs::PoseStamped>& global_plan_out, double resolution);
/**
* @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 computeTargetDistance(std::queue<MapCell*>& dist_queue, const robot_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 robot_costmap_2d::Costmap2D& costmap);
/**
* @brief Update what cells are considered path based on the global plan
*/
void setTargetCells(const robot_costmap_2d::Costmap2D& costmap, const std::vector<robot_geometry_msgs::PoseStamped>& global_plan);
/**
* @brief Update what cell is considered the next local goal
*/
void setLocalGoal(const robot_costmap_2d::Costmap2D& costmap,
const std::vector<robot_geometry_msgs::PoseStamped>& global_plan);
double goal_x_, goal_y_; /**< @brief The goal distance was last computed from */
unsigned int size_x_, size_y_; ///< @brief The dimensions of the grid
private:
std::vector<MapCell> map_; ///< @brief Storage for the MapCells
};
}
#endif

139
include/robot_base_local_planner/map_grid_cost_function.h Normal file
View File

@@ -0,0 +1,139 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ROBOT_MAP_GRID_COST_FUNCTION_H_
#define ROBOT_MAP_GRID_COST_FUNCTION_H_
#include <robot_base_local_planner/trajectory_cost_function.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_base_local_planner/map_grid.h>
namespace robot_base_local_planner {
/**
* when scoring a trajectory according to the values in mapgrid, we can take
*return the value of the last point (if no of the earlier points were in
* return collision), the sum for all points, or the product of all (non-zero) points
*/
enum CostAggregationType { Last, Sum, Product};
/**
* This class provides cost based on a map_grid of a small area of the world.
* The map_grid covers a the costmap, the costmap containing the information
* about sensed obstacles. The map_grid is used by setting
* certain cells to distance 0, and then propagating distances around them,
* filling up the area reachable around them.
*
* The approach using grid_maps is used for computational efficiency, allowing to
* score hundreds of trajectories very quickly.
*
* This can be used to favor trajectories which stay on a given path, or which
* approach a given goal.
* @param costmap_robot Reference to object giving updates of obstacles around robot
* @param xshift where the scoring point is with respect to robot center pose
* @param yshift where the scoring point is with respect to robot center pose
* @param is_local_goal_function, scores for local goal rather than whole path
* @param aggregationType how to combine costs along trajectory
*/
class MapGridCostFunction: public robot_base_local_planner::TrajectoryCostFunction {
public:
MapGridCostFunction(robot_costmap_2d::Costmap2D* costmap,
double xshift = 0.0,
double yshift = 0.0,
bool is_local_goal_function = false,
CostAggregationType aggregationType = Last);
~MapGridCostFunction() {}
/**
* set line segments on the grid with distance 0, resets the grid
*/
void setTargetPoses(std::vector<robot_geometry_msgs::PoseStamped> target_poses);
void setXShift(double xshift) {xshift_ = xshift;}
void setYShift(double yshift) {yshift_ = yshift;}
/** @brief If true, failures along the path cause the entire path to be rejected.
*
* Default is true. */
void setStopOnFailure(bool stop_on_failure) {stop_on_failure_ = stop_on_failure;}
/**
* propagate distances
*/
bool prepare();
double scoreTrajectory(Trajectory &traj);
/**
* return a value that indicates cell is in obstacle
*/
double obstacleCosts() {
return map_.obstacleCosts();
}
/**
* returns a value indicating cell was not reached by wavefront
* propagation of set cells. (is behind walls, regarding the region covered by grid)
*/
double unreachableCellCosts() {
return map_.unreachableCellCosts();
}
// used for easier debugging
double getCellCosts(unsigned int cx, unsigned int cy);
private:
std::vector<robot_geometry_msgs::PoseStamped> target_poses_;
robot_costmap_2d::Costmap2D* costmap_;
robot_base_local_planner::MapGrid map_;
CostAggregationType aggregationType_;
/// xshift and yshift allow scoring for different
// ooints of robots than center, like fron or back
// this can help with alignment or keeping specific
// wheels on tracks both default to 0
double xshift_;
double yshift_;
// if true, we look for a suitable local goal on path, else we use the full path for costs
bool is_local_goal_function_;
bool stop_on_failure_;
};
} /* namespace robot_base_local_planner */
#endif /* MAP_GRID_COST_FUNCTION_H_ */

71
include/robot_base_local_planner/map_grid_visualizer.h Normal file
View File

@@ -0,0 +1,71 @@
/*********************************************************************
* 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 ROBOT_MAP_GRID_VISUALIZER_H_
#define ROBOT_MAP_GRID_VISUALIZER_H_
#include <robot/robot.h>
#include <robot_base_local_planner/map_grid.h>
#include <robot_costmap_2d/costmap_2d.h>
namespace robot_base_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, std::string frame, boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function);
/**
* @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.
*/
robot_sensor_msgs::PointCloud2 publishCostCloud(const robot_costmap_2d::Costmap2D* costmap_p_);
private:
std::string name_; ///< @brief The name to get parameters relative to.
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
robot::NodeHandle ns_nh_;
std::string frame_id_;
// ros::Publisher pub_;
};
};
#endif

89
include/robot_base_local_planner/obstacle_cost_function.h Normal file
View File

@@ -0,0 +1,89 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ROBOT_OBSTACLE_COST_FUNCTION_H_
#define ROBOT_OBSTACLE_COST_FUNCTION_H_
#include <robot_base_local_planner/trajectory_cost_function.h>
#include <robot_base_local_planner/costmap_model.h>
#include <robot_costmap_2d/costmap_2d.h>
namespace robot_base_local_planner {
/**
* class ObstacleCostFunction
* @brief Uses costmap 2d to assign negative costs if robot footprint
* is in obstacle on any point of the trajectory.
*/
class ObstacleCostFunction : public TrajectoryCostFunction {
public:
ObstacleCostFunction(robot_costmap_2d::Costmap2D* costmap);
~ObstacleCostFunction();
bool prepare();
double scoreTrajectory(Trajectory &traj);
void setSumScores(bool score_sums){ sum_scores_=score_sums; }
void setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed);
void setFootprint(std::vector<robot_geometry_msgs::Point> footprint_spec);
// helper functions, made static for easy unit testing
static double getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor);
static double footprintCost(
const double& x,
const double& y,
const double& th,
double scale,
std::vector<robot_geometry_msgs::Point> footprint_spec,
robot_costmap_2d::Costmap2D* costmap,
robot_base_local_planner::WorldModel* world_model);
private:
robot_costmap_2d::Costmap2D* costmap_;
std::vector<robot_geometry_msgs::Point> footprint_spec_;
robot_base_local_planner::WorldModel* world_model_;
double max_trans_vel_;
bool sum_scores_;
//footprint scaling with velocity;
double max_scaling_factor_, scaling_speed_;
};
} /* namespace robot_base_local_planner */
#endif /* OBSTACLE_COST_FUNCTION_H_ */

89
include/robot_base_local_planner/oscillation_cost_function.h Normal file
View File

@@ -0,0 +1,89 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ROBOT_OSCILLATION_COST_FUNCTION_H_
#define ROBOT_OSCILLATION_COST_FUNCTION_H_
#include <robot_base_local_planner/trajectory_cost_function.h>
#include <Eigen/Core>
namespace robot_base_local_planner {
class OscillationCostFunction: public robot_base_local_planner::TrajectoryCostFunction {
public:
OscillationCostFunction();
virtual ~OscillationCostFunction();
double scoreTrajectory(Trajectory &traj);
bool prepare() {return true;};
/**
* @brief Reset the oscillation flags for the local planner
*/
void resetOscillationFlags();
void updateOscillationFlags(Eigen::Vector3f pos, robot_base_local_planner::Trajectory* traj, double min_vel_trans);
void setOscillationResetDist(double dist, double angle);
private:
void resetOscillationFlagsIfPossible(const Eigen::Vector3f& pos, const Eigen::Vector3f& prev);
/**
* @brief Given a trajectory that's selected, set flags if needed to
* prevent the robot from oscillating
* @param t The selected trajectory
* @return True if a flag was set, false otherwise
*/
bool setOscillationFlags(robot_base_local_planner::Trajectory* t, double min_vel_trans);
// flags
bool strafe_pos_only_, strafe_neg_only_, strafing_pos_, strafing_neg_;
bool rot_pos_only_, rot_neg_only_, rotating_pos_, rotating_neg_;
bool forward_pos_only_, forward_neg_only_, forward_pos_, forward_neg_;
// param
double oscillation_reset_dist_, oscillation_reset_angle_;
Eigen::Vector3f prev_stationary_pos_;
};
} /* namespace robot_base_local_planner */
#endif /* OSCILLATION_COST_FUNCTION_H_ */

57
include/robot_base_local_planner/planar_laser_scan.h Normal file
View File

@@ -0,0 +1,57 @@
/*********************************************************************
*
* 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 ROBOT_TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
#define ROBOT_TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
#include <robot_geometry_msgs/Point32.h>
#include <robot_sensor_msgs/PointCloud.h>
namespace robot_base_local_planner {
/**
* @class PlanarLaserScan
* @brief Stores a scan from a planar laser that can be used to clear freespace
*/
class PlanarLaserScan {
public:
PlanarLaserScan() {}
robot_geometry_msgs::Point32 origin;
robot_sensor_msgs::PointCloud cloud;
double angle_min, angle_max, angle_increment;
};
}
#endif

326
include/robot_base_local_planner/point_grid.h Normal file
View File

@@ -0,0 +1,326 @@
/*********************************************************************
*
* 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 ROBOT_POINT_GRID_H_
#define ROBOT_POINT_GRID_H_
#include <vector>
#include <list>
#include <cfloat>
#include <robot_geometry_msgs/Point.h>
#include <robot_costmap_2d/observation.h>
#include <robot_base_local_planner/world_model.h>
#include <robot_sensor_msgs/PointCloud2.h>
namespace robot_base_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, robot_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 robot_geometry_msgs::Point& lower_left, const robot_geometry_msgs::Point& upper_right, std::vector< std::list<robot_geometry_msgs::Point32>* >& 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 robot_geometry_msgs::Point& position, const std::vector<robot_geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
using WorldModel::footprintCost;
/**
* @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<robot_geometry_msgs::Point>& footprint,
const std::vector<robot_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(robot_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, robot_geometry_msgs::Point& lower_left, robot_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(const robot_geometry_msgs::Point32& pt1, const robot_geometry_msgs::Point32& 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(const robot_geometry_msgs::Point32& 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 robot_geometry_msgs::Point& a, const robot_geometry_msgs::Point& b, const robot_geometry_msgs::Point32& 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
*/
template<typename T>
inline double orient(const T& a, const T& b, const T& 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 robot_geometry_msgs::Point32& v1, const robot_geometry_msgs::Point32& v2,
const robot_geometry_msgs::Point32& u1, const robot_geometry_msgs::Point32& 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 robot_geometry_msgs::Point& v1, const robot_geometry_msgs::Point& v2,
const robot_geometry_msgs::Point& u1, const robot_geometry_msgs::Point& u2,
robot_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 robot_geometry_msgs::Point32& pt, const std::vector<robot_geometry_msgs::Point>& poly);
/**
* @brief Insert a point into the point grid
* @param pt The point to be inserted
*/
void insert(const robot_geometry_msgs::Point32& 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(const robot_geometry_msgs::Point32& 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(const robot_geometry_msgs::Point32& 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<robot_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 robot_geometry_msgs::Point32& 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(robot_sensor_msgs::PointCloud2& cloud);
private:
double resolution_; ///< @brief The resolution of the grid in meters/cell
robot_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<robot_geometry_msgs::Point32> > 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<robot_geometry_msgs::Point32>* > points_; ///< @brief The lists of points returned by a range search, made a member to save on memory allocation
};
}
#endif

64
include/robot_base_local_planner/prefer_forward_cost_function.h Normal file
View File

@@ -0,0 +1,64 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ROBOT_PREFER_FORWARD_COST_FUNCTION_H_
#define ROBOT_PREFER_FORWARD_COST_FUNCTION_H_
#include <robot_base_local_planner/trajectory_cost_function.h>
namespace robot_base_local_planner {
class PreferForwardCostFunction: public robot_base_local_planner::TrajectoryCostFunction {
public:
PreferForwardCostFunction(double penalty) : penalty_(penalty) {}
~PreferForwardCostFunction() {}
double scoreTrajectory(Trajectory &traj);
bool prepare() {return true;};
void setPenalty(double penalty) {
penalty_ = penalty;
}
private:
double penalty_;
};
} /* namespace robot_base_local_planner */
#endif /* PREFER_FORWARD_COST_FUNCTION_H_ */

109
include/robot_base_local_planner/simple_scored_sampling_planner.h Normal file
View File

@@ -0,0 +1,109 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ROBOT_SIMPLE_SCORED_SAMPLING_PLANNER_H_
#define ROBOT_SIMPLE_SCORED_SAMPLING_PLANNER_H_
#include <vector>
#include <robot_base_local_planner/trajectory.h>
#include <robot_base_local_planner/trajectory_cost_function.h>
#include <robot_base_local_planner/trajectory_sample_generator.h>
#include <robot_base_local_planner/trajectory_search.h>
namespace robot_base_local_planner {
/**
* @class SimpleScoredSamplingPlanner
* @brief Generates a local plan using the given generator and cost functions.
* Assumes less cost are best, and negative costs indicate infinite costs
*
* This is supposed to be a simple and robust implementation of
* the TrajectorySearch interface. More efficient search may well be
* possible using search heuristics, parallel search, etc.
*/
class SimpleScoredSamplingPlanner : public robot_base_local_planner::TrajectorySearch {
public:
~SimpleScoredSamplingPlanner() {}
SimpleScoredSamplingPlanner() {}
/**
* Takes a list of generators and critics. Critics return costs > 0, or negative costs for invalid trajectories.
* Generators other than the first are fallback generators, meaning they only get to generate if the previous
* generator did not find a valid trajectory.
* Will use every generator until it stops returning trajectories or count reaches max_samples.
* Then resets count and tries for the next in the list.
* passing max_samples = -1 (default): Each Sampling planner will continue to call
* generator until generator runs out of samples (or forever if that never happens)
*/
SimpleScoredSamplingPlanner(std::vector<TrajectorySampleGenerator*> gen_list, std::vector<TrajectoryCostFunction*>& critics, int max_samples = -1);
/**
* runs all scoring functions over the trajectory creating a weigthed sum
* of positive costs, aborting as soon as a negative cost are found or costs greater
* than positive best_traj_cost accumulated
*/
double scoreTrajectory(Trajectory& traj, double best_traj_cost);
/**
* Calls generator until generator has no more samples or max_samples is reached.
* For each generated traj, calls critics in turn. If any critic returns negative
* value, that value is assumed as costs, else the costs are the sum of all critics
* result. Returns true and sets the traj parameter to the first trajectory with
* minimal non-negative costs if sampling yields trajectories with non-negative costs,
* else returns false.
*
* @param traj The container to write the result to
* @param all_explored pass NULL or a container to collect all trajectories for debugging (has a penalty)
*/
bool findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored = 0);
private:
std::vector<TrajectorySampleGenerator*> gen_list_;
std::vector<TrajectoryCostFunction*> critics_;
int max_samples_;
};
} // namespace
#endif /* SIMPLE_SCORED_SAMPLING_PLANNER_H_ */

160
include/robot_base_local_planner/simple_trajectory_generator.h Normal file
View File

@@ -0,0 +1,160 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ROBOT_SIMPLE_TRAJECTORY_GENERATOR_H_
#define ROBOT_SIMPLE_TRAJECTORY_GENERATOR_H_
#include <robot_base_local_planner/trajectory_sample_generator.h>
#include <robot_base_local_planner/local_planner_limits.h>
#include <Eigen/Core>
namespace robot_base_local_planner {
/**
* generates trajectories based on equi-distant discretisation of the degrees of freedom.
* This is supposed to be a simple and robust implementation of the TrajectorySampleGenerator
* interface, more efficient implementations are thinkable.
*
* This can be used for both dwa and trajectory rollout approaches.
* As an example, assuming these values:
* sim_time = 1s, sim_period=200ms, dt = 200ms,
* vsamples_x=5,
* acc_limit_x = 1m/s^2, vel_x=0 (robot at rest, values just for easy calculations)
* dwa_planner will sample max-x-velocities from 0m/s to 0.2m/s.
* trajectory rollout approach will sample max-x-velocities 0m/s up to 1m/s
* trajectory rollout approach does so respecting the acceleration limit, so it gradually increases velocity
*/
class SimpleTrajectoryGenerator: public robot_base_local_planner::TrajectorySampleGenerator {
public:
SimpleTrajectoryGenerator() {
limits_ = NULL;
}
~SimpleTrajectoryGenerator() {}
/**
* @param pos current robot position
* @param vel current robot velocity
* @param limits Current velocity limits
* @param vsamples: in how many samples to divide the given dimension
* @param use_acceleration_limits: if true use physical model, else idealized robot model
* @param additional_samples (deprecated): Additional velocity samples to generate individual trajectories from.
* @param discretize_by_time if true, the trajectory is split according in chunks of the same duration, else of same length
*/
void initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
robot_base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
std::vector<Eigen::Vector3f> additional_samples,
bool discretize_by_time = false);
/**
* @param pos current robot position
* @param vel current robot velocity
* @param limits Current velocity limits
* @param vsamples: in how many samples to divide the given dimension
* @param use_acceleration_limits: if true use physical model, else idealized robot model
* @param discretize_by_time if true, the trajectory is split according in chunks of the same duration, else of same length
*/
void initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
robot_base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
bool discretize_by_time = false);
/**
* This function is to be called only when parameters change
*
* @param sim_granularity granularity of collision detection
* @param angular_sim_granularity angular granularity of collision detection
* @param use_dwa whether to use DWA or trajectory rollout
* @param sim_period distance between points in one trajectory
*/
void setParameters(double sim_time,
double sim_granularity,
double angular_sim_granularity,
bool use_dwa = false,
double sim_period = 0.0);
/**
* Whether this generator can create more trajectories
*/
bool hasMoreTrajectories();
/**
* Whether this generator can create more trajectories
*/
bool nextTrajectory(Trajectory &traj);
static Eigen::Vector3f computeNewPositions(const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel, double dt);
static Eigen::Vector3f computeNewVelocities(const Eigen::Vector3f& sample_target_vel,
const Eigen::Vector3f& vel, Eigen::Vector3f acclimits, double dt);
bool generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
robot_base_local_planner::Trajectory& traj);
protected:
unsigned int next_sample_index_;
// to store sample params of each sample between init and generation
std::vector<Eigen::Vector3f> sample_params_;
robot_base_local_planner::LocalPlannerLimits* limits_;
Eigen::Vector3f pos_;
Eigen::Vector3f vel_;
// whether velocity of trajectory changes over time or not
bool continued_acceleration_;
bool discretize_by_time_;
double sim_time_, sim_granularity_, angular_sim_granularity_;
bool use_dwa_;
double sim_period_; // only for dwa
};
} /* namespace robot_base_local_planner */
#endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */

118
include/robot_base_local_planner/trajectory.h Normal file
View File

@@ -0,0 +1,118 @@
/*********************************************************************
* 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 ROBOT_TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#define ROBOT_TRAJECTORY_ROLLOUT_TRAJECTORY_H_
#include <vector>
namespace robot_base_local_planner {
/**
* @class Trajectory
* @brief Holds a trajectory generated by considering 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, double time_delta, 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
double time_delta_; ///< @brief The time gap between points
/**
* @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) const;
/**
* @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) const;
/**
* @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() const;
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

86
include/robot_base_local_planner/trajectory_cost_function.h Normal file
View File

@@ -0,0 +1,86 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ROBOT_TRAJECTORYCOSTFUNCTION_H_
#define ROBOT_TRAJECTORYCOSTFUNCTION_H_
#include <robot_base_local_planner/trajectory.h>
namespace robot_base_local_planner {
/**
* @class TrajectoryCostFunction
* @brief Provides an interface for critics of trajectories
* During each sampling run, a batch of many trajectories will be scored using such a cost function.
* The prepare method is called before each batch run, and then for each
* trajectory of the sampling set, score_trajectory may be called.
*/
class TrajectoryCostFunction {
public:
/**
*
* General updating of context values if required.
* Subclasses may overwrite. Return false in case there is any error.
*/
virtual bool prepare() = 0;
/**
* return a score for trajectory traj
*/
virtual double scoreTrajectory(Trajectory &traj) = 0;
double getScale() {
return scale_;
}
void setScale(double scale) {
scale_ = scale;
}
virtual ~TrajectoryCostFunction() {}
protected:
TrajectoryCostFunction(double scale = 1.0): scale_(scale) {}
private:
double scale_;
};
}
#endif /* TRAJECTORYCOSTFUNCTION_H_ */

47
include/robot_base_local_planner/trajectory_inc.h Normal file
View File

@@ -0,0 +1,47 @@
/*********************************************************************
* 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 ROBOT_TRAJECTORY_INC_H_
#define ROBOT_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

385
include/robot_base_local_planner/trajectory_planner.h Normal file
View File

@@ -0,0 +1,385 @@
/*********************************************************************
*
* 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 ROBOT_TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
#define ROBOT_TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
#include <vector>
#include <cmath>
//for obstacle data access
#include <robot_costmap_2d/robot_costmap_2d.h>
#include <robot_costmap_2d/cost_values.h>
#include <robot_base_local_planner/footprint_helper.h>
#include <robot_base_local_planner/world_model.h>
#include <robot_base_local_planner/trajectory.h>
#include <robot_base_local_planner/Position2DInt.h>
#include <robot_base_local_planner/BaseLocalPlannerConfig.h>
//we'll take in a path as a vector of poses
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Point.h>
//for creating a local cost grid
#include <robot_base_local_planner/map_cell.h>
#include <robot_base_local_planner/map_grid.h>
namespace robot_base_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 path_distance_bias A scaling factor for how close the robot should stay to the path
* @param goal_distance_bias 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 meter_scoring adapt parameters to costmap resolution
* @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 robot_costmap_2d::Costmap2D& costmap,
std::vector<robot_geometry_msgs::Point> footprint_spec,
double acc_lim_x = 1.0, double acc_lim_y = 1.0, double acc_lim_theta = 1.0,
double sim_time = 1.0, double sim_granularity = 0.025,
int vx_samples = 20, int vtheta_samples = 20,
double path_distance_bias = 0.6, double goal_distance_bias = 0.8, double occdist_scale = 0.2,
double heading_lookahead = 0.325, double oscillation_reset_dist = 0.05,
double escape_reset_dist = 0.10, double escape_reset_theta = M_PI_2,
bool holonomic_robot = true,
double max_vel_x = 0.5, double min_vel_x = 0.1,
double max_vel_th = 1.0, double min_vel_th = -1.0, double min_in_place_vel_th = 0.4,
double backup_vel = -0.1,
bool dwa = false, bool heading_scoring = false, double heading_scoring_timestep = 0.1,
bool meter_scoring = true,
bool simple_attractor = false,
std::vector<double> y_vels = std::vector<double>(0),
double stop_time_buffer = 0.2,
double sim_period = 0.1, double angular_sim_granularity = 0.025);
/**
* @brief Destructs a trajectory controller
*/
~TrajectoryPlanner();
/**
* @brief Reconfigures the trajectory planner
*/
void reconfigure(BaseLocalPlannerConfig &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(const robot_geometry_msgs::PoseStamped& global_pose,
robot_geometry_msgs::PoseStamped& global_vel, robot_geometry_msgs::PoseStamped& drive_velocities);
/**
* @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<robot_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);
/** @brief Set the footprint specification of the robot. */
void setFootprint( std::vector<robot_geometry_msgs::Point> footprint ) { footprint_spec_ = footprint; }
/** @brief Return the footprint specification of the robot. */
robot_geometry_msgs::Polygon getFootprintPolygon() const { return robot_costmap_2d::toPolygon(footprint_spec_); }
std::vector<robot_geometry_msgs::Point> getFootprint() const { return footprint_spec_; }
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);
robot_base_local_planner::FootprintHelper footprint_helper_;
MapGrid path_map_; ///< @brief The local map grid where we propagate path distance
MapGrid goal_map_; ///< @brief The local map grid where we propagate goal distance
const robot_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<robot_geometry_msgs::Point> footprint_spec_; ///< @brief The footprint specification of the robot
std::vector<robot_geometry_msgs::PoseStamped> global_plan_; ///< @brief The global path for the robot to follow
bool stuck_left, stuck_right; ///< @brief Booleans to keep the robot from oscillating during rotation
bool rotating_left, rotating_right; ///< @brief Booleans to keep track of the direction of rotation for the robot
bool stuck_left_strafe, stuck_right_strafe; ///< @brief Booleans to keep the robot from oscillating during strafing
bool strafe_right, strafe_left; ///< @brief Booleans to keep track of strafe direction for the robot
bool escaping_; ///< @brief Boolean to keep track of whether we're in escape mode
bool meter_scoring_;
double goal_x_,goal_y_; ///< @brief Storage for the local goal the robot is pursuing
double final_goal_x_, final_goal_y_; ///< @brief The end position of the plan.
bool final_goal_position_valid_; ///< @brief True if final_goal_x_ and final_goal_y_ have valid data. Only false if an empty path is sent.
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 path_distance_bias_, goal_distance_bias_, occdist_scale_; ///< @brief Scaling factors for the controller's cost function
double acc_lim_x_, acc_lim_y_, acc_lim_theta_; ///< @brief The acceleration limits of the robot
double prev_x_, prev_y_; ///< @brief Used to calculate the distance the robot has traveled before reseting oscillation booleans
double escape_x_, escape_y_, escape_theta_; ///< @brief Used to calculate the distance the robot has traveled before reseting escape booleans
Trajectory traj_one, traj_two; ///< @brief Used for scoring trajectories
double heading_lookahead_; ///< @brief How far the robot should look ahead of itself when differentiating between different rotational velocities
double oscillation_reset_dist_; ///< @brief The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
double escape_reset_dist_, escape_reset_theta_; ///< @brief The distance the robot must travel before it can leave escape mode
bool holonomic_robot_; ///< @brief Is the robot holonomic or not?
double max_vel_x_, min_vel_x_, max_vel_th_, min_vel_th_, min_in_place_vel_th_; ///< @brief Velocity limits for the controller
double backup_vel_; ///< @brief The velocity to use while backing up
bool dwa_; ///< @brief Should we use the dynamic window approach?
bool heading_scoring_; ///< @brief Should we score based on the rollout approach or the heading approach
double heading_scoring_timestep_; ///< @brief How far to look ahead in time when we score a heading
bool simple_attractor_; ///< @brief Enables simple attraction to a goal point
std::vector<double> y_vels_; ///< @brief Y velocities to explore
double stop_time_buffer_; ///< @brief How long before hitting something we're going to enforce that the robot stop
double sim_period_; ///< @brief The number of seconds to use to compute max/min vels for dwa
double inscribed_radius_, circumscribed_radius_;
boost::mutex configuration_mutex_;
/**
* @brief Compute x position based on velocity
* @param xi The current x position
* @param vx The current x velocity
* @param vy The current y velocity
* @param theta The current orientation
* @param dt The timestep to take
* @return The new x position
*/
inline double computeNewXPosition(double xi, double vx, double vy, double theta, double dt){
return xi + (vx * cos(theta) + vy * cos(M_PI_2 + theta)) * dt;
}
/**
* @brief Compute y position based on velocity
* @param yi The current y position
* @param vx The current x velocity
* @param vy The current y velocity
* @param theta The current orientation
* @param dt The timestep to take
* @return The new y position
*/
inline double computeNewYPosition(double yi, double vx, double vy, double theta, double dt){
return yi + (vx * sin(theta) + vy * sin(M_PI_2 + theta)) * dt;
}
/**
* @brief Compute orientation based on velocity
* @param thetai The current orientation
* @param vth The current theta velocity
* @param dt The timestep to take
* @return The new orientation
*/
inline double computeNewThetaPosition(double thetai, double vth, double dt){
return thetai + vth * dt;
}
//compute velocity based on acceleration
/**
* @brief Compute velocity based on acceleration
* @param vg The desired velocity, what we're accelerating up to
* @param vi The current velocity
* @param a_max An acceleration limit
* @param dt The timestep to take
* @return The new velocity
*/
inline double computeNewVelocity(double vg, double vi, double a_max, double dt){
if((vg - vi) >= 0) {
return std::min(vg, vi + a_max * dt);
}
return std::max(vg, vi - a_max * dt);
}
void getMaxSpeedToStopInTime(double time, double& vx, double& vy, double& vth){
vx = acc_lim_x_ * std::max(time, 0.0);
vy = acc_lim_y_ * std::max(time, 0.0);
vth = acc_lim_theta_ * std::max(time, 0.0);
}
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

74
include/robot_base_local_planner/trajectory_sample_generator.h Normal file
View File

@@ -0,0 +1,74 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ROBOT_TRAJECTORY_SAMPLE_GENERATOR_H_
#define ROBOT_TRAJECTORY_SAMPLE_GENERATOR_H_
#include <robot_base_local_planner/trajectory.h>
namespace robot_base_local_planner {
/**
* @class TrajectorySampleGenerator
* @brief Provides an interface for navigation trajectory generators
*/
class TrajectorySampleGenerator {
public:
/**
* Whether this generator can create more trajectories
*/
virtual bool hasMoreTrajectories() = 0;
/**
* Whether this generator can create more trajectories
*/
virtual bool nextTrajectory(Trajectory &traj) = 0;
/**
* @brief Virtual destructor for the interface
*/
virtual ~TrajectorySampleGenerator() {}
protected:
TrajectorySampleGenerator() {}
};
} // end namespace
#endif /* TRAJECTORY_SAMPLE_GENERATOR_H_ */

71
include/robot_base_local_planner/trajectory_search.h Normal file
View File

@@ -0,0 +1,71 @@
/*********************************************************************
*
* 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 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: TKruse
*********************************************************************/
#ifndef ROBOT_TRAJECTORY_SEARCH_H_
#define ROBOT_TRAJECTORY_SEARCH_H_
#include <robot_base_local_planner/trajectory.h>
namespace robot_base_local_planner {
/**
* @class TrajectorySearch
* @brief Interface for modules finding a trajectory to use for navigation commands next
*/
class TrajectorySearch {
public:
/**
* searches the space of allowed trajectory and
* returns one considered the optimal given the
* constraints of the particular search.
*
* @param traj The container to write the result to
* @param all_explored pass NULL or a container to collect all trajectories for debugging (has a penalty)
*/
virtual bool findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) = 0;
virtual ~TrajectorySearch() {}
protected:
TrajectorySearch() {}
};
}
#endif /* TRAJECTORY_SEARCH_H_ */

64
include/robot_base_local_planner/twirling_cost_function.h Normal file
View File

@@ -0,0 +1,64 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Open Source Robotics Foundation, 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: Morgan Quigley
*********************************************************************/
#ifndef ROBOT_TWIRLING_COST_FUNCTION_H
#define ROBOT_TWIRLING_COST_FUNCTION_H
#include <robot_base_local_planner/trajectory_cost_function.h>
namespace robot_base_local_planner {
/**
* This class provides a cost based on how much a robot "twirls" on its
* way to the goal. With differential-drive robots, there isn't a choice,
* but with holonomic or near-holonomic robots, sometimes a robot spins
* more than you'd like on its way to a goal. This class provides a way
* to assign a penalty purely to rotational velocities.
*/
class TwirlingCostFunction: public robot_base_local_planner::TrajectoryCostFunction {
public:
TwirlingCostFunction() {}
~TwirlingCostFunction() {}
double scoreTrajectory(Trajectory &traj);
bool prepare() {return true;};
};
} /* namespace robot_base_local_planner */
#endif /* TWIRLING_COST_FUNCTION_H_ */

99
include/robot_base_local_planner/velocity_iterator.h Normal file
View File

@@ -0,0 +1,99 @@
/*********************************************************************
*
* 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 ROBOT_DWA_LOCAL_PLANNER_VELOCITY_ITERATOR_H_
#define ROBOT_DWA_LOCAL_PLANNER_VELOCITY_ITERATOR_H_
#include <algorithm>
#include <cmath>
#include <vector>
namespace robot_base_local_planner {
/**
* We use the class to get even sized samples between min and max, inluding zero if it is not included (and range goes from negative to positive
*/
class VelocityIterator {
public:
VelocityIterator(double min, double max, int num_samples):
current_index(0)
{
if (min == max) {
samples_.push_back(min);
} else {
num_samples = std::max(2, num_samples);
// e.g. for 4 samples, split distance in 3 even parts
double step_size = (max - min) / double(std::max(1, (num_samples - 1)));
// we make sure to avoid rounding errors around min and max.
double current;
double next = min;
for (int j = 0; j < num_samples - 1; ++j) {
current = next;
next += step_size;
samples_.push_back(current);
// if 0 is among samples, this is never true. Else it inserts a 0 between the positive and negative samples
if ((current < 0) && (next > 0)) {
samples_.push_back(0.0);
}
}
samples_.push_back(max);
}
}
double getVelocity(){
return samples_.at(current_index);
}
VelocityIterator& operator++(int){
current_index++;
return *this;
}
void reset(){
current_index = 0;
}
bool isFinished(){
return current_index >= samples_.size();
}
private:
std::vector<double> samples_;
unsigned int current_index;
};
}
#endif

179
include/robot_base_local_planner/voxel_grid_model.h Normal file
View File

@@ -0,0 +1,179 @@
/*********************************************************************
*
* 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 ROBOT_TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
#define ROBOT_TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
#include <vector>
#include <list>
#include <cfloat>
#include <robot_geometry_msgs/Point.h>
#include <robot_costmap_2d/observation.h>
#include <robot_base_local_planner/world_model.h>
//voxel grid stuff
#include <robot_voxel_grid/voxel_grid.h>
namespace robot_base_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 robot_geometry_msgs::Point& position, const std::vector<robot_geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
using WorldModel::footprintCost;
/**
* @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<robot_geometry_msgs::Point>& footprint,
const std::vector<robot_costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
/**
* @brief Function copying the Voxel points into a point cloud
* @param cloud the point cloud to copy data to. It has the usual x,y,z channels
*/
void getPoints(robot_sensor_msgs::PointCloud2& 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(const robot_geometry_msgs::Point32& 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);
}
robot_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

114
include/robot_base_local_planner/world_model.h Normal file
View File

@@ -0,0 +1,114 @@
/*********************************************************************
*
* 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 ROBOT_TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#define ROBOT_TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#include <vector>
#include <robot_costmap_2d/observation.h>
#include <robot_costmap_2d/footprint.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_base_local_planner/planar_laser_scan.h>
namespace robot_base_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:
* -1 if footprint covers at least a lethal obstacle cell, or
* -2 if footprint covers at least a no-information cell, or
* -3 if footprint is partially or totally outside of the map
*/
virtual double footprintCost(const robot_geometry_msgs::Point& position, const std::vector<robot_geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius) = 0;
double footprintCost(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec, double inscribed_radius = 0.0, double circumscribed_radius=0.0){
double cos_th = cos(theta);
double sin_th = sin(theta);
std::vector<robot_geometry_msgs::Point> oriented_footprint;
for(unsigned int i = 0; i < footprint_spec.size(); ++i){
robot_geometry_msgs::Point new_pt;
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
oriented_footprint.push_back(new_pt);
}
robot_geometry_msgs::Point robot_position;
robot_position.x = x;
robot_position.y = y;
if(inscribed_radius==0.0){
robot_costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius, circumscribed_radius);
}
return footprintCost(robot_position, oriented_footprint, inscribed_radius, circumscribed_radius);
}
/**
* @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
*/
double footprintCost(const robot_geometry_msgs::Point& position, const std::vector<robot_geometry_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius, double extra) {
return footprintCost(position, footprint, inscribed_radius, circumscribed_radius);
}
/**
* @brief Subclass will implement a destructor
*/
virtual ~WorldModel(){}
protected:
WorldModel(){}
};
}
#endif