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

Hiep update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 10:24:18 +07:00
parent 4246453ae6
commit 72b2f3c639
49 changed files with 476 additions and 476 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
include/robot_costmap_2d/array_parser.h Executable file
View File

@@ -0,0 +1,51 @@
/*
* 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.
*
* author: Dave Hershberger
*/
#ifndef ROBOT_COSTMAP_2D_ARRAY_PARSER_H
#define ROBOT_COSTMAP_2D_ARRAY_PARSER_H
#include <vector>
#include <string>
namespace robot_costmap_2d
{
/** @brief Parse a vector of vectors of floats from a string.
* @param error_return If no error, error_return is set to "". If
* error, error_return will describe the error.
* Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...]
*
* On error, error_return is set and the return value could be
* anything, like part of a successful parse. */
std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return);
} // end namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_ARRAY_PARSER_H

50
include/robot_costmap_2d/cost_values.h Executable file
View File

@@ -0,0 +1,50 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_COST_VALUES_H_
#define ROBOT_COSTMAP_2D_COST_VALUES_H_
/** Provides a mapping for often used cost values */
namespace robot_costmap_2d
{
static const unsigned char NO_INFORMATION = 255;
static const unsigned char LETHAL_OBSTACLE = 254;
static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
static const unsigned char CRITICAL_SPACE = 0;
static const unsigned char FREE_SPACE = 60;
static const unsigned char PREFERRED_SPACE = 20;
static const unsigned char UNPREFERRED_SPACE = 100;
}
#endif // ROBOT_COSTMAP_2D_COST_VALUES_H_

469
include/robot_costmap_2d/costmap_2d.h Executable file
View File

@@ -0,0 +1,469 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
#include <vector>
#include <queue>
#include <robot_geometry_msgs/Point.h>
#include <boost/thread.hpp>
namespace robot_costmap_2d
{
// convenient for storing x/y point pairs
struct MapLocation
{
unsigned int x;
unsigned int y;
};
/**
* @class Costmap2D
* @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
*/
class Costmap2D
{
friend class CostmapTester; // Need this for gtest to work correctly
public:
/**
* @brief Constructor for a costmap
* @param cells_size_x The x size of the map in cells
* @param cells_size_y The y size of the map in cells
* @param resolution The resolution of the map in meters/cell
* @param origin_x The x origin of the map
* @param origin_y The y origin of the map
* @param default_value Default Value
*/
Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value = 0);
/**
* @brief Copy constructor for a costmap, creates a copy efficiently
* @param map The costmap to copy
*/
Costmap2D(const Costmap2D& map);
/**
* @brief Overloaded assignment operator
* @param map The costmap to copy
* @return A reference to the map after the copy has finished
*/
Costmap2D& operator=(const Costmap2D& map);
/**
* @brief Turn this costmap into a copy of a window of a costmap passed in
* @param map The costmap to copy
* @param win_origin_x The x origin (lower left corner) for the window to copy, in meters
* @param win_origin_y The y origin (lower left corner) for the window to copy, in meters
* @param win_size_x The x size of the window, in meters
* @param win_size_y The y size of the window, in meters
*/
bool copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x,
double win_size_y);
/**
* @brief Default constructor
*/
Costmap2D();
/**
* @brief Destructor
*/
virtual ~Costmap2D();
/**
* @brief Get the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @return The cost of the cell
*/
unsigned char getCost(unsigned int mx, unsigned int my) const;
/**
* @brief Set the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @param cost The cost to set the cell to
*/
void setCost(unsigned int mx, unsigned int my, unsigned char cost);
/**
* @brief Convert from map coordinates to world coordinates
* @param mx The x map coordinate
* @param my The y map coordinate
* @param wx Will be set to the associated world x coordinate
* @param wy Will be set to the associated world y coordinate
*/
void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
/**
* @brief Convert from world coordinates to map coordinates
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @return True if the conversion was successful (legal bounds) false otherwise
*/
bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
/**
* @brief Convert from world coordinates to map coordinates without checking for legal bounds
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @note The returned map coordinates <b>are not guaranteed to lie within the map.</b>
*/
void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;
/**
* @brief Convert from world coordinates to map coordinates, constraining results to legal bounds.
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @note The returned map coordinates are guaranteed to lie within the map.
*/
void worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const;
/**
* @brief Given two map coordinates... compute the associated index
* @param mx The x coordinate
* @param my The y coordinate
* @return The associated index
*/
inline unsigned int getIndex(unsigned int mx, unsigned int my) const
{
return my * size_x_ + mx;
}
/**
* @brief Given an index... compute the associated map coordinates
* @param index The index
* @param mx Will be set to the x coordinate
* @param my Will be set to the y coordinate
*/
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
{
my = index / size_x_;
mx = index - (my * size_x_);
}
/**
* @brief Will return a pointer to the underlying unsigned char array used as the costmap
* @return A pointer to the underlying unsigned char array storing cost values
*/
unsigned char* getCharMap() const;
/**
* @brief Accessor for the x size of the costmap in cells
* @return The x size of the costmap
*/
unsigned int getSizeInCellsX() const;
/**
* @brief Accessor for the y size of the costmap in cells
* @return The y size of the costmap
*/
unsigned int getSizeInCellsY() const;
/**
* @brief Accessor for the x size of the costmap in meters
* @return The x size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
double getSizeInMetersX() const;
/**
* @brief Accessor for the y size of the costmap in meters
* @return The y size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
double getSizeInMetersY() const;
/**
* @brief Accessor for the x origin of the costmap
* @return The x origin of the costmap
*/
double getOriginX() const;
/**
* @brief Accessor for the y origin of the costmap
* @return The y origin of the costmap
*/
double getOriginY() const;
/**
* @brief Accessor for the resolution of the costmap
* @return The resolution of the costmap
*/
double getResolution() const;
void setDefaultValue(unsigned char c)
{
default_value_ = c;
}
unsigned char getDefaultValue()
{
return default_value_;
}
/**
* @brief Sets the cost of a convex polygon to a desired value
* @param polygon The polygon to perform the operation on
* @param cost_value The value to set costs to
* @return True if the polygon was filled... false if it could not be filled
*/
bool setConvexPolygonCost(const std::vector<robot_geometry_msgs::Point>& polygon, unsigned char cost_value);
/**
* @brief Get the map cells that make up the outline of a polygon
* @param polygon The polygon in map coordinates to rasterize
* @param polygon_cells Will be set to the cells contained in the outline of the polygon
*/
void polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
/**
* @brief Get the map cells that fill a convex polygon
* @param polygon The polygon in map coordinates to rasterize
* @param polygon_cells Will be set to the cells that fill the polygon
*/
void convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
/**
* @brief Move the origin of the costmap to a new location.... keeping data when it can
* @param new_origin_x The x coordinate of the new origin
* @param new_origin_y The y coordinate of the new origin
*/
virtual void updateOrigin(double new_origin_x, double new_origin_y);
/**
* @brief Save the costmap out to a pgm file
* @param file_name The name of the file to save
*/
bool saveMap(std::string file_name);
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
double origin_y);
void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn);
/**
* @brief Given distance in the world... convert it to cells
* @param world_dist The world distance
* @return The equivalent cell distance
*/
unsigned int cellDistance(double world_dist);
// Provide a typedef to ease future code maintenance
typedef boost::recursive_mutex mutex_t;
mutex_t* getMutex()
{
return access_;
}
protected:
/**
* @brief Copy a region of a source map into a destination map
* @param source_map The source map
* @param sm_lower_left_x The lower left x point of the source map to start the copy
* @param sm_lower_left_y The lower left y point of the source map to start the copy
* @param sm_size_x The x size of the source map
* @param dest_map The destination map
* @param dm_lower_left_x The lower left x point of the destination map to start the copy
* @param dm_lower_left_y The lower left y point of the destination map to start the copy
* @param dm_size_x The x size of the destination map
* @param region_size_x The x size of the region to copy
* @param region_size_y The y size of the region to copy
*/
template<typename data_type>
void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x,
unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
unsigned int region_size_y)
{
// we'll first need to compute the starting points for each map
data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
// now, we'll copy the source map into the destination map
for (unsigned int i = 0; i < region_size_y; ++i)
{
memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
sm_index += sm_size_x;
dm_index += dm_size_x;
}
}
/**
* @brief Deletes the costmap, static_map, and markers data structures
*/
virtual void deleteMaps();
/**
* @brief Resets the costmap and static_map to be unknown space
*/
virtual void resetMaps();
/**
* @brief Initializes the costmap, static_map, and markers data structures
* @param size_x The x size to use for map initialization
* @param size_y The y size to use for map initialization
*/
virtual void initMaps(unsigned int size_x, unsigned int size_y);
/**
* @brief Raytrace a line and apply some action at each step
* @param at The action to take... a functor
* @param x0 The starting x coordinate
* @param y0 The starting y coordinate
* @param x1 The ending x coordinate
* @param y1 The ending y coordinate
* @param max_length The maximum desired length of the segment... allows you to not go all the way to the endpoint
*/
template<class ActionType>
inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
unsigned int max_length = UINT_MAX)
{
int dx = x1 - x0;
int dy = y1 - y0;
unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy);
int offset_dx = sign(dx);
int offset_dy = sign(dy) * size_x_;
unsigned int offset = y0 * size_x_ + x0;
// we need to chose how much to scale our dominant dimension, based on the maximum length of the line
double dist = hypot(dx, dy);
double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
// if x is dominant
if (abs_dx >= abs_dy)
{
int error_y = abs_dx / 2;
bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
return;
}
// otherwise y is dominant
int error_x = abs_dy / 2;
bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
}
private:
/**
* @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step
*/
template<class ActionType>
inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,
int offset_b, unsigned int offset, unsigned int max_length)
{
unsigned int end = std::min(max_length, abs_da);
for (unsigned int i = 0; i < end; ++i)
{
at(offset);
offset += offset_a;
error_b += abs_db;
if ((unsigned int)error_b >= abs_da)
{
offset += offset_b;
error_b -= abs_da;
}
}
at(offset);
}
inline int sign(int x)
{
return x > 0 ? 1.0 : -1.0;
}
mutex_t* access_;
protected:
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
double origin_x_;
double origin_y_;
unsigned char* costmap_;
unsigned char default_value_;
class MarkCell
{
public:
MarkCell(unsigned char* costmap, unsigned char value) :
costmap_(costmap), value_(value)
{
}
inline void operator()(unsigned int offset)
{
costmap_[offset] = value_;
}
private:
unsigned char* costmap_;
unsigned char value_;
};
class PolygonOutlineCells
{
public:
PolygonOutlineCells(const Costmap2D& costmap, const unsigned char* char_map, std::vector<MapLocation>& cells) :
costmap_(costmap), char_map_(char_map), cells_(cells)
{
}
// just push the relevant cells back onto the list
inline void operator()(unsigned int offset)
{
MapLocation loc;
costmap_.indexToCells(offset, loc.x, loc.y);
cells_.push_back(loc);
}
private:
const Costmap2D& costmap_;
const unsigned char* char_map_;
std::vector<MapLocation>& cells_;
};
};
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H

