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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
CMakeLists.txt
View File

@ -1,6 +1,6 @@
# --- CMake version và project name --- # --- CMake version và project name ---
cmake_minimum_required(VERSION 3.10) cmake_minimum_required(VERSION 3.10)
project(costmap_2d) project(robot_costmap_2d)
# --- C++ standard và position independent code --- # --- C++ standard và position independent code ---
set(CMAKE_CXX_STANDARD 17) # Sử dụng C++17 set(CMAKE_CXX_STANDARD 17) # Sử dụng C++17
@ -10,8 +10,8 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) # Thư viện có thể build thành sh
# Dùng để runtime linker tìm thư viện đã build trước khi install # Dùng để runtime linker tìm thư viện đã build trước khi install
set(CMAKE_SKIP_BUILD_RPATH FALSE) set(CMAKE_SKIP_BUILD_RPATH FALSE)
set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/costmap_2d") set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/robot_costmap_2d")
set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d") set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/robot_costmap_2d")
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
# --- Dependencies --- # --- Dependencies ---
@ -23,7 +23,7 @@ find_package(GTest REQUIRED) # Google Test cho unit test
find_package(PCL REQUIRED COMPONENTS common io) # Point Cloud Library find_package(PCL REQUIRED COMPONENTS common io) # Point Cloud Library
# --- Define macro để dùng trong code --- # --- Define macro để dùng trong code ---
add_definitions(-DCOSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}") add_definitions(-DROBOT_COSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
# --- Include directories --- # --- Include directories ---
# Thêm các folder chứa header files # Thêm các folder chứa header files
@ -39,9 +39,9 @@ link_directories(${PCL_LIBRARY_DIRS}) # Thêm thư viện PCL vào linker path
# --- Eigen và PCL definitions --- # --- Eigen và PCL definitions ---
add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS}) add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})
# --- Core library: costmap_2d --- # --- Core library: robot_costmap_2d ---
# Tạo thư viện chính # Tạo thư viện chính
add_library(costmap_2d add_library(robot_costmap_2d
src/costmap_2d_robot.cpp src/costmap_2d_robot.cpp
src/array_parser.cpp src/array_parser.cpp
src/costmap_2d.cpp src/costmap_2d.cpp
@ -54,7 +54,7 @@ add_library(costmap_2d
) )
# --- Link các thư viện phụ thuộc --- # --- Link các thư viện phụ thuộc ---
target_link_libraries(costmap_2d target_link_libraries(robot_costmap_2d
${Boost_LIBRARIES} # Boost ${Boost_LIBRARIES} # Boost
robot_std_msgs robot_std_msgs
robot_sensor_msgs robot_sensor_msgs
@ -65,8 +65,8 @@ target_link_libraries(costmap_2d
robot_visualization_msgs robot_visualization_msgs
voxel_grid voxel_grid
tf3 tf3
tf3_geometry_msgs robot_tf3_geometry_msgs
tf3_sensor_msgs robot_tf3_sensor_msgs
data_convert data_convert
robot_xmlrpcpp # XMLRPC robot_xmlrpcpp # XMLRPC
yaml-cpp yaml-cpp
@ -75,7 +75,7 @@ target_link_libraries(costmap_2d
) )
# --- Include directories cho target --- # --- Include directories cho target ---
target_include_directories(costmap_2d target_include_directories(robot_costmap_2d
PUBLIC PUBLIC
${Boost_INCLUDE_DIRS} # Boost headers ${Boost_INCLUDE_DIRS} # Boost headers
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> # Khi build từ source $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> # Khi build từ source
@ -83,23 +83,23 @@ target_include_directories(costmap_2d
) )
# --- Cài đặt thư viện vào hệ thống khi chạy make install --- # --- Cài đặt thư viện vào hệ thống khi chạy make install ---
install(TARGETS costmap_2d install(TARGETS robot_costmap_2d
EXPORT costmap_2d-targets EXPORT robot_costmap_2d-targets
ARCHIVE DESTINATION lib # Thư viện tĩnh .a ARCHIVE DESTINATION lib # Thư viện tĩnh .a
LIBRARY DESTINATION lib # Thư viện động .so LIBRARY DESTINATION lib # Thư viện động .so
RUNTIME DESTINATION bin # File thực thi (nếu có) RUNTIME DESTINATION bin # File thực thi (nếu có)
INCLUDES DESTINATION include # Cài đặt include INCLUDES DESTINATION include # Cài đặt include
) )
# --- Xuất export set costmap_2dTargets thành file CMake module --- # --- Xuất export set robot_costmap_2dTargets thành file CMake module ---
# --- Tạo file lib/cmake/costmap_2d/costmap_2dTargets.cmake --- # --- Tạo file lib/cmake/robot_costmap_2d/robot_costmap_2dTargets.cmake ---
# --- File này chứa cấu hình giúp project khác có thể dùng --- # --- File này chứa cấu hình giúp project khác có thể dùng ---
# --- Find_package(costmap_2d REQUIRED) --- # --- Find_package(robot_costmap_2d REQUIRED) ---
# --- Target_link_libraries(my_app PRIVATE costmap_2d::costmap_2d) --- # --- Target_link_libraries(my_app PRIVATE robot_costmap_2d::robot_costmap_2d) ---
install(EXPORT costmap_2d-targets install(EXPORT robot_costmap_2d-targets
FILE costmap_2d-targets.cmake FILE robot_costmap_2d-targets.cmake
NAMESPACE costmap_2d:: NAMESPACE robot_costmap_2d::
DESTINATION lib/cmake/costmap_2d DESTINATION lib/cmake/robot_costmap_2d
) )
# --- Cài đặt headers --- # --- Cài đặt headers ---
@ -123,7 +123,7 @@ add_library(plugins
target_link_libraries(plugins target_link_libraries(plugins
PRIVATE PRIVATE
costmap_2d robot_costmap_2d
${Boost_LIBRARIES} ${Boost_LIBRARIES}
yaml-cpp yaml-cpp
robot_time robot_time
@ -144,13 +144,13 @@ install(TARGETS plugins
install(EXPORT plugins-targets install(EXPORT plugins-targets
FILE plugins-targets.cmake FILE plugins-targets.cmake
NAMESPACE costmap_2d:: NAMESPACE robot_costmap_2d::
DESTINATION lib/cmake/plugins DESTINATION lib/cmake/plugins
) )
# --- Option để bật/tắt test --- # --- Option để bật/tắt test ---
option(BUILD_COSTMAP_TESTS "Build costmap_2d test executables" ON) option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" ON)
if(BUILD_COSTMAP_TESTS) if(BUILD_COSTMAP_TESTS)
# --- Test executables --- # --- Test executables ---
@ -159,8 +159,8 @@ if(BUILD_COSTMAP_TESTS)
add_executable(test_plugin test/static_layer_test.cpp) add_executable(test_plugin test/static_layer_test.cpp)
# --- Link thư viện cho test --- # --- Link thư viện cho test ---
target_link_libraries(test_array_parser PRIVATE costmap_2d GTest::GTest GTest::Main pthread) target_link_libraries(test_array_parser PRIVATE robot_costmap_2d GTest::GTest GTest::Main pthread)
target_link_libraries(test_costmap PRIVATE costmap_2d GTest::GTest GTest::Main pthread) target_link_libraries(test_costmap PRIVATE robot_costmap_2d GTest::GTest GTest::Main pthread)
target_link_libraries(test_plugin PRIVATE target_link_libraries(test_plugin PRIVATE
${Boost_LIBRARIES} ${Boost_LIBRARIES}
@ -171,24 +171,24 @@ if(BUILD_COSTMAP_TESTS)
yaml-cpp yaml-cpp
tf3 tf3
robot_time robot_time
costmap_2d robot_costmap_2d
GTest::GTest GTest::Main GTest::GTest GTest::Main
) )
# --- Set RPATH để tìm thư viện của project này thay vì system ROS libraries --- # --- Set RPATH để tìm thư viện của project này thay vì system ROS libraries ---
set_target_properties(test_array_parser PROPERTIES set_target_properties(test_array_parser PROPERTIES
BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3" BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d" INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
LINK_FLAGS "-Wl,--disable-new-dtags" LINK_FLAGS "-Wl,--disable-new-dtags"
) )
set_target_properties(test_costmap PROPERTIES set_target_properties(test_costmap PROPERTIES
BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3" BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d" INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
LINK_FLAGS "-Wl,--disable-new-dtags" LINK_FLAGS "-Wl,--disable-new-dtags"
) )
set_target_properties(test_plugin PROPERTIES set_target_properties(test_plugin PROPERTIES
BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3" BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d" INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
LINK_FLAGS "-Wl,--disable-new-dtags" LINK_FLAGS "-Wl,--disable-new-dtags"
) )
endif() endif()

2
config/costmap_params.yaml
View File

@ -1,4 +1,4 @@
costmap_2d: robot_costmap_2d:
global_frame: map global_frame: map
robot_base_frame: base_link robot_base_frame: base_link
rolling_window: false rolling_window: false

19
include/costmap_2d/critical_layer.h
View File

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

18
include/costmap_2d/preferred_layer.h
View File

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

99
include/costmap_2d/testing_helper.h
View File

@ -1,99 +0,0 @@
#ifndef COSTMAP_2D_TESTING_HELPER_H
#define COSTMAP_2D_TESTING_HELPER_H
#include<costmap_2d/cost_values.h>
#include<costmap_2d/costmap_2d.h>
#include <costmap_2d/static_layer.h>
#include <costmap_2d/obstacle_layer.h>
#include <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 costmap_2d::NO_INFORMATION: return '?';
case costmap_2d::LETHAL_OBSTACLE: return 'L';
case costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
case costmap_2d::FREE_SPACE: return '.';
default: return '0' + (unsigned char) (10 * cost / 255);
}
}
void printMap(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(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(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
{
costmap_2d::StaticLayer* slayer = new costmap_2d::StaticLayer();
layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
slayer->initialize(&layers, "static", &tf);
}
costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
{
costmap_2d::ObstacleLayer* olayer = new costmap_2d::ObstacleLayer();
olayer->initialize(&layers, "obstacles", &tf);
layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(olayer));
return olayer;
}
void addObservation(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;
costmap_2d::Observation obs(p, cloud, 100.0, 100.0); // obstacle range = raytrace range = 100.0
olayer->addStaticObservation(obs, true, true);
}
costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
{
costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer();
ilayer->initialize(&layers, "inflation", &tf);
boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
layers.addPlugin(ipointer);
return ilayer;
}
#endif // COSTMAP_2D_TESTING_HELPER_H

21
include/costmap_2d/unpreferred_layer.h
View File

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

10
include/costmap_2d/array_parser.h → include/robot_costmap_2d/array_parser.h
View File

@ -28,13 +28,13 @@
* *
* author: Dave Hershberger * author: Dave Hershberger
*/ */
#ifndef COSTMAP_2D_ARRAY_PARSER_H #ifndef ROBOT_COSTMAP_2D_ARRAY_PARSER_H
#define COSTMAP_2D_ARRAY_PARSER_H #define ROBOT_COSTMAP_2D_ARRAY_PARSER_H
#include <vector> #include <vector>
#include <string> #include <string>
namespace costmap_2d namespace robot_costmap_2d
{ {
/** @brief Parse a vector of vectors of floats from a string. /** @brief Parse a vector of vectors of floats from a string.
@ -46,6 +46,6 @@ namespace costmap_2d
* anything, like part of a successful parse. */ * anything, like part of a successful parse. */
std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return); std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return);
} // end namespace costmap_2d } // end namespace robot_costmap_2d
#endif // COSTMAP_2D_ARRAY_PARSER_H #endif // ROBOT_COSTMAP_2D_ARRAY_PARSER_H

8
include/costmap_2d/cost_values.h → include/robot_costmap_2d/cost_values.h
View File

@ -34,10 +34,10 @@
* *
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_COST_VALUES_H_ #ifndef ROBOT_COSTMAP_2D_COST_VALUES_H_
#define COSTMAP_2D_COST_VALUES_H_ #define ROBOT_COSTMAP_2D_COST_VALUES_H_
/** Provides a mapping for often used cost values */ /** Provides a mapping for often used cost values */
namespace costmap_2d namespace robot_costmap_2d
{ {
static const unsigned char NO_INFORMATION = 255; static const unsigned char NO_INFORMATION = 255;
static const unsigned char LETHAL_OBSTACLE = 254; static const unsigned char LETHAL_OBSTACLE = 254;
@ -47,4 +47,4 @@ static const unsigned char FREE_SPACE = 60;
static const unsigned char PREFERRED_SPACE = 20; static const unsigned char PREFERRED_SPACE = 20;
static const unsigned char UNPREFERRED_SPACE = 100; static const unsigned char UNPREFERRED_SPACE = 100;
} }
#endif // COSTMAP_2D_COST_VALUES_H_ #endif // ROBOT_COSTMAP_2D_COST_VALUES_H_

10
include/costmap_2d/costmap_2d.h → include/robot_costmap_2d/costmap_2d.h
View File

@ -35,15 +35,15 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_2D_H_ #ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
#define COSTMAP_2D_COSTMAP_2D_H_ #define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
#include <vector> #include <vector>
#include <queue> #include <queue>
#include <robot_geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
#include <boost/thread.hpp> #include <boost/thread.hpp>
namespace costmap_2d namespace robot_costmap_2d
{ {
// convenient for storing x/y point pairs // convenient for storing x/y point pairs
@ -464,6 +464,6 @@ protected:
std::vector<MapLocation>& cells_; std::vector<MapLocation>& cells_;
}; };
}; };
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_COSTMAP_2D_H #endif // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H

20
include/costmap_2d/costmap_2d_robot.h → include/robot_costmap_2d/costmap_2d_robot.h
View File

@ -35,12 +35,12 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_2D_ROBOT_H_ #ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
#define COSTMAP_2D_COSTMAP_2D_ROBOT_H_ #define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
#include <costmap_2d/layered_costmap.h> #include <robot_costmap_2d/layered_costmap.h>
#include <costmap_2d/layer.h> #include <robot_costmap_2d/layer.h>
#include <costmap_2d/footprint.h> #include <robot_costmap_2d/footprint.h>
#include <robot_geometry_msgs/Polygon.h> #include <robot_geometry_msgs/Polygon.h>
#include <robot_geometry_msgs/PolygonStamped.h> #include <robot_geometry_msgs/PolygonStamped.h>
@ -49,7 +49,7 @@
#include <tf3/LinearMath/Transform.h> #include <tf3/LinearMath/Transform.h>
#include <robot/rate.h> #include <robot/rate.h>
#include <data_convert/data_convert.h> #include <data_convert/data_convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h> #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_xmlrpcpp/XmlRpcValue.h> #include <robot_xmlrpcpp/XmlRpcValue.h>
@ -70,7 +70,7 @@ public:
} }
}; };
namespace costmap_2d namespace robot_costmap_2d
{ {
/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to /** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
@ -182,7 +182,7 @@ public:
/** @brief Returns the current padded footprint as a robot_geometry_msgs::Polygon. */ /** @brief Returns the current padded footprint as a robot_geometry_msgs::Polygon. */
robot_geometry_msgs::Polygon getRobotFootprintPolygon() const robot_geometry_msgs::Polygon getRobotFootprintPolygon() const
{ {
return costmap_2d::toPolygon(padded_footprint_); return robot_costmap_2d::toPolygon(padded_footprint_);
} }
/** @brief Return the current footprint of the robot as a vector of points. /** @brief Return the current footprint of the robot as a vector of points.
@ -275,6 +275,6 @@ private:
void getParams(const std::string& config_file_name, robot::NodeHandle& nh); void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
}; };
// class Costmap2DROBOT // class Costmap2DROBOT
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_COSTMAP_2D_ROBOT_H #endif // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H

22
include/costmap_2d/costmap_layer.h → include/robot_costmap_2d/costmap_layer.h
View File

@ -35,13 +35,13 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_LAYER_H_ #ifndef ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
#define COSTMAP_2D_COSTMAP_LAYER_H_ #define ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
#include <costmap_2d/layer.h> #include <robot_costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h> #include <robot_costmap_2d/layered_costmap.h>
namespace costmap_2d namespace robot_costmap_2d
{ {
class CostmapLayer : public Layer, public Costmap2D class CostmapLayer : public Layer, public Costmap2D
@ -79,7 +79,7 @@ protected:
* TrueOverwrite means every value from this layer * TrueOverwrite means every value from this layer
* is written into the master grid. * is written into the master grid.
*/ */
void updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); 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 * Updates the master_grid within the specified
@ -88,7 +88,7 @@ protected:
* Overwrite means every valid value from this layer * Overwrite means every valid value from this layer
* is written into the master grid (does not copy NO_INFORMATION) * is written into the master grid (does not copy NO_INFORMATION)
*/ */
void updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); 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 * Updates the master_grid within the specified
@ -99,7 +99,7 @@ protected:
* it is overwritten. If the layer's value is NO_INFORMATION, * it is overwritten. If the layer's value is NO_INFORMATION,
* the master value does not change. * the master value does not change.
*/ */
void updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); 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 * Updates the master_grid within the specified
@ -113,7 +113,7 @@ protected:
* If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE, * If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE,
* the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1). * the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1).
*/ */
void updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); 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 * Updates the bounding box specified in the parameters to include
@ -147,5 +147,5 @@ private:
double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_; double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
}; };
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_COSTMAP_LAYER_H_ #endif // ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_

