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_minimum_required(VERSION 3.10)
project(costmap_2d)
project(robot_costmap_2d)
# --- C++ standard và position independent code ---
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
set(CMAKE_SKIP_BUILD_RPATH FALSE)
set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/robot_costmap_2d")
set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/robot_costmap_2d")
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
# --- Dependencies ---
@ -23,7 +23,7 @@ find_package(GTest REQUIRED) # Google Test cho unit test
find_package(PCL REQUIRED COMPONENTS common io) # Point Cloud Library
# --- 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 ---
# 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 ---
add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})
# --- Core library: costmap_2d ---
# --- Core library: robot_costmap_2d ---
# Tạo thư viện chính
add_library(costmap_2d
add_library(robot_costmap_2d
src/costmap_2d_robot.cpp
src/array_parser.cpp
src/costmap_2d.cpp
@ -54,7 +54,7 @@ add_library(costmap_2d
)
# --- Link các thư viện phụ thuộc ---
target_link_libraries(costmap_2d
target_link_libraries(robot_costmap_2d
${Boost_LIBRARIES} # Boost
robot_std_msgs
robot_sensor_msgs
@ -65,8 +65,8 @@ target_link_libraries(costmap_2d
robot_visualization_msgs
voxel_grid
tf3
tf3_geometry_msgs
tf3_sensor_msgs
robot_tf3_geometry_msgs
robot_tf3_sensor_msgs
data_convert
robot_xmlrpcpp # XMLRPC
yaml-cpp
@ -75,7 +75,7 @@ target_link_libraries(costmap_2d
)
# --- Include directories cho target ---
target_include_directories(costmap_2d
target_include_directories(robot_costmap_2d
PUBLIC
${Boost_INCLUDE_DIRS} # Boost headers
$<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 ---
install(TARGETS costmap_2d
EXPORT costmap_2d-targets
install(TARGETS robot_costmap_2d
EXPORT robot_costmap_2d-targets
ARCHIVE DESTINATION lib # Thư viện tĩnh .a
LIBRARY DESTINATION lib # Thư viện động .so
RUNTIME DESTINATION bin # File thực thi (nếu có)
INCLUDES DESTINATION include # Cài đặt include
)
# --- Xuất export set costmap_2dTargets thành file CMake module ---
# --- Tạo file lib/cmake/costmap_2d/costmap_2dTargets.cmake ---
# --- Xuất export set robot_costmap_2dTargets thành file CMake module ---
# --- 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 ---
# --- Find_package(costmap_2d REQUIRED) ---
# --- Target_link_libraries(my_app PRIVATE costmap_2d::costmap_2d) ---
install(EXPORT costmap_2d-targets
FILE costmap_2d-targets.cmake
NAMESPACE costmap_2d::
DESTINATION lib/cmake/costmap_2d
# --- Find_package(robot_costmap_2d REQUIRED) ---
# --- Target_link_libraries(my_app PRIVATE robot_costmap_2d::robot_costmap_2d) ---
install(EXPORT robot_costmap_2d-targets
FILE robot_costmap_2d-targets.cmake
NAMESPACE robot_costmap_2d::
DESTINATION lib/cmake/robot_costmap_2d
)
# --- Cài đặt headers ---
@ -123,7 +123,7 @@ add_library(plugins
target_link_libraries(plugins
PRIVATE
costmap_2d
robot_costmap_2d
${Boost_LIBRARIES}
yaml-cpp
robot_time
@ -144,13 +144,13 @@ install(TARGETS plugins
install(EXPORT plugins-targets
FILE plugins-targets.cmake
NAMESPACE costmap_2d::
NAMESPACE robot_costmap_2d::
DESTINATION lib/cmake/plugins
)
# --- 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)
# --- Test executables ---
@ -159,8 +159,8 @@ if(BUILD_COSTMAP_TESTS)
add_executable(test_plugin test/static_layer_test.cpp)
# --- Link thư viện cho test ---
target_link_libraries(test_array_parser PRIVATE costmap_2d GTest::GTest GTest::Main pthread)
target_link_libraries(test_costmap 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 robot_costmap_2d GTest::GTest GTest::Main pthread)
target_link_libraries(test_plugin PRIVATE
${Boost_LIBRARIES}
@ -171,24 +171,24 @@ if(BUILD_COSTMAP_TESTS)
yaml-cpp
tf3
robot_time
costmap_2d
robot_costmap_2d
GTest::GTest GTest::Main
)
# --- Set RPATH để tìm thư viện của project này thay vì system ROS libraries ---
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"
INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
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/robot_costmap_2d"
LINK_FLAGS "-Wl,--disable-new-dtags"
)
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"
INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
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/robot_costmap_2d"
LINK_FLAGS "-Wl,--disable-new-dtags"
)
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"
INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
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/robot_costmap_2d"
LINK_FLAGS "-Wl,--disable-new-dtags"
)
endif()

2
config/costmap_params.yaml
View File

@ -1,4 +1,4 @@
costmap_2d:
robot_costmap_2d:
global_frame: map
robot_base_frame: base_link
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
*/
#ifndef COSTMAP_2D_ARRAY_PARSER_H
#define COSTMAP_2D_ARRAY_PARSER_H
#ifndef ROBOT_COSTMAP_2D_ARRAY_PARSER_H
#define ROBOT_COSTMAP_2D_ARRAY_PARSER_H
#include <vector>
#include <string>
namespace costmap_2d
namespace robot_costmap_2d
{
/** @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. */
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
*********************************************************************/
#ifndef COSTMAP_2D_COST_VALUES_H_
#define COSTMAP_2D_COST_VALUES_H_
#ifndef ROBOT_COSTMAP_2D_COST_VALUES_H_
#define ROBOT_COSTMAP_2D_COST_VALUES_H_
/** 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 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 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
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_2D_H_
#define COSTMAP_2D_COSTMAP_2D_H_
#ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
#include <vector>
#include <queue>
#include <robot_geometry_msgs/Point.h>
#include <boost/thread.hpp>
namespace costmap_2d
namespace robot_costmap_2d
{
// convenient for storing x/y point pairs
@ -464,6 +464,6 @@ protected:
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
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_2D_ROBOT_H_
#define COSTMAP_2D_COSTMAP_2D_ROBOT_H_
#ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/footprint.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/footprint.h>
#include <robot_geometry_msgs/Polygon.h>
#include <robot_geometry_msgs/PolygonStamped.h>
@ -49,7 +49,7 @@
#include <tf3/LinearMath/Transform.h>
#include <robot/rate.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>
@ -70,7 +70,7 @@ public:
}
};
namespace costmap_2d
namespace robot_costmap_2d
{
/** @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. */
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.
@ -275,6 +275,6 @@ private:
void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
};
// 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
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_LAYER_H_
#define COSTMAP_2D_COSTMAP_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
#define ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
namespace costmap_2d
namespace robot_costmap_2d
{
class CostmapLayer : public Layer, public Costmap2D
@ -79,7 +79,7 @@ protected:
* TrueOverwrite means every value from this layer
* 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
@ -88,7 +88,7 @@ protected:
* Overwrite means every valid value from this layer
* 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
@ -99,7 +99,7 @@ protected:
* it is overwritten. If the layer's value is NO_INFORMATION,
* 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
@ -113,7 +113,7 @@ protected:
* If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE,
* 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
@ -147,5 +147,5 @@ private:
double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_COSTMAP_LAYER_H_
} // namespace robot_costmap_2d
#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
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_MATH_H_
#define COSTMAP_2D_COSTMAP_MATH_H_
#ifndef ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
#define ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
#include <math.h>
#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);
#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_
#define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#ifndef ROBOT_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>
namespace costmap_2d
namespace robot_costmap_2d
{
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
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_FOOTPRINT_H
#define COSTMAP_2D_FOOTPRINT_H
#ifndef ROBOT_COSTMAP_2D_FOOTPRINT_H
#define ROBOT_COSTMAP_2D_FOOTPRINT_H
#include <robot_geometry_msgs/Polygon.h>
#include <robot_geometry_msgs/PolygonStamped.h>
@ -46,7 +46,7 @@
#include <robot/node_handle.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. */
// 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
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_INFLATION_LAYER_H_
#define COSTMAP_2D_INFLATION_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
#define ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <boost/thread.hpp>
namespace costmap_2d
namespace robot_costmap_2d
{
/**
* @class CellData
@ -84,7 +84,7 @@ public:
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(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()
{
return true;
@ -192,6 +192,6 @@ private:
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!!
*********************************************************************/
#ifndef COSTMAP_2D_LAYER_H_
#define COSTMAP_2D_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_LAYER_H_
#define ROBOT_COSTMAP_2D_LAYER_H_
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/utils.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/utils.h>
#include <string>
#include <tf3/buffer_core.h>
#include <robot/node_handle.h>
namespace costmap_2d
namespace robot_costmap_2d
{
class LayeredCostmap;
@ -164,6 +164,6 @@ private:
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
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
#define COSTMAP_2D_LAYERED_COSTMAP_H_
#ifndef ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
#define ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
#include <costmap_2d/cost_values.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/cost_values.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <vector>
#include <string>
namespace costmap_2d
namespace robot_costmap_2d
{
class Layer;
@ -101,7 +101,7 @@ public:
bool isTrackingUnknown()
{
return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
return costmap_.getDefaultValue() == robot_costmap_2d::NO_INFORMATION;
}
std::vector<boost::shared_ptr<Layer> >* getPlugins()
@ -172,6 +172,6 @@ private:
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
*/
#ifndef COSTMAP_2D_OBSERVATION_H_
#define COSTMAP_2D_OBSERVATION_H_
#ifndef ROBOT_COSTMAP_2D_OBSERVATION_H_
#define ROBOT_COSTMAP_2D_OBSERVATION_H_
#include <robot_geometry_msgs/Point.h>
#include <robot_sensor_msgs/PointCloud2.h>
namespace costmap_2d
namespace robot_costmap_2d
{
/**
@ -98,5 +98,5 @@ public:
double obstacle_range_, raytrace_range_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_OBSERVATION_H_
} // namespace robot_costmap_2d
#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
*********************************************************************/
#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_
#define COSTMAP_2D_OBSERVATION_BUFFER_H_
#ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
#define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
#include <vector>
#include <list>
#include <string>
#include <robot/time.h>
#include <costmap_2d/observation.h>
#include <robot_costmap_2d/observation.h>
#include <tf3/buffer_core.h>
#include <robot_sensor_msgs/PointCloud2.h>
@ -49,7 +49,7 @@
// Thread support
#include <boost/thread.hpp>
namespace costmap_2d
namespace robot_costmap_2d
{
/**
* @class ObservationBuffer
@ -150,5 +150,5 @@ private:
double obstacle_range_, raytrace_range_;
double tf_tolerance_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_OBSERVATION_BUFFER_H_
} // namespace robot_costmap_2d
#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
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_OBSTACLE_LAYER_H_
#define COSTMAP_2D_OBSTACLE_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
#define ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/footprint.h>
#include <robot_costmap_2d/costmap_layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/observation_buffer.h>
#include <robot_costmap_2d/footprint.h>
#include <robot_nav_msgs/OccupancyGrid.h>
@ -51,7 +51,7 @@
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h>
namespace costmap_2d
namespace robot_costmap_2d
{
class ObstacleLayer : public CostmapLayer
@ -66,14 +66,14 @@ public:
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);
virtual void updateCosts(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 deactivate();
virtual void reset();
// 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);
protected:
@ -103,7 +103,7 @@ protected:
* @param buffer A pointer to the observation buffer to update
*/
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
@ -111,21 +111,21 @@ protected:
* @param buffer A pointer to the observation buffer to update
*/
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
* @param marking_observations A reference to a vector that will be populated with the observations
* @return True if all the observation buffers are current, false otherwise
*/
bool getMarkingObservations(std::vector<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
* @param clearing_observations A reference to a vector that will be populated with the observations
* @return True if all the observation buffers are current, false otherwise
*/
bool getClearingObservations(std::vector<costmap_2d::Observation>& clearing_observations) const;
bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;
/**
* @brief Clear freespace based on one observation
@ -135,7 +135,7 @@ protected:
* @param max_x
* @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);
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
std::vector<boost::shared_ptr<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<costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
// Used only for testing purposes
std::vector<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 stop_receiving_data_ = false;
@ -167,6 +167,6 @@ private:
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
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_STATIC_LAYER_H_
#define COSTMAP_2D_STATIC_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_STATIC_LAYER_H_
#define ROBOT_COSTMAP_2D_STATIC_LAYER_H_
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/costmap_layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <string>
namespace costmap_2d
namespace robot_costmap_2d
{
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,
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();
protected:
@ -91,6 +91,6 @@ private:
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
#define COSTMAP_2D_UTILS_H
#ifndef ROBOT_COSTMAP_2D_UTILS_H
#define ROBOT_COSTMAP_2D_UTILS_H
#include <yaml-cpp/yaml.h>
#include <filesystem>
#include <string>
#include <iostream>
namespace costmap_2d
namespace robot_costmap_2d
{
template<typename T>
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_z
#ifndef VOXEL_GRID_COSTMAP_2D_H
#define VOXEL_GRID_COSTMAP_2D_H
#ifndef VOXEL_GRID_ROBOT_COSTMAP_2D_H
#define VOXEL_GRID_ROBOT_COSTMAP_2D_H
#include <vector>
#include <robot_std_msgs/Header.h>
#include <robot_geometry_msgs/Point32.h>
#include <robot_geometry_msgs/Vector3.h>
namespace costmap_2d
namespace robot_costmap_2d
{
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
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_VOXEL_LAYER_H_
#define COSTMAP_2D_VOXEL_LAYER_H_
#ifndef ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
#define ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/voxel_grid.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/observation_buffer.h>
#include <robot_costmap_2d/voxel_grid.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.hpp>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h>
#include <costmap_2d/obstacle_layer.h>
#include <robot_costmap_2d/obstacle_layer.h>
#include <voxel_grid/voxel_grid.h>
namespace costmap_2d
namespace robot_costmap_2d
{
class VoxelLayer : public ObstacleLayer
@ -85,7 +85,7 @@ protected:
private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(const 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);
@ -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>
<name>costmap_2d</name>
<name>robot_costmap_2d</name>
<version>0.7.10</version>
<description>
costmap_2d is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. costmap_2d
robot_costmap_2d is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. robot_costmap_2d
maintains the relationship between coordinate frames in a tree
structure buffered in time, and lets the user transform points,
vectors, etc between any two coordinate frames at any desired
@ -15,7 +15,7 @@
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<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>

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>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::CRITICAL_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::CRITICAL_SPACE;
namespace costmap_2d
namespace robot_costmap_2d
{
CriticalLayer::CriticalLayer(){}
@ -21,7 +21,7 @@ unsigned char CriticalLayer::interpretValue(unsigned char value)
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");
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 <tf3/convert.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>
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
using costmap_2d::CRITICAL_SPACE;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;
using robot_costmap_2d::CRITICAL_SPACE;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::NO_INFORMATION;
namespace costmap_2d
namespace robot_costmap_2d
{
DirectionalLayer::DirectionalLayer()
{}
@ -79,7 +79,7 @@ namespace costmap_2d
int index = getIndex(ix / 2, iy / 2);
directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff);
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;
@ -144,7 +144,7 @@ namespace costmap_2d
|| 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> y_val(my, p.pose.position.y);
@ -187,7 +187,7 @@ namespace costmap_2d
for (int i = 0; i < size_costmap; i++)
{
int8_t value;
if (costmap[i] == costmap_2d::NO_INFORMATION)
if (costmap[i] == robot_costmap_2d::NO_INFORMATION)
{
value = -1;
}
@ -211,24 +211,24 @@ namespace costmap_2d
unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot)
{
double yaw_lane;
unsigned char cost = costmap_2d::NO_INFORMATION;
unsigned char cost = robot_costmap_2d::NO_INFORMATION;
int index = getIndex(x, y);
for (auto &lane : directional_areas_[index])
{
if (lane > 359)
{
cost = std::min(cost, costmap_2d::NO_INFORMATION);
cost = std::min(cost, robot_costmap_2d::NO_INFORMATION);
}
else
{
double yaw_lane = (double)lane / 180 * M_PI;
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)
cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE);
cost = std::min(cost, robot_costmap_2d::LETHAL_OBSTACLE);
else
cost = std::min(cost, costmap_2d::NO_INFORMATION);
cost = std::min(cost, robot_costmap_2d::NO_INFORMATION);
}
}
return cost;
@ -278,10 +278,10 @@ namespace costmap_2d
for (int j = y[i].first; j <= y_max; j++)
{
int y_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
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;
else
break;
@ -290,10 +290,10 @@ namespace costmap_2d
for (int k = y[i].first; k >= y_min; k--)
{
int y_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
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;
else
break;
@ -304,10 +304,10 @@ namespace costmap_2d
for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++)
{
int x_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
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;
else
break;
@ -316,10 +316,10 @@ namespace costmap_2d
for (int k = x[i].first; k >= 0; k--)
{
int x_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
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;
else
break;
@ -332,10 +332,10 @@ namespace costmap_2d
for (int j = x[i].first; j <= x_max; j++)
{
int x_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
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;
else
break;
@ -344,10 +344,10 @@ namespace costmap_2d
for (int k = x[i].first; k >= x_min; k--)
{
int x_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
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;
else
break;
@ -358,10 +358,10 @@ namespace costmap_2d
for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++)
{
int y_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
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;
else
break;
@ -370,10 +370,10 @@ namespace costmap_2d
for (int k = y[i].first; k >= 0; k--)
{
int y_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
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;
else
break;
@ -391,7 +391,7 @@ namespace costmap_2d
for (unsigned int ix = 0; ix < size_x; ix++)
{
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!!
*********************************************************************/
#include <algorithm>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/footprint.h>
#include <robot_costmap_2d/inflation_layer.h>
#include <robot_costmap_2d/costmap_math.h>
#include <robot_costmap_2d/footprint.h>
#include <boost/thread.hpp>
#include <boost/dll/alias.hpp>
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using robot_costmap_2d::NO_INFORMATION;
namespace costmap_2d
namespace robot_costmap_2d
{
InflationLayer::InflationLayer()
@ -91,7 +91,7 @@ void InflationLayer::onInitialize()
bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
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()
{
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();
cell_inflation_radius_ = cellDistance(inflation_radius_);
computeCaches();
@ -191,7 +191,7 @@ void InflationLayer::onFootprintChanged()
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_);
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)
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
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h>
#include <robot_costmap_2d/obstacle_layer.h>
#include <robot_costmap_2d/costmap_math.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
@ -44,14 +44,14 @@
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::FREE_SPACE;
using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;
using robot_costmap_2d::ObservationBuffer;
using robot_costmap_2d::Observation;
namespace costmap_2d
namespace robot_costmap_2d
{
void ObstacleLayer::onInitialize()
@ -75,7 +75,7 @@ ObstacleLayer::~ObstacleLayer()
bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
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_)
{
setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
setConvexPolygonCost(transformed_footprint_, robot_costmap_2d::FREE_SPACE);
}
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)
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)
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>
using costmap_2d::NO_INFORMATION;
using costmap_2d::FREE_SPACE;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::PREFERRED_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::FREE_SPACE;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::PREFERRED_SPACE;
namespace costmap_2d
namespace robot_costmap_2d
{
PreferredLayer::PreferredLayer(){}

20
plugins/static_layer.cpp
View File

@ -36,20 +36,20 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h>
#include <robot_costmap_2d/static_layer.h>
#include <robot_costmap_2d/costmap_math.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>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::FREE_SPACE;
namespace costmap_2d
namespace robot_costmap_2d
{
StaticLayer::StaticLayer()
@ -75,7 +75,7 @@ void StaticLayer::onInitialize()
bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
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;
}
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_)
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)
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>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::PREFERRED_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::PREFERRED_SPACE;
namespace costmap_2d
namespace robot_costmap_2d
{
UnPreferredLayer::UnPreferredLayer(){}

20
plugins/voxel_layer.cpp
View File

@ -35,20 +35,20 @@
* Author: Eitan Marder-Eppstein
* 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 <boost/dll/alias.hpp>
#define VOXEL_BITS 16
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::FREE_SPACE;
using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;
using robot_costmap_2d::ObservationBuffer;
using robot_costmap_2d::Observation;
namespace costmap_2d
namespace robot_costmap_2d
{
void VoxelLayer::onInitialize()
@ -68,7 +68,7 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
{
try
{
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
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_)
// {
// costmap_2d::VoxelGrid grid_msg;
// robot_costmap_2d::VoxelGrid grid_msg;
// unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
// grid_msg.size_x = voxel_grid_.sizeX();
// 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)
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 <vector>
namespace costmap_2d
namespace robot_costmap_2d
{
/** @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;
}
} // end namespace costmap_2d
} // end namespace robot_costmap_2d

6
src/costmap_2d.cpp
View File

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

14
src/costmap_2d_robot.cpp
View File

@ -41,9 +41,9 @@
#include <algorithm>
#include <vector>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/utils.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/utils.h>
#include <tf3/convert.h>
#include <tf3/utils.h>
@ -53,7 +53,7 @@
using namespace std;
namespace costmap_2d
namespace robot_costmap_2d
{
Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
layered_costmap_(NULL),
@ -83,11 +83,11 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
{
try
{
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["costmap_2d"];
YAML::Node layer = config["robot_costmap_2d"];
std::string global_frame =
loadParam(layer, "global_frame", std::string("map"));
@ -567,4 +567,4 @@ void Costmap2DROBOT::getOrientedFootprint(std::vector<robot_geometry_msgs::Point
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)
@ -60,7 +60,7 @@ void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, d
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_)
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)
{
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_)
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_)
return;
@ -161,8 +161,8 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int mi
else
{
int sum = old_cost + costmap_[it];
if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
if (sum >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
master_array[it] = robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
else
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.
*/
#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)
{

10
src/footprint.cpp
View File

@ -27,15 +27,15 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <costmap_2d/costmap_math.h>
#include <robot_costmap_2d/costmap_math.h>
#include <boost/tokenizer.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <costmap_2d/footprint.h>
#include <costmap_2d/array_parser.h>
#include <robot_costmap_2d/footprint.h>
#include <robot_costmap_2d/array_parser.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)
@ -301,4 +301,4 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::X
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.
*/
#include "costmap_2d/layer.h"
#include "robot_costmap_2d/layer.h"
namespace costmap_2d
namespace robot_costmap_2d
{
Layer::Layer()
@ -53,4 +53,4 @@ const std::vector<robot_geometry_msgs::Point>& Layer::getFootprint() const
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
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/footprint.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/footprint.h>
#include <cstdio>
#include <string>
#include <algorithm>
@ -44,7 +44,7 @@
using std::vector;
namespace costmap_2d
namespace robot_costmap_2d
{
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)
{
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();
++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
*********************************************************************/
#include <costmap_2d/observation_buffer.h>
#include <robot_costmap_2d/observation_buffer.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
using namespace std;
using namespace tf3;
namespace costmap_2d
namespace robot_costmap_2d
{
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
@ -276,5 +276,5 @@ void ObservationBuffer::resetLastUpdated()
{
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 "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)
{

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
#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)
{

30
test/static_layer_test.cpp
View File

@ -1,28 +1,28 @@
#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/cost_values.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/cost_values.h>
#include <gtest/gtest.h>
#include <tf3/buffer_core.h>
#include <robot_geometry_msgs/PoseWithCovariance.h>
namespace costmap_2d {
namespace robot_costmap_2d {
class CostmapTester : public testing::Test {
public:
CostmapTester(tf3::BufferCore& tf);
void checkConsistentCosts();
void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
void compareCells(costmap_2d::Costmap2D& costmap,
void compareCellToNeighbors(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
void compareCells(robot_costmap_2d::Costmap2D& costmap,
unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
virtual void TestBody(){}
private:
costmap_2d::Costmap2DROBOT costmap_ros_;
robot_costmap_2d::Costmap2DROBOT costmap_ros_;
};
CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_params", tf){}
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
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
for(int offset_x = -1; offset_x <= 1; ++offset_x){
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
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));
unsigned char cell_cost = costmap.getCost(x, y);
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
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
double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
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);
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){
tf3::BufferCore tf(tf3::Duration(10));
map_tester = new costmap_2d::CostmapTester(tf);
map_tester = new robot_costmap_2d::CostmapTester(tf);
return(0);