280
include/robot_costmap_2d/costmap_2d_robot.h Executable file
View File

@@ -0,0 +1,280 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/footprint.h>
#include <robot_geometry_msgs/Polygon.h>
#include <robot_geometry_msgs/PolygonStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <tf3/LinearMath/Transform.h>
#include <robot/rate.h>
#include <data_convert/data_convert.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
#include <robot/node_handle.h>
#include <robot/plugin_loader_helper.h>
class SuperValue : public robot::XmlRpc::XmlRpcValue
{
public:
void setStruct(robot::XmlRpc::XmlRpcValue::ValueStruct* a)
{
_type = TypeStruct;
_value.asStruct = new robot::XmlRpc::XmlRpcValue::ValueStruct(*a);
}
void setArray(robot::XmlRpc::XmlRpcValue::ValueArray* a)
{
_type = TypeArray;
_value.asArray = new std::vector<robot::XmlRpc::XmlRpcValue>(*a);
}
};
namespace robot_costmap_2d
{
/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
* topics that provide observations about obstacles in either the form
* of PointCloud or LaserScan messages. */
class Costmap2DROBOT
{
public:
/**
* @brief Constructor for the wrapper
* @param name The name for this costmap
* @param tf A reference to a TransformListener
*/
Costmap2DROBOT(const std::string &name, tf3::BufferCore& tf);
~Costmap2DROBOT();
/**
* @brief Subscribes to sensor topics if necessary and starts costmap
* updates, can be called to restart the costmap after calls to either
* stop() or pause()
*/
void start();
/**
* @brief Stops costmap updates and unsubscribes from sensor topics
*/
void stop();
/**
* @brief Stops the costmap from updating, but sensor data still comes in over the wire
*/
void pause();
/**
* @brief Resumes costmap updates
*/
void resume();
void updateMap();
/**
* @brief Reset each individual layer
*/
void resetLayers();
/** @brief Same as getLayeredCostmap()->isCurrent(). */
bool isCurrent() const
{
return layered_costmap_->isCurrent();
}
/**
* @brief Is the costmap stopped
*/
bool isStopped() const
{
return stopped_;
}
/**
* @brief Get the pose of the robot in the global frame of the costmap
* @param global_pose Will be set to the pose of the robot in the global frame of the costmap
* @return True if the pose was set successfully, false otherwise
*/
bool getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) const;
/** @brief Returns costmap name */
inline const std::string& getName() const noexcept
{
return name_;
}
/** @brief Returns the delay in transform (tf) data that is tolerable in seconds */
double getTransformTolerance() const
{
return transform_tolerance_;
}
/** @brief Return a pointer to the "master" costmap which receives updates from all the layers.
*
* Same as calling getLayeredCostmap()->getCostmap(). */
Costmap2D* getCostmap() const
{
return layered_costmap_->getCostmap();
}
/**
* @brief Returns the global frame of the costmap
* @return The global frame of the costmap
*/
inline const std::string& getGlobalFrameID() const noexcept
{
return global_frame_;
}
/**
* @brief Returns the local frame of the costmap
* @return The local frame of the costmap
*/
inline const std::string& getBaseFrameID() const noexcept
{
return robot_base_frame_;
}
LayeredCostmap* getLayeredCostmap() const
{
return layered_costmap_;
}
/** @brief Returns the current padded footprint as a robot_geometry_msgs::Polygon. */
robot_geometry_msgs::Polygon getRobotFootprintPolygon() const
{
return robot_costmap_2d::toPolygon(padded_footprint_);
}
/** @brief Return the current footprint of the robot as a vector of points.
*
* This version of the footprint is padded by the footprint_padding_
* distance, set in the rosparam "footprint_padding".
*
* The footprint initially comes from the rosparam "footprint" but
* can be overwritten by dynamic reconfigure or by messages received
* on the "footprint" topic. */
inline const std::vector<robot_geometry_msgs::Point>& getRobotFootprint() const noexcept
{
return padded_footprint_;
}
/** @brief Return the current unpadded footprint of the robot as a vector of points.
*
* This is the raw version of the footprint without padding.
*
* The footprint initially comes from the rosparam "footprint" but
* can be overwritten by dynamic reconfigure or by messages received
* on the "footprint" topic. */
inline const std::vector<robot_geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
{
return unpadded_footprint_;
}
/**
* @brief Build the oriented footprint of the robot at the robot's current pose
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
void getOrientedFootprint(std::vector<robot_geometry_msgs::Point>& oriented_footprint) const;
/** @brief Set the footprint of the robot to be the given set of
* points, padded by footprint_padding.
*
* Should be a convex polygon, though this is not enforced.
*
* First expands the given polygon by footprint_padding_ and then
* sets padded_footprint_ and calls
* layered_costmap_->setFootprint(). Also saves the unpadded
* footprint, which is available from
* getUnpaddedRobotFootprint(). */
void setUnpaddedRobotFootprint(const std::vector<robot_geometry_msgs::Point>& points);
/** @brief Set the footprint of the robot to be the given polygon,
* padded by footprint_padding.
*
* Should be a convex polygon, though this is not enforced.
*
* First expands the given polygon by footprint_padding_ and then
* sets padded_footprint_ and calls
* layered_costmap_->setFootprint(). Also saves the unpadded
* footprint, which is available from
* getUnpaddedRobotFootprint(). */
void setUnpaddedRobotFootprintPolygon(const robot_geometry_msgs::Polygon& footprint);
protected:
LayeredCostmap* layered_costmap_;
std::string name_;
tf3::BufferCore& tf_; ///< @brief Used for transforming point clouds
std::string global_frame_; ///< @brief The global frame for the costmap
std::string robot_base_frame_; ///< @brief The frame_id of the robot base
double transform_tolerance_; ///< timeout before transform errors
private:
/** @brief Set the footprint from the new_config object.
*
* If the values of footprint and robot_radius are the same in
* new_config and old_config, nothing is changed. */
void readFootprintFromConfig(const std::vector<robot_geometry_msgs::Point> &new_footprint,
const std::vector<robot_geometry_msgs::Point> &old_footprint,
const double &robot_radius);
std::vector<boost::function<PluginLayerPtr()>> creators_;
void checkMovement();
void mapUpdateLoop(double frequency);
bool map_update_thread_shutdown_;
bool stop_updates_, initialized_, stopped_;
boost::thread* map_update_thread_; ///< @brief A thread for updating the map
robot::Time last_publish_;
robot::Duration publish_cycle;
boost::recursive_mutex configuration_mutex_;
std::vector<robot_geometry_msgs::Point> unpadded_footprint_;
std::vector<robot_geometry_msgs::Point> padded_footprint_;
float footprint_padding_;
private:
void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
};
// class Costmap2DROBOT
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H