6
include/costmap_2d/costmap_math.h → include/robot_costmap_2d/costmap_math.h
View File

@ -35,8 +35,8 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_MATH_H_ #ifndef ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
#define COSTMAP_2D_COSTMAP_MATH_H_ #define ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
#include <math.h> #include <math.h>
#include <algorithm> #include <algorithm>
@ -66,4 +66,4 @@ bool intersects(std::vector<robot_geometry_msgs::Point>& polygon, float testx, f
bool intersects(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2); bool intersects(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2);
#endif // COSTMAP_2D_COSTMAP_MATH_H_ #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_

10
include/costmap_2d/directional_layer.h → include/robot_costmap_2d/directional_layer.h
View File

@ -1,10 +1,10 @@
#ifndef COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_ #ifndef ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_ #define ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#include <costmap_2d/static_layer.h> #include <robot_costmap_2d/static_layer.h>
#include <robot_nav_msgs/Path.h> #include <robot_nav_msgs/Path.h>
namespace costmap_2d namespace robot_costmap_2d
{ {
class DirectionalLayer : public StaticLayer class DirectionalLayer : public StaticLayer
{ {
@ -32,4 +32,4 @@ namespace costmap_2d
}; };
} }
#endif // COSTMAP_2D_DIRECTIONAL_LAYER_H_ #endif // ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_H_

10
include/costmap_2d/footprint.h → include/robot_costmap_2d/footprint.h
View File

@ -35,8 +35,8 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_FOOTPRINT_H #ifndef ROBOT_COSTMAP_2D_FOOTPRINT_H
#define COSTMAP_2D_FOOTPRINT_H #define ROBOT_COSTMAP_2D_FOOTPRINT_H
#include <robot_geometry_msgs/Polygon.h> #include <robot_geometry_msgs/Polygon.h>
#include <robot_geometry_msgs/PolygonStamped.h> #include <robot_geometry_msgs/PolygonStamped.h>
@ -46,7 +46,7 @@
#include <robot/node_handle.h> #include <robot/node_handle.h>
#include <robot_xmlrpcpp/XmlRpcValue.h> #include <robot_xmlrpcpp/XmlRpcValue.h>
namespace costmap_2d namespace robot_costmap_2d
{ {
/** /**
@ -144,6 +144,6 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::X
// * will see the new value. */ // * will see the new value. */
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<robot_geometry_msgs::Point>& footprint); // void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<robot_geometry_msgs::Point>& footprint);
} // end namespace costmap_2d } // end namespace robot_costmap_2d
#endif // COSTMAP_2D_FOOTPRINT_H #endif // ROBOT_COSTMAP_2D_FOOTPRINT_H

16
include/costmap_2d/inflation_layer.h → include/robot_costmap_2d/inflation_layer.h
View File

@ -35,14 +35,14 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_INFLATION_LAYER_H_ #ifndef ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
#define COSTMAP_2D_INFLATION_LAYER_H_ #define ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
#include <costmap_2d/layer.h> #include <robot_costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h> #include <robot_costmap_2d/layered_costmap.h>
#include <boost/thread.hpp> #include <boost/thread.hpp>
namespace costmap_2d namespace robot_costmap_2d
{ {
/** /**
* @class CellData * @class CellData
@ -84,7 +84,7 @@ public:
virtual void onInitialize(); virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y); double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual bool isDiscretized() virtual bool isDiscretized()
{ {
return true; return true;
@ -192,6 +192,6 @@ private:
bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around. bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around.
}; };
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_INFLATION_LAYER_H_ #endif // ROBOT_COSTMAP_2D_INFLATION_LAYER_H_

16
include/costmap_2d/layer.h → include/robot_costmap_2d/layer.h
View File

@ -34,16 +34,16 @@
* *
* Author: David V. Lu!! * Author: David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_LAYER_H_ #ifndef ROBOT_COSTMAP_2D_LAYER_H_
#define COSTMAP_2D_LAYER_H_ #define ROBOT_COSTMAP_2D_LAYER_H_
#include <costmap_2d/costmap_2d.h> #include <robot_costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h> #include <robot_costmap_2d/layered_costmap.h>
#include <costmap_2d/utils.h> #include <robot_costmap_2d/utils.h>
#include <string> #include <string>
#include <tf3/buffer_core.h> #include <tf3/buffer_core.h>
#include <robot/node_handle.h> #include <robot/node_handle.h>
namespace costmap_2d namespace robot_costmap_2d
{ {
class LayeredCostmap; class LayeredCostmap;
@ -164,6 +164,6 @@ private:
using PluginLayerPtr = boost::shared_ptr<Layer>; using PluginLayerPtr = boost::shared_ptr<Layer>;
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_LAYER_H_ #endif // ROBOT_COSTMAP_2D_LAYER_H_

18
include/costmap_2d/layered_costmap.h → include/robot_costmap_2d/layered_costmap.h
View File

@ -35,16 +35,16 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_ #ifndef ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
#define COSTMAP_2D_LAYERED_COSTMAP_H_ #define ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
#include <costmap_2d/cost_values.h> #include <robot_costmap_2d/cost_values.h>
#include <costmap_2d/layer.h> #include <robot_costmap_2d/layer.h>
#include <costmap_2d/costmap_2d.h> #include <robot_costmap_2d/costmap_2d.h>
#include <vector> #include <vector>
#include <string> #include <string>
namespace costmap_2d namespace robot_costmap_2d
{ {
class Layer; class Layer;
@ -101,7 +101,7 @@ public:
bool isTrackingUnknown() bool isTrackingUnknown()
{ {
return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION; return costmap_.getDefaultValue() == robot_costmap_2d::NO_INFORMATION;
} }
std::vector<boost::shared_ptr<Layer> >* getPlugins() std::vector<boost::shared_ptr<Layer> >* getPlugins()
@ -172,6 +172,6 @@ private:
std::vector<robot_geometry_msgs::Point> footprint_; std::vector<robot_geometry_msgs::Point> footprint_;
}; };
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_LAYERED_COSTMAP_H_ #endif // ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_

10
include/costmap_2d/observation.h → include/robot_costmap_2d/observation.h
View File

@ -29,13 +29,13 @@
* Authors: Conor McGann * Authors: Conor McGann
*/ */
#ifndef COSTMAP_2D_OBSERVATION_H_ #ifndef ROBOT_COSTMAP_2D_OBSERVATION_H_
#define COSTMAP_2D_OBSERVATION_H_ #define ROBOT_COSTMAP_2D_OBSERVATION_H_
#include <robot_geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
namespace costmap_2d namespace robot_costmap_2d
{ {
/** /**
@ -98,5 +98,5 @@ public:
double obstacle_range_, raytrace_range_; double obstacle_range_, raytrace_range_;
}; };
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_OBSERVATION_H_ #endif // ROBOT_COSTMAP_2D_OBSERVATION_H_

12
include/costmap_2d/observation_buffer.h → include/robot_costmap_2d/observation_buffer.h
View File

@ -34,14 +34,14 @@
* *
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_ #ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
#define COSTMAP_2D_OBSERVATION_BUFFER_H_ #define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
#include <vector> #include <vector>
#include <list> #include <list>
#include <string> #include <string>
#include <robot/time.h> #include <robot/time.h>
#include <costmap_2d/observation.h> #include <robot_costmap_2d/observation.h>
#include <tf3/buffer_core.h> #include <tf3/buffer_core.h>
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
@ -49,7 +49,7 @@
// Thread support // Thread support
#include <boost/thread.hpp> #include <boost/thread.hpp>
namespace costmap_2d namespace robot_costmap_2d
{ {
/** /**
* @class ObservationBuffer * @class ObservationBuffer
@ -150,5 +150,5 @@ private:
double obstacle_range_, raytrace_range_; double obstacle_range_, raytrace_range_;
double tf_tolerance_; double tf_tolerance_;
}; };
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_OBSERVATION_BUFFER_H_ #endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

40
include/costmap_2d/obstacle_layer.h → include/robot_costmap_2d/obstacle_layer.h
View File

@ -35,13 +35,13 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_OBSTACLE_LAYER_H_ #ifndef ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
#define COSTMAP_2D_OBSTACLE_LAYER_H_ #define ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
#include <costmap_2d/costmap_layer.h> #include <robot_costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h> #include <robot_costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h> #include <robot_costmap_2d/observation_buffer.h>
#include <costmap_2d/footprint.h> #include <robot_costmap_2d/footprint.h>
#include <robot_nav_msgs/OccupancyGrid.h> #include <robot_nav_msgs/OccupancyGrid.h>
@ -51,7 +51,7 @@
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h> #include <robot_sensor_msgs/point_cloud_conversion.h>
namespace costmap_2d namespace robot_costmap_2d
{ {
class ObstacleLayer : public CostmapLayer class ObstacleLayer : public CostmapLayer
@ -66,14 +66,14 @@ public:
virtual void onInitialize(); virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y); double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); 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 activate();
virtual void deactivate(); virtual void deactivate();
virtual void reset(); virtual void reset();
// for testing purposes // for testing purposes
void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing); void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
void clearStaticObservations(bool marking, bool clearing); void clearStaticObservations(bool marking, bool clearing);
protected: protected:
@ -103,7 +103,7 @@ protected:
* @param buffer A pointer to the observation buffer to update * @param buffer A pointer to the observation buffer to update
*/ */
void pointCloudCallback(const robot_sensor_msgs::PointCloud& message, void pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer); const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
/** /**
* @brief A callback to handle buffering PointCloud2 messages * @brief A callback to handle buffering PointCloud2 messages
@ -111,21 +111,21 @@ protected:
* @param buffer A pointer to the observation buffer to update * @param buffer A pointer to the observation buffer to update
*/ */
void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message, void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer); const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
/** /**
* @brief Get the observations used to mark space * @brief Get the observations used to mark space
* @param marking_observations A reference to a vector that will be populated with the observations * @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 * @return True if all the observation buffers are current, false otherwise
*/ */
bool getMarkingObservations(std::vector<costmap_2d::Observation>& marking_observations) const; bool getMarkingObservations(std::vector<robot_costmap_2d::Observation>& marking_observations) const;
/** /**
* @brief Get the observations used to clear space * @brief Get the observations used to clear space
* @param clearing_observations A reference to a vector that will be populated with the observations * @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 * @return True if all the observation buffers are current, false otherwise
*/ */
bool getClearingObservations(std::vector<costmap_2d::Observation>& clearing_observations) const; bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;
/** /**
* @brief Clear freespace based on one observation * @brief Clear freespace based on one observation
@ -135,7 +135,7 @@ protected:
* @param max_x * @param max_x
* @param max_y * @param max_y
*/ */
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y, virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_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, void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
@ -151,12 +151,12 @@ protected:
laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles 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<costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing 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 // Used only for testing purposes
std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_; std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
bool rolling_window_; bool rolling_window_;
bool stop_receiving_data_ = false; bool stop_receiving_data_ = false;
@ -167,6 +167,6 @@ private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh); bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
}; };
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_OBSTACLE_LAYER_H_ #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_

16
include/costmap_2d/static_layer.h → include/robot_costmap_2d/static_layer.h
View File

@ -35,17 +35,17 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_STATIC_LAYER_H_ #ifndef ROBOT_COSTMAP_2D_STATIC_LAYER_H_
#define COSTMAP_2D_STATIC_LAYER_H_ #define ROBOT_COSTMAP_2D_STATIC_LAYER_H_
#include <costmap_2d/costmap_layer.h> #include <robot_costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h> #include <robot_costmap_2d/layered_costmap.h>
#include <robot_nav_msgs/OccupancyGrid.h> #include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_map_msgs/OccupancyGridUpdate.h> #include <robot_map_msgs/OccupancyGridUpdate.h>
#include <string> #include <string>
namespace costmap_2d namespace robot_costmap_2d
{ {
class StaticLayer : public CostmapLayer class StaticLayer : public CostmapLayer
@ -60,7 +60,7 @@ public:
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y); double* max_x, double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void matchSize(); virtual void matchSize();
protected: protected:
@ -91,6 +91,6 @@ private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh); bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
}; };
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_STATIC_LAYER_H_ #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_