151
include/robot_costmap_2d/costmap_layer.h Executable file
View File

@@ -0,0 +1,151 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
#define ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
namespace robot_costmap_2d
{
class CostmapLayer : public Layer, public Costmap2D
{
public:
CostmapLayer() : has_extra_bounds_(false),
extra_min_x_(1e6), extra_max_x_(-1e6),
extra_min_y_(1e6), extra_max_y_(-1e6) {}
bool isDiscretized()
{
return true;
}
virtual void matchSize();
virtual void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area=false);
/**
* If an external source changes values in the costmap,
* it should call this method with the area that it changed
* to ensure that the costmap includes this region as well.
* @param mx0 Minimum x value of the bounding box
* @param my0 Minimum y value of the bounding box
* @param mx1 Maximum x value of the bounding box
* @param my1 Maximum y value of the bounding box
*/
void addExtraBounds(double mx0, double my0, double mx1, double my1);
protected:
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* TrueOverwrite means every value from this layer
* is written into the master grid.
*/
void updateWithTrueOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* Overwrite means every valid value from this layer
* is written into the master grid (does not copy NO_INFORMATION)
*/
void updateWithOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* Sets the new value to the maximum of the master_grid's value
* and this layer's value. If the master value is NO_INFORMATION,
* it is overwritten. If the layer's value is NO_INFORMATION,
* the master value does not change.
*/
void updateWithMax(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* Sets the new value to the sum of the master grid's value
* and this layer's value. If the master value is NO_INFORMATION,
* it is overwritten with the layer's value. If the layer's value
* is NO_INFORMATION, then the master value does not change.
*
* If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE,
* the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1).
*/
void updateWithAddition(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
/**
* Updates the bounding box specified in the parameters to include
* the location (x,y)
*
* @param x x-coordinate to include
* @param y y-coordinate to include
* @param min_x bounding box
* @param min_y bounding box
* @param max_x bounding box
* @param max_y bounding box
*/
void touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y);
/*
* Updates the bounding box specified in the parameters
* to include the bounding box from the addExtraBounds
* call. If addExtraBounds was not called, the method will do nothing.
*
* Should be called at the beginning of the updateBounds method
*
* @param min_x bounding box (input and output)
* @param min_y bounding box (input and output)
* @param max_x bounding box (input and output)
* @param max_y bounding box (input and output)
*/
void useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y);
bool has_extra_bounds_;
private:
double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
};
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_

69
include/robot_costmap_2d/costmap_math.h Executable file
View File

@@ -0,0 +1,69 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
#define ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
#include <math.h>
#include <algorithm>
#include <vector>
#include <robot_geometry_msgs/Point.h>
/** @brief Return -1 if x < 0, +1 otherwise. */
inline double sign(double x)
{
return x < 0.0 ? -1.0 : 1.0;
}
/** @brief Same as sign(x) but returns 0 if x is 0. */
inline double sign0(double x)
{
return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0);
}
inline double distance(double x0, double y0, double x1, double y1)
{
return hypot(x1 - x0, y1 - y0);
}
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1);
bool intersects(std::vector<robot_geometry_msgs::Point>& polygon, float testx, float testy);
bool intersects(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2);
#endif // ROBOT_COSTMAP_2D_COSTMAP_MATH_H_

19
include/robot_costmap_2d/critical_layer.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_
#define ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_
#include <robot_costmap_2d/static_layer.h>
namespace robot_costmap_2d
{
class CriticalLayer : public StaticLayer
{
public:
CriticalLayer();
virtual ~CriticalLayer();
private:
unsigned char interpretValue(unsigned char value) override;
void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
};
}
#endif // ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_

35
include/robot_costmap_2d/directional_layer.h Normal file
View File

@@ -0,0 +1,35 @@
#ifndef ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#define ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#include <robot_costmap_2d/static_layer.h>
#include <robot_nav_msgs/Path.h>
namespace robot_costmap_2d
{
class DirectionalLayer : public StaticLayer
{
public:
DirectionalLayer();
virtual ~DirectionalLayer();
bool laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan);
void resetMap();
private:
void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path);
unsigned char laneFilter(unsigned int x, unsigned int y, double yaw_robot);
void laneFilter(std::vector<unsigned int> x, std::vector<unsigned int> y, std::vector<double> yaw_robot);
void laneFilter(std::vector<std::pair<unsigned int, double>> x, std::vector<std::pair<unsigned int, double>> y, std::vector<double> yaw_robot);
bool inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance);
void convertToMap(unsigned char *costmap, robot_nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th);
bool getRobotPose(double &x, double &y, double &yaw);
std::vector<std::array<uint16_t, 2>> directional_areas_;
double pose_x_, pose_y_, pose_yaw_;
double distance_skip_ = 1.0;
// ros::Publisher lane_mask_pub_;
robot_nav_msgs::OccupancyGrid new_map_;
};
}
#endif // ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_H_

149
include/robot_costmap_2d/footprint.h Executable file
View File

@@ -0,0 +1,149 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_FOOTPRINT_H
#define ROBOT_COSTMAP_2D_FOOTPRINT_H
#include <robot_geometry_msgs/Polygon.h>
#include <robot_geometry_msgs/PolygonStamped.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/Point32.h>
#include <robot/node_handle.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
namespace robot_costmap_2d
{
/**
* @brief Calculate the extreme distances for the footprint
*
* @param footprint The footprint to examine
* @param min_dist Output parameter of the minimum distance
* @param max_dist Output parameter of the maximum distance
*/
void calculateMinAndMaxDistances(const std::vector<robot_geometry_msgs::Point>& footprint,
double& min_dist, double& max_dist);
/**
* @brief Convert Point32 to Point
*/
robot_geometry_msgs::Point toPoint(robot_geometry_msgs::Point32 pt);
/**
* @brief Convert Point to Point32
*/
robot_geometry_msgs::Point32 toPoint32(robot_geometry_msgs::Point pt);
/**
* @brief Convert vector of Points to Polygon msg
*/
robot_geometry_msgs::Polygon toPolygon(std::vector<robot_geometry_msgs::Point> pts);
/**
* @brief Convert Polygon msg to vector of Points.
*/
std::vector<robot_geometry_msgs::Point> toPointVector(robot_geometry_msgs::Polygon polygon);
/**
* @brief Given a pose and base footprint, build the oriented footprint of the robot (list of Points)
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
std::vector<robot_geometry_msgs::Point>& oriented_footprint);
/**
* @brief Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped)
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
robot_geometry_msgs::PolygonStamped & oriented_footprint);
/**
* @brief Adds the specified amount of padding to the footprint (in place)
*/
void padFootprint(std::vector<robot_geometry_msgs::Point>& footprint, double padding);
/**
* @brief Create a circular footprint from a given radius
*/
std::vector<robot_geometry_msgs::Point> makeFootprintFromRadius(double radius);
/**
* @brief Make the footprint from the given string.
*
* Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...]
*
*/
bool makeFootprintFromString(const std::string& footprint_string, std::vector<robot_geometry_msgs::Point>& footprint);
/**
* @brief Read the ros-params "footprint" and/or "robot_radius" from
* the given NodeHandle using searchParam() to go up the tree.
*/
std::vector<robot_geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh);
/**
* @brief Create the footprint from the given XmlRpcValue.
*
* @param footprint_xmlrpc should be an array of arrays, where the
* top-level array should have 3 or more elements, and the
* sub-arrays should all have exactly 2 elements (x and y
* coordinates).
*
* @param full_param_name this is the full name of the rosparam from
* which the footprint_xmlrpc value came. It is used only for
* reporting errors. */
std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name);
// /** @brief Write the current unpadded_footprint_ to the "footprint"
// * parameter of the given NodeHandle so that dynamic_reconfigure
// * will see the new value. */
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<robot_geometry_msgs::Point>& footprint);
} // end namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_FOOTPRINT_H

197
include/robot_costmap_2d/inflation_layer.h Executable file
View File