8
include/costmap_2d/utils.h → include/robot_costmap_2d/utils.h
View File

@ -1,11 +1,11 @@
#ifndef COSTMAP_2D_UTILS_H #ifndef ROBOT_COSTMAP_2D_UTILS_H
#define COSTMAP_2D_UTILS_H #define ROBOT_COSTMAP_2D_UTILS_H
#include <yaml-cpp/yaml.h> #include <yaml-cpp/yaml.h>
#include <filesystem> #include <filesystem>
#include <string> #include <string>
#include <iostream> #include <iostream>
namespace costmap_2d namespace robot_costmap_2d
{ {
template<typename T> template<typename T>
T loadParam(const YAML::Node& node, const std::string& key, const T& default_value) T loadParam(const YAML::Node& node, const std::string& key, const T& default_value)
@ -64,4 +64,4 @@ namespace costmap_2d
} }
#endif // COSTMAP_2D_UTILS_H #endif // ROBOT_COSTMAP_2D_UTILS_H

8
include/costmap_2d/voxel_grid.h → include/robot_costmap_2d/voxel_grid.h
View File

@ -6,15 +6,15 @@
// uint32 size_y // uint32 size_y
// uint32 size_z // uint32 size_z
#ifndef VOXEL_GRID_COSTMAP_2D_H #ifndef VOXEL_GRID_ROBOT_COSTMAP_2D_H
#define VOXEL_GRID_COSTMAP_2D_H #define VOXEL_GRID_ROBOT_COSTMAP_2D_H
#include <vector> #include <vector>
#include <robot_std_msgs/Header.h> #include <robot_std_msgs/Header.h>
#include <robot_geometry_msgs/Point32.h> #include <robot_geometry_msgs/Point32.h>
#include <robot_geometry_msgs/Vector3.h> #include <robot_geometry_msgs/Vector3.h>
namespace costmap_2d namespace robot_costmap_2d
{ {
struct VoxelGrid struct VoxelGrid
{ {
@ -29,4 +29,4 @@ namespace costmap_2d
} }
#endif // VOXEL_GRID_COSTMAP_2D_H #endif // VOXEL_GRID_ROBOT_COSTMAP_2D_H

22
include/costmap_2d/voxel_layer.h → include/robot_costmap_2d/voxel_layer.h
View File

@ -35,23 +35,23 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_VOXEL_LAYER_H_ #ifndef ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
#define COSTMAP_2D_VOXEL_LAYER_H_ #define ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
#include <costmap_2d/layer.h> #include <robot_costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h> #include <robot_costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h> #include <robot_costmap_2d/observation_buffer.h>
#include <costmap_2d/voxel_grid.h> #include <robot_costmap_2d/voxel_grid.h>
#include <robot_nav_msgs/OccupancyGrid.h> #include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h> #include <robot_sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.hpp> #include <laser_geometry/laser_geometry.hpp>
#include <robot_sensor_msgs/PointCloud.h> #include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h> #include <robot_sensor_msgs/point_cloud_conversion.h>
#include <costmap_2d/obstacle_layer.h> #include <robot_costmap_2d/obstacle_layer.h>
#include <voxel_grid/voxel_grid.h> #include <voxel_grid/voxel_grid.h>
namespace costmap_2d namespace robot_costmap_2d
{ {
class VoxelLayer : public ObstacleLayer class VoxelLayer : public ObstacleLayer
@ -85,7 +85,7 @@ protected:
private: private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh); 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); void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y, virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y); double* max_x, double* max_y);
@ -137,6 +137,6 @@ private:
} }
}; };
} // namespace costmap_2d } // namespace robot_costmap_2d
#endif // COSTMAP_2D_VOXEL_LAYER_H_ #endif // ROBOT_COSTMAP_2D_VOXEL_LAYER_H_