@@ -0,0 +1,197 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
#define ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <boost/thread.hpp>
namespace robot_costmap_2d
{
/**
* @class CellData
* @brief Storage for cell information used during obstacle inflation
*/
class CellData
{
public:
/**
* @brief Constructor for a CellData objects
* @param i The index of the cell in the cost map
* @param x The x coordinate of the cell in the cost map
* @param y The y coordinate of the cell in the cost map
* @param sx The x coordinate of the closest obstacle cell in the costmap
* @param sy The y coordinate of the closest obstacle cell in the costmap
* @return
*/
CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy)
{
}
unsigned int index_;
unsigned int x_, y_;
unsigned int src_x_, src_y_;
};
class InflationLayer : public Layer
{
public:
InflationLayer();
virtual ~InflationLayer()
{
deleteKernels();
if (seen_)
delete[] seen_;
}
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual bool isDiscretized()
{
return true;
}
virtual void matchSize();
virtual void reset() { onInitialize(); }
/** @brief Given a distance, compute a cost.
* @param distance The distance from an obstacle in cells
* @return A cost value for the distance */
virtual inline unsigned char computeCost(double distance) const
{
unsigned char cost = 0;
if (distance == 0)
cost = LETHAL_OBSTACLE;
else if (distance * resolution_ <= inscribed_radius_)
cost = INSCRIBED_INFLATED_OBSTACLE;
else
{
// make sure cost falls off by Euclidean distance
double euclidean_distance = distance * resolution_;
double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
}
return cost;
}
/**
* @brief Change the values of the inflation radius parameters
* @param inflation_radius The new inflation radius
* @param cost_scaling_factor The new weight
*/
void setInflationParameters(double inflation_radius, double cost_scaling_factor);
protected:
virtual void onFootprintChanged();
boost::recursive_mutex* inflation_access_;
double resolution_;
double inflation_radius_;
double inscribed_radius_;
double weight_;
bool inflate_unknown_;
private:
void handleImpl(const void* data,
const std::type_info& info,
const std::string& source) override;
/**
* @brief Lookup pre-computed distances
* @param mx The x coordinate of the current cell
* @param my The y coordinate of the current cell
* @param src_x The x coordinate of the source cell
* @param src_y The y coordinate of the source cell
* @return
*/
inline double distanceLookup(int mx, int my, int src_x, int src_y)
{
unsigned int dx = abs(mx - src_x);
unsigned int dy = abs(my - src_y);
return cached_distances_[dx][dy];
}
/**
* @brief Lookup pre-computed costs
* @param mx The x coordinate of the current cell
* @param my The y coordinate of the current cell
* @param src_x The x coordinate of the source cell
* @param src_y The y coordinate of the source cell
* @return
*/
inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
{
unsigned int dx = abs(mx - src_x);
unsigned int dy = abs(my - src_y);
return cached_costs_[dx][dy];
}
void computeCaches();
void deleteKernels();
void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
unsigned int cellDistance(double world_dist)
{
return layered_costmap_->getCostmap()->cellDistance(world_dist);
}
inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y);
unsigned int cell_inflation_radius_;
unsigned int cached_cell_inflation_radius_;
std::map<double, std::vector<CellData> > inflation_cells_;
bool* seen_;
int seen_size_;
unsigned char** cached_costs_;
double** cached_distances_;
double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around.
};
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_INFLATION_LAYER_H_

169
include/robot_costmap_2d/layer.h Executable file
View File

@@ -0,0 +1,169 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_LAYER_H_
#define ROBOT_COSTMAP_2D_LAYER_H_
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/utils.h>
#include <string>
#include <tf3/buffer_core.h>
#include <robot/node_handle.h>
namespace robot_costmap_2d
{
class LayeredCostmap;
class Layer
{
public:
Layer();
void initialize(LayeredCostmap* parent, std::string name, tf3::BufferCore *tf);
/**
* @brief This is called by the LayeredCostmap to poll this plugin as to how
* much of the costmap it needs to update. Each layer can increase
* the size of this bounds.
*
* For more details, see "Layered Costmaps for Context-Sensitive Navigation",
* by Lu et. Al, IROS 2014.
*/
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y) {}
/**
* @brief Actually update the underlying costmap, only within the bounds
* calculated during UpdateBounds().
*/
virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}
/** @brief Stop publishers. */
virtual void deactivate() {}
/** @brief Restart publishers if they've been stopped. */
virtual void activate() {}
virtual void reset() {}
virtual ~Layer() {}
/**
* @brief Check to make sure all the data in the layer is up to date.
* If the layer is not up to date, then it may be unsafe to
* plan using the data from this layer, and the planner may
* need to know.
*
* A layer's current state should be managed by the protected
* variable current_.
* @return Whether the data in the layer is up to date.
*/
bool isCurrent() const
{
return current_;
}
/**
* @brief getter if the current layer is enabled.
*
* The user may enable/disable a layer though dynamic reconfigure.
* Disabled layers won't receive calls to
* - Layer::updateCosts
* - Layer::updateBounds
* - Layer::isCurrent
* from the LayeredCostmap.
*
* Calls to Layer::activate, Layer::deactivate and Layer::reset won't be
* blocked.
*/
inline bool isEnabled() const noexcept
{
return enabled_;
}
/** @brief Implement this to make this layer match the size of the parent costmap. */
virtual void matchSize() {}
inline const std::string& getName() const noexcept
{
return name_;
}
/** @brief Convenience function for layered_costmap_->getFootprint(). */
const std::vector<robot_geometry_msgs::Point>& getFootprint() const;
/** @brief LayeredCostmap calls this whenever the footprint there
* changes (via LayeredCostmap::setFootprint()). Override to be
* notified of changes to the robot's footprint. */
virtual void onFootprintChanged() {}
template<typename T>
void dataCallBack(const T& value, const std::string& topic) {
handle(value, topic);
}
protected:
// Hàm template public, dùng để gửi dữ liệu
template<typename T>
void handle(const T& data, const std::string& topic) {
handleImpl(static_cast<const void*>(&data), typeid(T), topic);
}
// Hàm ảo duy nhất mà plugin sẽ override
virtual void handleImpl(const void* data,
const std::type_info& type,
const std::string& topic) = 0;
/** @brief This is called at the end of initialize(). Override to
* implement subclass-specific initialization.
*
* tf_, name_, and layered_costmap_ will all be set already when this is called. */
virtual void onInitialize() {}
LayeredCostmap* layered_costmap_;
bool current_;
bool enabled_;
std::string name_;
tf3::BufferCore *tf_;
private:
std::vector<robot_geometry_msgs::Point> footprint_spec_;
};
using PluginLayerPtr = boost::shared_ptr<Layer>;
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_LAYER_H_

177
include/robot_costmap_2d/layered_costmap.h Executable file
View File