8
package.xml
View File

@ -1,9 +1,9 @@
<package> <package>
<name>costmap_2d</name> <name>robot_costmap_2d</name>
<version>0.7.10</version> <version>0.7.10</version>
<description> <description>
costmap_2d is the second generation of the transform library, which lets robot_costmap_2d is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. costmap_2d the user keep track of multiple coordinate frames over time. robot_costmap_2d
maintains the relationship between coordinate frames in a tree maintains the relationship between coordinate frames in a tree
structure buffered in time, and lets the user transform points, structure buffered in time, and lets the user transform points,
vectors, etc between any two coordinate frames at any desired vectors, etc between any two coordinate frames at any desired
@ -15,7 +15,7 @@
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer> <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license> <license>BSD</license>
<url type="website">http://www.ros.org/wiki/costmap_2d</url> <url type="website">http://www.ros.org/wiki/robot_costmap_2d</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend> <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>

12
plugins/critical_layer.cpp
View File

@ -1,11 +1,11 @@
#include <costmap_2d/critical_layer.h> #include <robot_costmap_2d/critical_layer.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION; using robot_costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE; using robot_costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::CRITICAL_SPACE; using robot_costmap_2d::CRITICAL_SPACE;
namespace costmap_2d namespace robot_costmap_2d
{ {
CriticalLayer::CriticalLayer(){} CriticalLayer::CriticalLayer(){}
@ -21,7 +21,7 @@ unsigned char CriticalLayer::interpretValue(unsigned char value)
return NO_INFORMATION; return NO_INFORMATION;
} }
void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) void CriticalLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{ {
// printf("TEST PLUGIN CRITICAL\n"); // printf("TEST PLUGIN CRITICAL\n");
if (!map_received_) if (!map_received_)

62
plugins/directional_layer.cpp
View File

@ -1,18 +1,18 @@
#include <costmap_2d/directional_layer.h> #include <robot_costmap_2d/directional_layer.h>
#include <data_convert/data_convert.h> #include <data_convert/data_convert.h>
#include <tf3/convert.h> #include <tf3/convert.h>
#include <tf3/utils.h> #include <tf3/utils.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h> #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
using costmap_2d::CRITICAL_SPACE; using robot_costmap_2d::CRITICAL_SPACE;
using costmap_2d::LETHAL_OBSTACLE; using robot_costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION; using robot_costmap_2d::NO_INFORMATION;
namespace costmap_2d namespace robot_costmap_2d
{ {
DirectionalLayer::DirectionalLayer() DirectionalLayer::DirectionalLayer()
{} {}
@ -79,7 +79,7 @@ namespace costmap_2d
int index = getIndex(ix / 2, iy / 2); int index = getIndex(ix / 2, iy / 2);
directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff); directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff);
directional_areas_[index][1] = (uint16_t)(color_avg >> 12u); directional_areas_[index][1] = (uint16_t)(color_avg >> 12u);
costmap_[index] = costmap_2d::NO_INFORMATION; costmap_[index] = robot_costmap_2d::NO_INFORMATION;
} }
} }
map_frame_ = new_map.header.frame_id; map_frame_ = new_map.header.frame_id;
@ -144,7 +144,7 @@ namespace costmap_2d
|| p == path.poses.back() || p == path.poses.back()
) )
{ {
if (value == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == costmap_2d::LETHAL_OBSTACLE) if (value == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == robot_costmap_2d::LETHAL_OBSTACLE)
{ {
std::pair<unsigned int, double> x_val(mx, p.pose.position.x); std::pair<unsigned int, double> x_val(mx, p.pose.position.x);
std::pair<unsigned int, double> y_val(my, p.pose.position.y); std::pair<unsigned int, double> y_val(my, p.pose.position.y);
@ -187,7 +187,7 @@ namespace costmap_2d
for (int i = 0; i < size_costmap; i++) for (int i = 0; i < size_costmap; i++)
{ {
int8_t value; int8_t value;
if (costmap[i] == costmap_2d::NO_INFORMATION) if (costmap[i] == robot_costmap_2d::NO_INFORMATION)
{ {
value = -1; value = -1;
} }
@ -211,24 +211,24 @@ namespace costmap_2d
unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot) unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot)
{ {
double yaw_lane; double yaw_lane;
unsigned char cost = costmap_2d::NO_INFORMATION; unsigned char cost = robot_costmap_2d::NO_INFORMATION;
int index = getIndex(x, y); int index = getIndex(x, y);
for (auto &lane : directional_areas_[index]) for (auto &lane : directional_areas_[index])
{ {
if (lane > 359) if (lane > 359)
{ {
cost = std::min(cost, costmap_2d::NO_INFORMATION); cost = std::min(cost, robot_costmap_2d::NO_INFORMATION);
} }
else else
{ {
double yaw_lane = (double)lane / 180 * M_PI; double yaw_lane = (double)lane / 180 * M_PI;
if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0) if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0)
cost = std::min(cost, costmap_2d::FREE_SPACE); cost = std::min(cost, robot_costmap_2d::FREE_SPACE);
else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4) else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4)
cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE); cost = std::min(cost, robot_costmap_2d::LETHAL_OBSTACLE);
else else
cost = std::min(cost, costmap_2d::NO_INFORMATION); cost = std::min(cost, robot_costmap_2d::NO_INFORMATION);
} }
} }
return cost; return cost;
@ -278,10 +278,10 @@ namespace costmap_2d
for (int j = y[i].first; j <= y_max; j++) for (int j = y[i].first; j <= y_max; j++)
{ {
int y_ = j; int y_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue; continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]); unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value; costmap_[getIndex(x_, y_)] = value;
else else
break; break;
@ -290,10 +290,10 @@ namespace costmap_2d
for (int k = y[i].first; k >= y_min; k--) for (int k = y[i].first; k >= y_min; k--)
{ {
int y_ = k; int y_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue; continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]); unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value; costmap_[getIndex(x_, y_)] = value;
else else
break; break;
@ -304,10 +304,10 @@ namespace costmap_2d
for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++) for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++)
{ {
int x_ = j; int x_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue; continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]); unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value; costmap_[getIndex(x_, y_)] = value;
else else
break; break;
@ -316,10 +316,10 @@ namespace costmap_2d
for (int k = x[i].first; k >= 0; k--) for (int k = x[i].first; k >= 0; k--)
{ {
int x_ = k; int x_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue; continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]); unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value; costmap_[getIndex(x_, y_)] = value;
else else
break; break;
@ -332,10 +332,10 @@ namespace costmap_2d
for (int j = x[i].first; j <= x_max; j++) for (int j = x[i].first; j <= x_max; j++)
{ {
int x_ = j; int x_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue; continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]); unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value; costmap_[getIndex(x_, y_)] = value;
else else
break; break;
@ -344,10 +344,10 @@ namespace costmap_2d
for (int k = x[i].first; k >= x_min; k--) for (int k = x[i].first; k >= x_min; k--)
{ {
int x_ = k; int x_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue; continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]); unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value; costmap_[getIndex(x_, y_)] = value;
else else
break; break;
@ -358,10 +358,10 @@ namespace costmap_2d
for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++) for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++)
{ {
int y_ = j; int y_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue; continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]); unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value; costmap_[getIndex(x_, y_)] = value;
else else
break; break;
@ -370,10 +370,10 @@ namespace costmap_2d
for (int k = y[i].first; k >= 0; k--) for (int k = y[i].first; k >= 0; k--)
{ {
int y_ = k; int y_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue; continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]); unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value; costmap_[getIndex(x_, y_)] = value;
else else
break; break;
@ -391,7 +391,7 @@ namespace costmap_2d
for (unsigned int ix = 0; ix < size_x; ix++) for (unsigned int ix = 0; ix < size_x; ix++)
{ {
int index = getIndex(ix, iy); int index = getIndex(ix, iy);
costmap_[index] = costmap_2d::NO_INFORMATION; costmap_[index] = robot_costmap_2d::NO_INFORMATION;
} }
} }
} }

22
plugins/inflation_layer.cpp
View File

@ -36,18 +36,18 @@
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#include <algorithm> #include <algorithm>
#include <costmap_2d/inflation_layer.h> #include <robot_costmap_2d/inflation_layer.h>
#include <costmap_2d/costmap_math.h> #include <robot_costmap_2d/costmap_math.h>
#include <costmap_2d/footprint.h> #include <robot_costmap_2d/footprint.h>
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
using costmap_2d::LETHAL_OBSTACLE; using robot_costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::INSCRIBED_INFLATED_OBSTACLE; using robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using costmap_2d::NO_INFORMATION; using robot_costmap_2d::NO_INFORMATION;
namespace costmap_2d namespace robot_costmap_2d
{ {
InflationLayer::InflationLayer() InflationLayer::InflationLayer()
@ -91,7 +91,7 @@ void InflationLayer::onInitialize()
bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh) bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{ {
try { try {
std::string folder = COSTMAP_2D_DIR; std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name); std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source); YAML::Node config = YAML::LoadFile(path_source);
@ -132,7 +132,7 @@ bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeH
void InflationLayer::matchSize() void InflationLayer::matchSize()
{ {
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap(); robot_costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
resolution_ = costmap->getResolution(); resolution_ = costmap->getResolution();
cell_inflation_radius_ = cellDistance(inflation_radius_); cell_inflation_radius_ = cellDistance(inflation_radius_);
computeCaches(); computeCaches();
@ -191,7 +191,7 @@ void InflationLayer::onFootprintChanged()
layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_); layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
} }
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{ {
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
if (cell_inflation_radius_ == 0) if (cell_inflation_radius_ == 0)
@ -416,4 +416,4 @@ static boost::shared_ptr<Layer> create_inflation_plugin() {
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_inflation_plugin, InflationLayer) BOOST_DLL_ALIAS(create_inflation_plugin, InflationLayer)
} // namespace costmap_2d } // namespace robot_costmap_2d

26
plugins/obstacle_layer.cpp
View File

@ -35,8 +35,8 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#include <costmap_2d/obstacle_layer.h> #include <robot_costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h> #include <robot_costmap_2d/costmap_math.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h> #include <robot_sensor_msgs/point_cloud2_iterator.h>
@ -44,14 +44,14 @@
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION; using robot_costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE; using robot_costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE; using robot_costmap_2d::FREE_SPACE;
using costmap_2d::ObservationBuffer; using robot_costmap_2d::ObservationBuffer;
using costmap_2d::Observation; using robot_costmap_2d::Observation;
namespace costmap_2d namespace robot_costmap_2d
{ {
void ObstacleLayer::onInitialize() void ObstacleLayer::onInitialize()
@ -75,7 +75,7 @@ ObstacleLayer::~ObstacleLayer()
bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh) bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{ {
try { try {
std::string folder = COSTMAP_2D_DIR; std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name); std::string path_source = getSourceFile(folder,config_file_name);
@ -405,11 +405,11 @@ void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot
} }
} }
void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) void ObstacleLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{ {
if (footprint_clearing_enabled_) if (footprint_clearing_enabled_)
{ {
setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); setConvexPolygonCost(transformed_footprint_, robot_costmap_2d::FREE_SPACE);
} }
switch (combination_method_) switch (combination_method_)
@ -426,7 +426,7 @@ void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i
} }
void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing) void ObstacleLayer::addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing)
{ {
if (marking) if (marking)
static_marking_observations_.push_back(obs); static_marking_observations_.push_back(obs);
@ -597,4 +597,4 @@ static boost::shared_ptr<Layer> create_obstacle_plugin() {
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_obstacle_plugin, ObstacleLayer) BOOST_DLL_ALIAS(create_obstacle_plugin, ObstacleLayer)
} // namespace costmap_2d } // namespace robot_costmap_2d

12
plugins/preferred_layer.cpp
View File

@ -1,12 +1,12 @@
#include <costmap_2d/preferred_layer.h> #include <robot_costmap_2d/preferred_layer.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION; using robot_costmap_2d::NO_INFORMATION;
using costmap_2d::FREE_SPACE; using robot_costmap_2d::FREE_SPACE;
using costmap_2d::LETHAL_OBSTACLE; using robot_costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::PREFERRED_SPACE; using robot_costmap_2d::PREFERRED_SPACE;
namespace costmap_2d namespace robot_costmap_2d
{ {
PreferredLayer::PreferredLayer(){} PreferredLayer::PreferredLayer(){}

20
plugins/static_layer.cpp
View File

@ -36,20 +36,20 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#include <costmap_2d/static_layer.h> #include <robot_costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h> #include <robot_costmap_2d/costmap_math.h>
#include <tf3/convert.h> #include <tf3/convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h> #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION; using robot_costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE; using robot_costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE; using robot_costmap_2d::FREE_SPACE;
namespace costmap_2d namespace robot_costmap_2d
{ {
StaticLayer::StaticLayer() StaticLayer::StaticLayer()
@ -75,7 +75,7 @@ void StaticLayer::onInitialize()
bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh) bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{ {
try { try {
std::string folder = COSTMAP_2D_DIR; std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name); std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source); YAML::Node config = YAML::LoadFile(path_source);
@ -324,7 +324,7 @@ void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
has_updated_data_ = false; has_updated_data_ = false;
} }
void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{ {
if (!map_received_) if (!map_received_)
return; return;
@ -389,4 +389,4 @@ static boost::shared_ptr<Layer> create_static_plugin() {
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_static_plugin, StaticLayer) BOOST_DLL_ALIAS(create_static_plugin, StaticLayer)
} // namespace costmap_2d } // namespace robot_costmap_2d

10
plugins/unpreferred_layer.cpp
View File

@ -1,11 +1,11 @@
#include <costmap_2d/unpreferred_layer.h> #include <robot_costmap_2d/unpreferred_layer.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION; using robot_costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE; using robot_costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::PREFERRED_SPACE; using robot_costmap_2d::PREFERRED_SPACE;
namespace costmap_2d namespace robot_costmap_2d
{ {
UnPreferredLayer::UnPreferredLayer(){} UnPreferredLayer::UnPreferredLayer(){}

20
plugins/voxel_layer.cpp
View File

@ -35,20 +35,20 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#include <costmap_2d/voxel_layer.h> #include <robot_costmap_2d/voxel_layer.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h> #include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
#define VOXEL_BITS 16 #define VOXEL_BITS 16
using costmap_2d::NO_INFORMATION; using robot_costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE; using robot_costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE; using robot_costmap_2d::FREE_SPACE;
using costmap_2d::ObservationBuffer; using robot_costmap_2d::ObservationBuffer;
using costmap_2d::Observation; using robot_costmap_2d::Observation;
namespace costmap_2d namespace robot_costmap_2d
{ {
void VoxelLayer::onInitialize() void VoxelLayer::onInitialize()
@ -68,7 +68,7 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
{ {
try try
{ {
std::string folder = COSTMAP_2D_DIR; std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name); std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source); YAML::Node config = YAML::LoadFile(path_source);
@ -228,7 +228,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
// if (publish_voxel_) // if (publish_voxel_)
// { // {
// costmap_2d::VoxelGrid grid_msg; // robot_costmap_2d::VoxelGrid grid_msg;
// unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); // unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
// grid_msg.size_x = voxel_grid_.sizeX(); // grid_msg.size_x = voxel_grid_.sizeX();
// grid_msg.size_y = voxel_grid_.sizeY(); // grid_msg.size_y = voxel_grid_.sizeY();
@ -490,4 +490,4 @@ static boost::shared_ptr<Layer> create_voxel_plugin() {
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_voxel_plugin, VoxelLayer) BOOST_DLL_ALIAS(create_voxel_plugin, VoxelLayer)
} // namespace costmap_2d } // namespace robot_costmap_2d

4
src/array_parser.cpp
View File

@ -34,7 +34,7 @@
#include <sstream> #include <sstream>
#include <vector> #include <vector>
namespace costmap_2d namespace robot_costmap_2d
{ {
/** @brief Parse a vector of vector of floats from a string. /** @brief Parse a vector of vector of floats from a string.
@ -112,4 +112,4 @@ std::vector<std::vector<float> > parseVVF(const std::string& input, std::string&
return result; return result;
} }
} // end namespace costmap_2d } // end namespace robot_costmap_2d

6
src/costmap_2d.cpp
View File

@ -35,12 +35,12 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#include <costmap_2d/costmap_2d.h> #include <robot_costmap_2d/costmap_2d.h>
#include <cstdio> #include <cstdio>
using namespace std; using namespace std;
namespace costmap_2d namespace robot_costmap_2d
{ {
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution, Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value) : double origin_x, double origin_y, unsigned char default_value) :
@ -485,4 +485,4 @@ bool Costmap2D::saveMap(std::string file_name)
return true; return true;
} }
} // namespace costmap_2d } // namespace robot_costmap_2d

14
src/costmap_2d_robot.cpp
View File

@ -41,9 +41,9 @@
#include <algorithm> #include <algorithm>
#include <vector> #include <vector>
#include <costmap_2d/layered_costmap.h> #include <robot_costmap_2d/layered_costmap.h>
#include <costmap_2d/costmap_2d_robot.h> #include <robot_costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/utils.h> #include <robot_costmap_2d/utils.h>
#include <tf3/convert.h> #include <tf3/convert.h>
#include <tf3/utils.h> #include <tf3/utils.h>
@ -53,7 +53,7 @@
using namespace std; using namespace std;
namespace costmap_2d namespace robot_costmap_2d
{ {
Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) : Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
layered_costmap_(NULL), layered_costmap_(NULL),
@ -83,11 +83,11 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
{ {
try try
{ {
std::string folder = COSTMAP_2D_DIR; std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name); std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source); YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["costmap_2d"]; YAML::Node layer = config["robot_costmap_2d"];
std::string global_frame = std::string global_frame =
loadParam(layer, "global_frame", std::string("map")); loadParam(layer, "global_frame", std::string("map"));
@ -567,4 +567,4 @@ void Costmap2DROBOT::getOrientedFootprint(std::vector<robot_geometry_msgs::Point
padded_footprint_, oriented_footprint); padded_footprint_, oriented_footprint);
} }
} // namespace costmap_2d } // namespace robot_costmap_2d

18
src/costmap_layer.cpp
View File

@ -1,6 +1,6 @@
#include<costmap_2d/costmap_layer.h> #include<robot_costmap_2d/costmap_layer.h>
namespace costmap_2d namespace robot_costmap_2d
{ {
void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y) void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
@ -60,7 +60,7 @@ void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, d
has_extra_bounds_ = false; has_extra_bounds_ = false;
} }
void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) void CostmapLayer::updateWithMax(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{ {
if (!enabled_) if (!enabled_)
return; return;
@ -100,7 +100,7 @@ void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i,
} }
} }
void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, void CostmapLayer::updateWithTrueOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
int max_i, int max_j) int max_i, int max_j)
{ {
if (!enabled_) if (!enabled_)
@ -119,7 +119,7 @@ void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, i
} }
} }
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) void CostmapLayer::updateWithOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{ {
if (!enabled_) if (!enabled_)
return; return;
@ -138,7 +138,7 @@ void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int m
} }
} }
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) void CostmapLayer::updateWithAddition(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{ {
if (!enabled_) if (!enabled_)
return; return;
@ -161,8 +161,8 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int mi
else else
{ {
int sum = old_cost + costmap_[it]; int sum = old_cost + costmap_[it];
if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) if (sum >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1; master_array[it] = robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
else else
master_array[it] = sum; master_array[it] = sum;
} }
@ -170,4 +170,4 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int mi
} }
} }
} }
} // namespace costmap_2d } // namespace robot_costmap_2d

2
src/costmap_math.cpp
View File

@ -27,7 +27,7 @@
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <costmap_2d/costmap_math.h> #include <robot_costmap_2d/costmap_math.h>
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1) double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
{ {

10
src/footprint.cpp
View File

@ -27,15 +27,15 @@
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <costmap_2d/costmap_math.h> #include <robot_costmap_2d/costmap_math.h>
#include <boost/tokenizer.hpp> #include <boost/tokenizer.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <costmap_2d/footprint.h> #include <robot_costmap_2d/footprint.h>
#include <costmap_2d/array_parser.h> #include <robot_costmap_2d/array_parser.h>
#include <robot_geometry_msgs/Point32.h> #include <robot_geometry_msgs/Point32.h>
namespace costmap_2d namespace robot_costmap_2d
{ {
void calculateMinAndMaxDistances(const std::vector<robot_geometry_msgs::Point>& footprint, double& min_dist, double& max_dist) void calculateMinAndMaxDistances(const std::vector<robot_geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
@ -301,4 +301,4 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::X
return footprint; return footprint;
} }
} // end namespace costmap_2d } // end namespace robot_costmap_2d

6
src/layer.cpp
View File

@ -27,9 +27,9 @@
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*/ */
#include "costmap_2d/layer.h" #include "robot_costmap_2d/layer.h"
namespace costmap_2d namespace robot_costmap_2d
{ {
Layer::Layer() Layer::Layer()
@ -53,4 +53,4 @@ const std::vector<robot_geometry_msgs::Point>& Layer::getFootprint() const
return layered_costmap_->getFootprint(); return layered_costmap_->getFootprint();
} }
} // end namespace costmap_2d } // end namespace robot_costmap_2d

10
src/layered_costmap.cpp
View File

@ -35,8 +35,8 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#include <costmap_2d/layered_costmap.h> #include <robot_costmap_2d/layered_costmap.h>
#include <costmap_2d/footprint.h> #include <robot_costmap_2d/footprint.h>
#include <cstdio> #include <cstdio>
#include <string> #include <string>
#include <algorithm> #include <algorithm>
@ -44,7 +44,7 @@
using std::vector; using std::vector;
namespace costmap_2d namespace robot_costmap_2d
{ {
LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)
@ -178,7 +178,7 @@ namespace costmap_2d
void LayeredCostmap::setFootprint(const std::vector<robot_geometry_msgs::Point> &footprint_spec) void LayeredCostmap::setFootprint(const std::vector<robot_geometry_msgs::Point> &footprint_spec)
{ {
footprint_ = footprint_spec; footprint_ = footprint_spec;
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_); robot_costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin) ++plugin)
@ -187,4 +187,4 @@ namespace costmap_2d
} }
} }
} // namespace costmap_2d } // namespace robot_costmap_2d

10
src/observation_buffer.cpp
View File

@ -34,16 +34,16 @@
* *
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
*********************************************************************/ *********************************************************************/
#include <costmap_2d/observation_buffer.h> #include <robot_costmap_2d/observation_buffer.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h> #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <tf3_sensor_msgs/tf3_sensor_msgs.h> #include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h> #include <robot_sensor_msgs/point_cloud2_iterator.h>
using namespace std; using namespace std;
using namespace tf3; using namespace tf3;
namespace costmap_2d namespace robot_costmap_2d
{ {
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate, ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range, double min_obstacle_height, double max_obstacle_height, double obstacle_range,
@ -276,5 +276,5 @@ void ObservationBuffer::resetLastUpdated()
{ {
last_updated_ = robot::Time::now(); last_updated_ = robot::Time::now();
} }
} // namespace costmap_2d } // namespace robot_costmap_2d

4
test/array_parser_test.cpp
View File

@ -29,9 +29,9 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include "costmap_2d/array_parser.h" #include "robot_costmap_2d/array_parser.h"
using namespace costmap_2d; using namespace robot_costmap_2d;
TEST(array_parser, basic_operation) TEST(array_parser, basic_operation)
{ {

4
test/coordinates_test.cpp
View File

@ -35,9 +35,9 @@
// Tests ripped from https://github.com/locusrobotics/robot_navigation/blob/master/nav_grid/test/utest.cpp // Tests ripped from https://github.com/locusrobotics/robot_navigation/blob/master/nav_grid/test/utest.cpp
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <costmap_2d/costmap_2d.h> #include <robot_costmap_2d/costmap_2d.h>
using namespace costmap_2d; using namespace robot_costmap_2d;
TEST(CostmapCoordinates, easy_coordinates_test) TEST(CostmapCoordinates, easy_coordinates_test)
{ {

30
test/static_layer_test.cpp
View File

@ -1,28 +1,28 @@
#include <costmap_2d/costmap_2d_robot.h> #include <robot_costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/cost_values.h> #include <robot_costmap_2d/cost_values.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <tf3/buffer_core.h> #include <tf3/buffer_core.h>
#include <robot_geometry_msgs/PoseWithCovariance.h> #include <robot_geometry_msgs/PoseWithCovariance.h>
namespace costmap_2d { namespace robot_costmap_2d {
class CostmapTester : public testing::Test { class CostmapTester : public testing::Test {
public: public:
CostmapTester(tf3::BufferCore& tf); CostmapTester(tf3::BufferCore& tf);
void checkConsistentCosts(); void checkConsistentCosts();
void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y); void compareCellToNeighbors(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
void compareCells(costmap_2d::Costmap2D& costmap, void compareCells(robot_costmap_2d::Costmap2D& costmap,
unsigned int x, unsigned int y, unsigned int nx, unsigned int ny); unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
virtual void TestBody(){} virtual void TestBody(){}
private: private:
costmap_2d::Costmap2DROBOT costmap_ros_; robot_costmap_2d::Costmap2DROBOT costmap_ros_;
}; };
CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_params", tf){} CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_params", tf){}
void CostmapTester::checkConsistentCosts(){ void CostmapTester::checkConsistentCosts(){
costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap(); robot_costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
//get a copy of the costmap contained by our ros wrapper //get a copy of the costmap contained by our ros wrapper
costmap->saveMap("costmap_test.pgm"); costmap->saveMap("costmap_test.pgm");
@ -35,7 +35,7 @@ void CostmapTester::checkConsistentCosts(){
} }
} }
void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){ void CostmapTester::compareCellToNeighbors(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){
//we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable //we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable
for(int offset_x = -1; offset_x <= 1; ++offset_x){ for(int offset_x = -1; offset_x <= 1; ++offset_x){
for(int offset_y = -1; offset_y <= 1; ++offset_y){ for(int offset_y = -1; offset_y <= 1; ++offset_y){
@ -51,18 +51,18 @@ void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsig
} }
//for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect //for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect
void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){ void CostmapTester::compareCells(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){
double cell_distance = hypot(static_cast<int>(x-nx), static_cast<int>(y-ny)); double cell_distance = hypot(static_cast<int>(x-nx), static_cast<int>(y-ny));
unsigned char cell_cost = costmap.getCost(x, y); unsigned char cell_cost = costmap.getCost(x, y);
unsigned char neighbor_cost = costmap.getCost(nx, ny); unsigned char neighbor_cost = costmap.getCost(nx, ny);
if(cell_cost == costmap_2d::LETHAL_OBSTACLE){ if(cell_cost == robot_costmap_2d::LETHAL_OBSTACLE){
//if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost //if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost
unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(cell_distance); unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(cell_distance);
EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == costmap_2d::FREE_SPACE)); EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == robot_costmap_2d::FREE_SPACE));
} }
else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ else if(cell_cost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
//the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away //the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away
double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1; double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance); unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
@ -72,19 +72,19 @@ void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x,
printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny); printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
costmap.saveMap("failing_costmap.pgm"); costmap.saveMap("failing_costmap.pgm");
} }
EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == costmap_2d::FREE_SPACE)); EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == robot_costmap_2d::FREE_SPACE));
} }
} }
}; };
costmap_2d::CostmapTester* map_tester = NULL; robot_costmap_2d::CostmapTester* map_tester = NULL;
int main(int argc, char** argv){ int main(int argc, char** argv){
tf3::BufferCore tf(tf3::Duration(10)); tf3::BufferCore tf(tf3::Duration(10));
map_tester = new costmap_2d::CostmapTester(tf); map_tester = new robot_costmap_2d::CostmapTester(tf);
return(0); return(0);