@@ -0,0 +1,177 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
#define ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
#include <robot_costmap_2d/cost_values.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <vector>
#include <string>
namespace robot_costmap_2d
{
class Layer;
/**
* @class LayeredCostmap
* @brief Instantiates different layer plugins and aggregates them into one score
*/
class LayeredCostmap
{
public:
/**
* @brief Constructor for a costmap
*/
LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);
/**
* @brief Destructor
*/
~LayeredCostmap();
/**
* @brief Update the underlying costmap with new data.
* If you want to update the map outside of the update loop that runs, you can call this.
*/
void updateMap(double robot_x, double robot_y, double robot_yaw);
inline const std::string& getGlobalFrameID() const noexcept
{
return global_frame_;
}
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y,
bool size_locked = false);
void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy)
{
minx = minx_;
miny = miny_;
maxx = maxx_;
maxy = maxy_;
}
bool isCurrent();
Costmap2D* getCostmap()
{
return &costmap_;
}
bool isRolling()
{
return rolling_window_;
}
bool isTrackingUnknown()
{
return costmap_.getDefaultValue() == robot_costmap_2d::NO_INFORMATION;
}
std::vector<boost::shared_ptr<Layer> >* getPlugins()
{
return &plugins_;
}
void addPlugin(boost::shared_ptr<Layer> plugin)
{
plugins_.push_back(plugin);
}
bool isSizeLocked()
{
return size_locked_;
}
void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn)
{
*x0 = bx0_;
*xn = bxn_;
*y0 = by0_;
*yn = byn_;
}
bool isInitialized()
{
return initialized_;
}
/** @brief Updates the stored footprint, updates the circumscribed
* and inscribed radii, and calls onFootprintChanged() in all
* layers. */
void setFootprint(const std::vector<robot_geometry_msgs::Point>& footprint_spec);
/** @brief Returns the latest footprint stored with setFootprint(). */
const std::vector<robot_geometry_msgs::Point>& getFootprint() { return footprint_; }
/** @brief The radius of a circle centered at the origin of the
* robot which just surrounds all points on the robot's
* footprint.
*
* This is updated by setFootprint(). */
double getCircumscribedRadius() { return circumscribed_radius_; }
/** @brief The radius of a circle centered at the origin of the
* robot which is just within all points and edges of the robot's
* footprint.
*
* This is updated by setFootprint(). */
double getInscribedRadius() { return inscribed_radius_; }
private:
Costmap2D costmap_;
std::string global_frame_;
bool rolling_window_; /// < @brief Whether or not the costmap should roll with the robot
bool current_;
double minx_, miny_, maxx_, maxy_;
unsigned int bx0_, bxn_, by0_, byn_;
std::vector<boost::shared_ptr<Layer> > plugins_;
bool initialized_;
bool size_locked_;
double circumscribed_radius_, inscribed_radius_;
std::vector<robot_geometry_msgs::Point> footprint_;
};
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_

102
include/robot_costmap_2d/observation.h Executable file
View File

@@ -0,0 +1,102 @@
/*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of 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.
*
* Authors: Conor McGann
*/
#ifndef ROBOT_COSTMAP_2D_OBSERVATION_H_
#define ROBOT_COSTMAP_2D_OBSERVATION_H_
#include <robot_geometry_msgs/Point.h>
#include <robot_sensor_msgs/PointCloud2.h>
namespace robot_costmap_2d
{
/**
* @brief Stores an observation in terms of a point cloud and the origin of the source
* @note Tried to make members and constructor arguments const but the compiler would not accept the default
* assignment operator for vector insertion!
*/
class Observation
{
public:
/**
* @brief Creates an empty observation
*/
Observation() :
cloud_(new robot_sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
{
}
virtual ~Observation()
{
delete cloud_;
}
/**
* @brief Creates an observation from an origin point and a point cloud
* @param origin The origin point of the observation
* @param cloud The point cloud of the observation
* @param obstacle_range The range out to which an observation should be able to insert obstacles
* @param raytrace_range The range out to which an observation should be able to clear via raytracing
*/
Observation(robot_geometry_msgs::Point& origin, const robot_sensor_msgs::PointCloud2 &cloud,
double obstacle_range, double raytrace_range) :
origin_(origin), cloud_(new robot_sensor_msgs::PointCloud2(cloud)),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
{
}
/**
* @brief Copy constructor
* @param obs The observation to copy
*/
Observation(const Observation& obs) :
origin_(obs.origin_), cloud_(new robot_sensor_msgs::PointCloud2(*(obs.cloud_))),
obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
{
}
/**
* @brief Creates an observation from a point cloud
* @param cloud The point cloud of the observation
* @param obstacle_range The range out to which an observation should be able to insert obstacles
*/
Observation(const robot_sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
cloud_(new robot_sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
{
}
robot_geometry_msgs::Point origin_;
robot_sensor_msgs::PointCloud2* cloud_;
double obstacle_range_, raytrace_range_;
};
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_OBSERVATION_H_

154
include/robot_costmap_2d/observation_buffer.h Executable file
View File

@@ -0,0 +1,154 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
#define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
#include <vector>
#include <list>
#include <string>
#include <robot/time.h>
#include <robot_costmap_2d/observation.h>
#include <tf3/buffer_core.h>
#include <robot_sensor_msgs/PointCloud2.h>
// Thread support
#include <boost/thread.hpp>
namespace robot_costmap_2d
{
/**
* @class ObservationBuffer
* @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
*/
class ObservationBuffer
{
public:
/**
* @brief Constructs an observation buffer
* @param topic_name The topic of the observations, used as an identifier for error and warning messages
* @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
* @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
* @param min_obstacle_height The minimum height of a hitpoint to be considered legal
* @param max_obstacle_height The minimum height of a hitpoint to be considered legal
* @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
* @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
* @param tf2_buffer A reference to a tf2 Buffer
* @param global_frame The frame to transform PointClouds into
* @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
* @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
*/
ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
std::string sensor_frame, double tf_tolerance);
/**
* @brief Destructor... cleans up
*/
~ObservationBuffer();
/**
* @brief Sets the global frame of an observation buffer. This will
* transform all the currently cached observations to the new global
* frame
* @param new_global_frame The name of the new global frame.
* @return True if the operation succeeds, false otherwise
*/
bool setGlobalFrame(const std::string new_global_frame);
/**
* @brief Transforms a PointCloud to the global frame and buffers it
* <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
* @param cloud The cloud to be buffered
*/
void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
/**
* @brief Pushes copies of all current observations onto the end of the vector passed in
* @param observations The vector to be filled
*/
void getObservations(std::vector<Observation>& observations);
/**
* @brief Check if the observation buffer is being update at its expected rate
* @return True if it is being updated at the expected rate, false otherwise
*/
bool isCurrent() const;
/**
* @brief Lock the observation buffer
*/
inline void lock()
{
lock_.lock();
}
/**
* @brief Lock the observation buffer
*/
inline void unlock()
{
lock_.unlock();
}
/**
* @brief Reset last updated timestamp
*/
void resetLastUpdated();
private:
/**
* @brief Removes any stale observations from the buffer list
*/
void purgeStaleObservations();
tf3::BufferCore& tf3_buffer_;
const robot::Duration observation_keep_time_;
const robot::Duration expected_update_rate_;
robot::Time last_updated_;
std::string global_frame_;
std::string sensor_frame_;
std::list<Observation> observation_list_;
std::string topic_name_;
double min_obstacle_height_, max_obstacle_height_;
boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
double obstacle_range_, raytrace_range_;
double tf_tolerance_;
};
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

172
include/robot_costmap_2d/obstacle_layer.h Executable file
View File

@@ -0,0 +1,172 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
#define ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
#include <robot_costmap_2d/costmap_layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/observation_buffer.h>
#include <robot_costmap_2d/footprint.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.hpp>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h>
namespace robot_costmap_2d
{
class ObstacleLayer : public CostmapLayer
{
public:
ObstacleLayer()
{
costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D.
}
virtual ~ObstacleLayer();
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void activate();
virtual void deactivate();
virtual void reset();
// for testing purposes
void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
void clearStaticObservations(bool marking, bool clearing);
protected:
void handleImpl(const void* data,
const std::type_info&,
const std::string& topic) override;
/**
* @brief A callback to handle buffering LaserScan messages
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void laserScanCallback(const robot_sensor_msgs::LaserScan& message,
const boost::shared_ptr<ObservationBuffer>& buffer);
/**
* @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void laserScanValidInfCallback(const robot_sensor_msgs::LaserScan& message,
const boost::shared_ptr<ObservationBuffer>& buffer);
/**
* @brief A callback to handle buffering PointCloud messages
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
/**
* @brief A callback to handle buffering PointCloud2 messages
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
/**
* @brief Get the observations used to mark space
* @param marking_observations A reference to a vector that will be populated with the observations
* @return True if all the observation buffers are current, false otherwise
*/
bool getMarkingObservations(std::vector<robot_costmap_2d::Observation>& marking_observations) const;
/**
* @brief Get the observations used to clear space
* @param clearing_observations A reference to a vector that will be populated with the observations
* @return True if all the observation buffers are current, false otherwise
*/
bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;
/**
* @brief Clear freespace based on one observation
* @param clearing_observation The observation used to raytrace
* @param min_x
* @param min_y
* @param max_x
* @param max_y
*/
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
double* max_x, double* max_y);
std::vector<robot_geometry_msgs::Point> transformed_footprint_;
bool footprint_clearing_enabled_;
void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
std::string global_frame_; ///< @brief The global frame for the costmap
double max_obstacle_height_; ///< @brief Max Obstacle Height
laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
// Used only for testing purposes
std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
bool rolling_window_;
bool stop_receiving_data_ = false;
int combination_method_;
private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
};
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_

18
include/robot_costmap_2d/preferred_layer.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef ROBOT_COSTMAP_2D_PREFERRED_LAYER_H_
#define ROBOT_COSTMAP_2D_PREFERRED_LAYER_H_
#include <robot_costmap_2d/static_layer.h>
namespace robot_costmap_2d
{
class PreferredLayer : public StaticLayer
{
public:
PreferredLayer();
virtual ~PreferredLayer();
private:
unsigned char interpretValue(unsigned char value);
};
}
#endif // ROBOT_COSTMAP_2D_PREFERRED_LAYER_

96
include/robot_costmap_2d/static_layer.h Executable file
View File

@@ -0,0 +1,96 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_STATIC_LAYER_H_
#define ROBOT_COSTMAP_2D_STATIC_LAYER_H_
#include <robot_costmap_2d/costmap_layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <string>
namespace robot_costmap_2d
{
class StaticLayer : public CostmapLayer
{
public:
StaticLayer();
virtual ~StaticLayer();
virtual void onInitialize();
virtual void activate();
virtual void deactivate();
virtual void reset();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void matchSize();
protected:
void handleImpl(const void* data,
const std::type_info& type,
const std::string& topic) override;
virtual unsigned char interpretValue(unsigned char value);
virtual void incomingMap(const robot_nav_msgs::OccupancyGrid& new_map);
virtual void incomingUpdate(const robot_map_msgs::OccupancyGridUpdate& update);
unsigned char* threshold_;
std::string base_frame_id_;
std::string global_frame_; ///< @brief The global frame for the costmap
std::string map_frame_; /// @brief frame that map is located in
std::string map_topic_; // Hiep thêm vào mục đich phân biết zones
bool subscribe_to_updates_;
bool map_received_;
bool has_updated_data_;
unsigned int x_, y_, width_, height_;
bool track_unknown_space_;
bool use_maximum_;
bool first_map_only_; ///< @brief Store the first static map and reuse it on reinitializing
bool trinary_costmap_;
bool map_shutdown_ = false;
bool map_update_shutdown_ = false;
unsigned char lethal_threshold_, unknown_cost_value_;
private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
};
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_STATIC_LAYER_H_

99
include/robot_costmap_2d/testing_helper.h Normal file
View File

@@ -0,0 +1,99 @@
#ifndef ROBOT_COSTMAP_2D_TESTING_HELPER_H
#define ROBOT_COSTMAP_2D_TESTING_HELPER_H
#include<robot_costmap_2d/cost_values.h>
#include<robot_costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/static_layer.h>
#include <robot_costmap_2d/obstacle_layer.h>
#include <robot_costmap_2d/inflation_layer.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
const double MAX_Z(1.0);
char printableCost(unsigned char cost)
{
switch (cost)
{
case robot_costmap_2d::NO_INFORMATION: return '?';
case robot_costmap_2d::LETHAL_OBSTACLE: return 'L';
case robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
case robot_costmap_2d::FREE_SPACE: return '.';
default: return '0' + (unsigned char) (10 * cost / 255);
}
}
void printMap(robot_costmap_2d::Costmap2D& costmap)
{
printf("map:\n");
for (int i = 0; i < costmap.getSizeInCellsY(); i++){
for (int j = 0; j < costmap.getSizeInCellsX(); j++){
printf("%4d", int(costmap.getCost(j, i)));
}
printf("\n\n");
}
}
unsigned int countValues(robot_costmap_2d::Costmap2D& costmap, unsigned char value, bool equal = true)
{
unsigned int count = 0;
for (int i = 0; i < costmap.getSizeInCellsY(); i++){
for (int j = 0; j < costmap.getSizeInCellsX(); j++){
unsigned char c = costmap.getCost(j, i);
if ((equal && c == value) || (!equal && c != value))
{
count+=1;
}
}
}
return count;
}
void addStaticLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
{
robot_costmap_2d::StaticLayer* slayer = new robot_costmap_2d::StaticLayer();
layers.addPlugin(boost::shared_ptr<robot_costmap_2d::Layer>(slayer));
slayer->initialize(&layers, "static", &tf);
}
robot_costmap_2d::ObstacleLayer* addObstacleLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
{
robot_costmap_2d::ObstacleLayer* olayer = new robot_costmap_2d::ObstacleLayer();
olayer->initialize(&layers, "obstacles", &tf);
layers.addPlugin(boost::shared_ptr<robot_costmap_2d::Layer>(olayer));
return olayer;
}
void addObservation(robot_costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
robot_sensor_msgs::PointCloud2 cloud;
robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(1);
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
*iter_x = x;
*iter_y = y;
*iter_z = z;
robot_geometry_msgs::Point p;
p.x = ox;
p.y = oy;
p.z = oz;
robot_costmap_2d::Observation obs(p, cloud, 100.0, 100.0); // obstacle range = raytrace range = 100.0
olayer->addStaticObservation(obs, true, true);
}
robot_costmap_2d::InflationLayer* addInflationLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
{
robot_costmap_2d::InflationLayer* ilayer = new robot_costmap_2d::InflationLayer();
ilayer->initialize(&layers, "inflation", &tf);
boost::shared_ptr<robot_costmap_2d::Layer> ipointer(ilayer);
layers.addPlugin(ipointer);
return ilayer;
}
#endif // ROBOT_COSTMAP_2D_TESTING_HELPER_H

21
include/robot_costmap_2d/unpreferred_layer.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_H_
#define ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_H_
#include <robot_costmap_2d/static_layer.h>
namespace robot_costmap_2d
{
class UnPreferredLayer : public StaticLayer
{
public:
UnPreferredLayer();
virtual ~UnPreferredLayer();
private:
unsigned char interpretValue(unsigned char value);
};
}
#endif // ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_

67
include/robot_costmap_2d/utils.h Normal file
View File

@@ -0,0 +1,67 @@
#ifndef ROBOT_COSTMAP_2D_UTILS_H
#define ROBOT_COSTMAP_2D_UTILS_H
#include <yaml-cpp/yaml.h>
#include <filesystem>
#include <string>
#include <iostream>
namespace robot_costmap_2d
{
template<typename T>
T loadParam(const YAML::Node& node, const std::string& key, const T& default_value)
{
if (node[key] && node[key].IsDefined())
return node[key].as<T>();
return default_value;
}
inline std::vector<robot_geometry_msgs::Point> loadFootprint(const YAML::Node& node, const std::vector<robot_geometry_msgs::Point>& default_value)
{
if( !node || !node.IsDefined())
return default_value;
std::vector<robot_geometry_msgs::Point> fp;
for (const auto& p : node) {
if (p.size() != 2)
throw std::runtime_error("Footprint point must be [x, y]");
robot_geometry_msgs::Point point;
point.x = p[0].as<double>();
point.y = p[1].as<double>();
point.z = 0.0;
fp.push_back(point);
}
std::cout << "Loaded footprint with " << fp.size() << " points." << std::endl;
return fp;
}
inline std::string getSourceFile(const std::string& root, const std::string& config_file_name)
{
std::string path_source = " ";
std::string sub_folder = "config";
namespace fs = std::filesystem;
fs::path cfg_dir = fs::path(root) / sub_folder;
if (fs::exists(cfg_dir) && fs::is_directory(cfg_dir))
{
for (const auto& entry : fs::recursive_directory_iterator(cfg_dir))
{
if (entry.is_regular_file() && entry.path().filename() == config_file_name)
{
path_source = entry.path().string();
std::cout << "Path source: " << path_source << std::endl;
break; // tìm thấy thì dừng
}
}
}
if(path_source == " ")
std::cout<< config_file_name << " file not found at path: "<< cfg_dir << std::endl;
return path_source;
}
}
#endif // ROBOT_COSTMAP_2D_UTILS_H

32
include/robot_costmap_2d/voxel_grid.h Normal file
View File

@@ -0,0 +1,32 @@
// Header header
// uint32[] data
// geometry_msgs/Point32 origin
// geometry_msgs/Vector3 resolutions
// uint32 size_x
// uint32 size_y
// uint32 size_z
#ifndef VOXEL_GRID_ROBOT_COSTMAP_2D_H
#define VOXEL_GRID_ROBOT_COSTMAP_2D_H
#include <vector>
#include <robot_std_msgs/Header.h>
#include <robot_geometry_msgs/Point32.h>
#include <robot_geometry_msgs/Vector3.h>
namespace robot_costmap_2d
{
struct VoxelGrid
{
robot_std_msgs::Header header;
std::vector<uint32_t> data;
robot_geometry_msgs::Point32 origin;
robot_geometry_msgs::Vector3 resolutions;
uint32_t size_x;
uint32_t size_y;
uint32_t size_z;
};
}
#endif // VOXEL_GRID_ROBOT_COSTMAP_2D_H

142
include/robot_costmap_2d/voxel_layer.h Executable file
View File

@@ -0,0 +1,142 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
#define ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/observation_buffer.h>
#include <robot_costmap_2d/voxel_grid.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.hpp>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h>
#include <robot_costmap_2d/obstacle_layer.h>
#include <voxel_grid/voxel_grid.h>
namespace robot_costmap_2d
{
class VoxelLayer : public ObstacleLayer
{
public:
VoxelLayer() :
voxel_grid_(0, 0, 0)
{
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
}
virtual ~VoxelLayer();
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
void updateOrigin(double new_origin_x, double new_origin_y);
bool isDiscretized()
{
return true;
}
virtual void matchSize();
virtual void reset();
protected:
virtual void resetMaps();
private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
bool publish_voxel_;
voxel_grid::VoxelGrid voxel_grid_;
double z_resolution_, origin_z_;
unsigned int unknown_threshold_, mark_threshold_, size_z_;
robot_sensor_msgs::PointCloud clearing_endpoints_;
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
{
if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
return false;
mx = ((wx - origin_x_) / resolution_);
my = ((wy - origin_y_) / resolution_);
mz = ((wz - origin_z_) / z_resolution_);
if (mx < size_x_ && my < size_y_ && mz < size_z_)
return true;
return false;
}
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_) / resolution_);
my = (int)((wy - origin_y_) / resolution_);
mz = (int)((wz - origin_z_) / z_resolution_);
if (mx < size_x_ && my < size_y_ && mz < size_z_)
return true;
return false;
}
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) * resolution_;
wy = origin_y_ + (my + 0.5) * resolution_;
wz = origin_z_ + (mz + 0.5) * z_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));
}
};
} // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_VOXEL_LAYER_H_