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

update file costmap_2d_robot

Browse Source
This commit is contained in:
duongtd 2025-11-26 15:00:26 +07:00
parent 19a96c27d5
commit 2e0a4348dd
9 changed files with 551 additions and 1200 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored Normal file
View File

@ -0,0 +1,4 @@
# Bỏ qua thư mục build/
build/
CODE_REVIEW.md

233
CMakeLists.txt
View File

@ -1,10 +1,13 @@
# --- CMake version và project name ---
cmake_minimum_required(VERSION 3.10) cmake_minimum_required(VERSION 3.10)
project(costmap_2d) project(costmap_2d)
set(CMAKE_CXX_STANDARD 17) # --- C++ standard và position independent code ---
set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_CXX_STANDARD 17) # Sử dụng C++17
set(CMAKE_POSITION_INDEPENDENT_CODE ON) # Thư viện có thể build thành shared lib
# --- RPATH settings: ưu tiên thư viện build tại chỗ --- # --- RPATH settings: ưu tiên thư viện build tại chỗ ---
# 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}/costmap_2d")
@ -12,35 +15,24 @@ set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
# --- Dependencies --- # --- Dependencies ---
# find_package(tf3 REQUIRED) # Tìm các thư viện cần thiết
find_package(Eigen3 REQUIRED) # find_package(tf3 REQUIRED) # Nếu dùng tf3
find_package(Boost REQUIRED COMPONENTS system thread filesystem) find_package(Eigen3 REQUIRED) # Thư viện Eigen cho toán học
find_package(GTest REQUIRED) find_package(Boost REQUIRED COMPONENTS system thread filesystem) # Boost: system, thread, filesystem
find_package(PCL REQUIRED COMPONENTS common io) find_package(GTest REQUIRED) # Google Test cho unit test
# set(tf3_LIBRARY /usr/lib/libtf3.so) find_package(PCL REQUIRED COMPONENTS common io) # Point Cloud Library
find_package(xmlrpcpp REQUIRED) # XML-RPC client/server library
# --- Include other message packages if needed --- # --- Include other message packages nếu cần ---
# if (NOT TARGET sensor_msgs) # Có thể add_subdirectory nếu các package ROS msgs không có target
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build) # ví dụ sensor_msgs, geometry_msgs, nav_msgs,...
# endif() # mục đích để build mà không cần cài ROS
# if (NOT TARGET geometry_msgs)
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build)
# endif()
# if (NOT TARGET nav_msgs)
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/nav_msgs ${CMAKE_BINARY_DIR}/nav_msgs_build)
# endif()
# if (NOT TARGET map_msgs)
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build)
# endif()
# if (NOT TARGET laser_geometry)
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../laser_geometry ${CMAKE_BINARY_DIR}/laser_geometry_build)
# endif()
# if (NOT TARGET voxel_grid)
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../voxel_grid ${CMAKE_BINARY_DIR}/voxel_grid_build)
# endif()
# --- Define macro để dùng trong code ---
add_definitions(-DCOSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}") add_definitions(-DCOSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
# --- Include directories ---
# Thêm các folder chứa header files
include_directories( include_directories(
include include
${EIGEN3_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}
@ -48,11 +40,13 @@ include_directories(
${GTEST_INCLUDE_DIRS} ${GTEST_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}
) )
link_directories(${PCL_LIBRARY_DIRS}) 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}) add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})
# --- Core library --- # --- Core library: costmap_2d ---
# Tạo thư viện chính
add_library(costmap_2d add_library(costmap_2d
src/costmap_2d_robot.cpp src/costmap_2d_robot.cpp
src/array_parser.cpp src/array_parser.cpp
@ -65,140 +59,97 @@ add_library(costmap_2d
src/costmap_layer.cpp src/costmap_layer.cpp
) )
# --- Link các thư viện phụ thuộc ---
target_link_libraries(costmap_2d target_link_libraries(costmap_2d
${Boost_LIBRARIES} ${Boost_LIBRARIES} # Boost
std_msgs std_msgs # ROS msgs
sensor_msgs sensor_msgs
geometry_msgs geometry_msgs
nav_msgs nav_msgs
map_msgs map_msgs
laser_geometry laser_geometry
voxel_grid voxel_grid
# ${tf3_LIBRARY}
tf3 tf3
tf3_geometry_msgs
xmlrpcpp # XMLRPC
) )
# --- Include directories cho target ---
target_include_directories(costmap_2d target_include_directories(costmap_2d
PUBLIC ${Boost_INCLUDE_DIRS} PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> ${Boost_INCLUDE_DIRS} # Boost headers
$<INSTALL_INTERFACE:include/${PROJECT_NAME}> $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> # Khi build từ source
$<INSTALL_INTERFACE:include/${PROJECT_NAME}> # Khi install
) )
# --- Cài đặt thư viện ---
install(TARGETS costmap_2d
EXPORT costmap_2dTargets
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
)
# --- Cài đặt headers ---
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME}
)
# # --- Export CMake targets ---
# install(EXPORT costmap_2dTargets
# DESTINATION lib/cmake/costmap_2d
# )
# --- Plugin libraries --- # --- Plugin libraries ---
add_library(static_layer SHARED # Tạo các plugin shared library
plugins/static_layer.cpp add_library(static_layer SHARED plugins/static_layer.cpp)
) target_link_libraries(static_layer costmap_2d ${Boost_LIBRARIES} yaml-cpp)
target_link_libraries(static_layer
costmap_2d
${Boost_LIBRARIES}
yaml-cpp
)
add_library(obstacle_layer SHARED add_library(obstacle_layer SHARED plugins/obstacle_layer.cpp)
plugins/obstacle_layer.cpp target_link_libraries(obstacle_layer costmap_2d ${Boost_LIBRARIES} yaml-cpp)
)
target_link_libraries(obstacle_layer
costmap_2d
${Boost_LIBRARIES}
yaml-cpp
)
add_library(inflation_layer SHARED add_library(inflation_layer SHARED plugins/inflation_layer.cpp)
plugins/inflation_layer.cpp target_link_libraries(inflation_layer costmap_2d ${Boost_LIBRARIES} yaml-cpp)
)
target_link_libraries(inflation_layer
costmap_2d
${Boost_LIBRARIES}
yaml-cpp
)
add_library(voxel_layer SHARED add_library(voxel_layer SHARED plugins/voxel_layer.cpp)
plugins/voxel_layer.cpp target_link_libraries(voxel_layer costmap_2d ${Boost_LIBRARIES} yaml-cpp)
)
target_link_libraries(voxel_layer
costmap_2d
${Boost_LIBRARIES}
yaml-cpp
)
add_library(critical_layer SHARED add_library(critical_layer SHARED plugins/critical_layer.cpp)
plugins/critical_layer.cpp target_link_libraries(critical_layer costmap_2d static_layer ${Boost_LIBRARIES} yaml-cpp)
)
target_link_libraries(critical_layer
costmap_2d
static_layer
${Boost_LIBRARIES}
yaml-cpp
)
add_library(directional_layer SHARED add_library(directional_layer SHARED plugins/directional_layer.cpp)
plugins/directional_layer.cpp target_link_libraries(directional_layer costmap_2d static_layer ${Boost_LIBRARIES} yaml-cpp)
)
target_link_libraries(directional_layer
costmap_2d
static_layer
${Boost_LIBRARIES}
yaml-cpp
)
add_library(preferred_layer SHARED add_library(preferred_layer SHARED plugins/preferred_layer.cpp)
plugins/preferred_layer.cpp target_link_libraries(preferred_layer costmap_2d static_layer ${Boost_LIBRARIES} yaml-cpp)
)
target_link_libraries(preferred_layer
costmap_2d
static_layer
${Boost_LIBRARIES}
yaml-cpp
)
add_library(unpreferred_layer SHARED add_library(unpreferred_layer SHARED plugins/unpreferred_layer.cpp)
plugins/unpreferred_layer.cpp target_link_libraries(unpreferred_layer costmap_2d static_layer ${Boost_LIBRARIES} yaml-cpp)
)
target_link_libraries(unpreferred_layer
costmap_2d
static_layer
${Boost_LIBRARIES}
yaml-cpp
)
# add_library(costmap_2d_robot SHARED
# src/costmap_2d_robot.cpp
# )
# target_link_libraries(costmap_2d_robot
# costmap_2d
# ${Boost_LIBRARIES}
# yaml-cpp
# )
# --- Test executables --- # --- Option để bật/tắt test ---
add_executable(test_array_parser test/array_parser_test.cpp) option(BUILD_COSTMAP_TESTS "Build costmap_2d test executables" ON)
add_executable(test_costmap test/coordinates_test.cpp)
add_executable(test_plugin test/static_layer_test.cpp)
target_link_libraries(test_array_parser PRIVATE if(BUILD_COSTMAP_TESTS)
costmap_2d # --- Test executables ---
GTest::GTest add_executable(test_array_parser test/array_parser_test.cpp)
GTest::Main add_executable(test_costmap test/coordinates_test.cpp)
pthread add_executable(test_plugin test/static_layer_test.cpp)
)
target_link_libraries(test_costmap PRIVATE # --- Link thư viện cho test ---
costmap_2d target_link_libraries(test_array_parser PRIVATE costmap_2d GTest::GTest GTest::Main pthread)
GTest::GTest target_link_libraries(test_costmap PRIVATE costmap_2d GTest::GTest GTest::Main pthread)
GTest::Main target_link_libraries(test_plugin PRIVATE
pthread # ${tf3_LIBRARY}
) costmap_2d
${Boost_LIBRARIES}
target_link_libraries(test_plugin PRIVATE Boost::filesystem
${tf3_LIBRARY} Boost::system
costmap_2d dl
static_layer pthread
obstacle_layer yaml-cpp
inflation_layer tf3
${Boost_LIBRARIES} GTest::GTest GTest::Main
Boost::filesystem )
Boost::system endif()
dl
pthread
yaml-cpp
)

60
include/costmap_2d/costmap_2d_robot.h
View File

@ -41,27 +41,31 @@
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <costmap_2d/layer.h> #include <costmap_2d/layer.h>
#include <costmap_2d/footprint.h> #include <costmap_2d/footprint.h>
#include <geometry_msgs/Polygon.h> #include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h> #include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <tf3/LinearMath/Transform.h> #include <tf3/LinearMath/Transform.h>
#include <robot/rate.h> #include <robot/rate.h>
#include <costmap_2d/data_convert.h> #include <costmap_2d/data_convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
// class SuperValue : public XmlRpc::XmlRpcValue #include <xmlrpcpp/XmlRpcValue.h>
// { class SuperValue : public XmlRpc::XmlRpcValue
// public: {
// void setStruct(XmlRpc::XmlRpcValue::ValueStruct* a) public:
// { void setStruct(XmlRpc::XmlRpcValue::ValueStruct* a)
// _type = TypeStruct; {
// _value.asStruct = new XmlRpc::XmlRpcValue::ValueStruct(*a); _type = TypeStruct;
// } _value.asStruct = new XmlRpc::XmlRpcValue::ValueStruct(*a);
// void setArray(XmlRpc::XmlRpcValue::ValueArray* a) }
// { void setArray(XmlRpc::XmlRpcValue::ValueArray* a)
// _type = TypeArray; {
// _value.asArray = new std::vector<XmlRpc::XmlRpcValue>(*a); _type = TypeArray;
// } _value.asArray = new std::vector<XmlRpc::XmlRpcValue>(*a);
// }; }
};
namespace costmap_2d namespace costmap_2d
{ {
@ -242,40 +246,30 @@ protected:
double transform_tolerance_; ///< timeout before transform errors double transform_tolerance_; ///< timeout before transform errors
private: private:
// /** @brief Set the footprint from the new_config object. /** @brief Set the footprint from the new_config object.
// * *
// * If the values of footprint and robot_radius are the same in * If the values of footprint and robot_radius are the same in
// * new_config and old_config, nothing is changed. */ * new_config and old_config, nothing is changed. */
// void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config, void readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
// const costmap_2d::Costmap2DConfig &old_config); const std::vector<geometry_msgs::Point> &old_footprint,
const double &robot_radius);
// void loadOldParameters(ros::NodeHandle& nh); void checkMovement();
// void warnForOldParameters(ros::NodeHandle& nh);
// void checkOldParam(ros::NodeHandle& nh, const std::string &param_name);
// void copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh);
// void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
// void movementCB(const ros::TimerEvent &event);
void mapUpdateLoop(double frequency); void mapUpdateLoop(double frequency);
bool map_update_thread_shutdown_; bool map_update_thread_shutdown_;
bool stop_updates_, initialized_, stopped_; bool stop_updates_, initialized_, stopped_;
boost::thread* map_update_thread_; ///< @brief A thread for updating the map boost::thread* map_update_thread_; ///< @brief A thread for updating the map
robot::Time last_publish_; robot::Time last_publish_;
robot::Duration publish_cycle; robot::Duration publish_cycle;
// pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_;
// Costmap2DPublisher* publisher_;
// dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
boost::recursive_mutex configuration_mutex_; boost::recursive_mutex configuration_mutex_;
// ros::Subscriber footprint_sub_;
// ros::Publisher footprint_pub_;
std::vector<geometry_msgs::Point> unpadded_footprint_; std::vector<geometry_msgs::Point> unpadded_footprint_;
std::vector<geometry_msgs::Point> padded_footprint_; std::vector<geometry_msgs::Point> padded_footprint_;
float footprint_padding_; float footprint_padding_;
// costmap_2d::Costmap2DConfig old_config_;
private: private:
void getParams(); void getParams(const std::string& config_file_name);
}; };
// class Costmap2DROBOT // class Costmap2DROBOT
} // namespace costmap_2d } // namespace costmap_2d

21
include/costmap_2d/data_convert.h
View File

@ -1,5 +1,5 @@
#ifndef DATA_CONVERT_H #ifndef COSTMAP_2D_DATA_CONVERT_H
#define DATA_CONVERT_H #define COSTMAP_2D_DATA_CONVERT_H
#include <geometry_msgs/TransformStamped.h> #include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h> #include <geometry_msgs/Quaternion.h>
@ -56,7 +56,7 @@ namespace costmap_2d
return q; return q;
} }
robot::Time convertTime(tf3::Time time) inline robot::Time convertTime(tf3::Time time)
{ {
robot::Time time_tmp; robot::Time time_tmp;
time_tmp.sec = time.sec; time_tmp.sec = time.sec;
@ -64,7 +64,7 @@ namespace costmap_2d
return time_tmp; return time_tmp;
} }
tf3::Time convertTime(robot::Time time) inline tf3::Time convertTime(robot::Time time)
{ {
tf3::Time time_tmp; tf3::Time time_tmp;
time_tmp.sec = time.sec; time_tmp.sec = time.sec;
@ -72,18 +72,18 @@ namespace costmap_2d
return time_tmp; return time_tmp;
} }
tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q) inline tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q)
{ {
tf3::Quaternion out(q.x,q.y,q.z,q.w); tf3::Quaternion out(q.x,q.y,q.z,q.w);
return out; return out;
} }
geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q) inline geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
{ {
return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w()); return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
} }
tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg) inline tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
{ {
tf3::Quaternion q( tf3::Quaternion q(
msg.rotation.x, msg.rotation.x,
@ -102,7 +102,7 @@ namespace costmap_2d
return tf; return tf;
} }
tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg) inline tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg)
{ {
tf3::Quaternion q( tf3::Quaternion q(
msg.rotation.x, msg.rotation.x,
@ -121,7 +121,7 @@ namespace costmap_2d
return tf; return tf;
} }
tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
{ {
tf3::TransformStampedMsg out; tf3::TransformStampedMsg out;
out.header.seq = msg.header.seq; out.header.seq = msg.header.seq;
@ -137,7 +137,8 @@ namespace costmap_2d
out.transform.rotation.w = msg.transform.rotation.w; out.transform.rotation.w = msg.transform.rotation.w;
return out; return out;
} }
geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
{ {
geometry_msgs::TransformStamped out; geometry_msgs::TransformStamped out;
out.header.seq = msg.header.seq; out.header.seq = msg.header.seq;

46
include/costmap_2d/footprint.h
View File

@ -41,6 +41,8 @@
#include <geometry_msgs/Polygon.h> #include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h> #include <geometry_msgs/PolygonStamped.h>
#include <xmlrpcpp/XmlRpcValue.h>
#include<string.h> #include<string.h>
#include<vector> #include<vector>
@ -134,30 +136,30 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////
// /** /**
// * @brief Read the ros-params "footprint" and/or "robot_radius" from * @brief Read the ros-params "footprint" and/or "robot_radius" from
// * the given NodeHandle using searchParam() to go up the tree. * the given NodeHandle using searchParam() to go up the tree.
// */ */
// std::vector<geometry_msgs::Point> makeFootprintFromParams(std::string& nh); std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& file_name);
// /** /**
// * @brief Create the footprint from the given XmlRpcValue. * @brief Create the footprint from the given XmlRpcValue.
// * *
// * @param footprint_xmlrpc should be an array of arrays, where the * @param footprint_xmlrpc should be an array of arrays, where the
// * top-level array should have 3 or more elements, and the * top-level array should have 3 or more elements, and the
// * sub-arrays should all have exactly 2 elements (x and y * sub-arrays should all have exactly 2 elements (x and y
// * coordinates). * coordinates).
// * *
// * @param full_param_name this is the full name of the rosparam from * @param full_param_name this is the full name of the rosparam from
// * which the footprint_xmlrpc value came. It is used only for * which the footprint_xmlrpc value came. It is used only for
// * reporting errors. */ * reporting errors. */
// std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc, std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
// const std::string& full_param_name); const std::string& full_param_name);
// /** @brief Write the current unpadded_footprint_ to the "footprint" /** @brief Write the current unpadded_footprint_ to the "footprint"
// * parameter of the given NodeHandle so that dynamic_reconfigure * parameter of the given NodeHandle so that dynamic_reconfigure
// * will see the new value. */ * will see the new value. */
// void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint); void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint);
} // end namespace costmap_2d } // end namespace costmap_2d

2
plugins/static_layer.cpp
View File

@ -39,8 +39,6 @@ bool StaticLayer::getParams()
try { try {
std::string config_file_name = "config.yaml"; std::string config_file_name = "config.yaml";
std::string folder = COSTMAP_2D_DIR; std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name); std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ") if(path_source != " ")
{ {

647
src/costmap_2d_robot.cpp
View File

@ -40,7 +40,7 @@
#include <algorithm> #include <algorithm>
#include <vector> #include <vector>
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <costmap_2d/costmap_2d_robot.h> #include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/utils.h> #include <costmap_2d/utils.h>
@ -48,24 +48,10 @@
#include <tf3/utils.h> #include <tf3/utils.h>
#include <exception> #include <exception>
// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
using namespace std; using namespace std;
namespace costmap_2d namespace costmap_2d
{ {
// void move_parameter(ros::NodeHandle& old_h, ros::NodeHandle& new_h, std::string name, bool should_delete = true)
// {
// if (!old_h.hasParam(name))
// return;
// XmlRpc::XmlRpcValue value;
// old_h.getParam(name, value);
// new_h.setParam(name, value);
// if (should_delete) old_h.deleteParam(name);
// }
Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) : Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
layered_costmap_(NULL), layered_costmap_(NULL),
name_(name), name_(name),
@ -78,159 +64,144 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
map_update_thread_(NULL), map_update_thread_(NULL),
footprint_padding_(0.0) footprint_padding_(0.0)
{ {
getParams("config.yaml");
// ros::NodeHandle private_nh("~/" + name); // create a thread to handle updating the map
// ros::NodeHandle g_nh; stop_updates_ = false;
initialized_ = true;
stopped_ = false;
// // get global and robot base frame names
// private_nh.param("global_frame", global_frame_, std::string("map"));
// private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
// ros::Time last_error = ros::Time::now();
// std::string tf_error;
// // // we need to make sure that the transform between the robot base frame and the global frame is available
// while (ros::ok()
// && !tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), &tf_error))
// {
// ros::spinOnce();
// if (last_error + ros::Duration(5.0) < ros::Time::now())
// {
// ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
// robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
// last_error = ros::Time::now();
// }
// // The error string will accumulate and errors will typically be the same, so the last
// // will do for the warning above. Reset the string here to avoid accumulation.
// tf_error.clear();
// }
// // check if we want a rolling window version of the costmap
// bool rolling_window, track_unknown_space, always_send_full_costmap;
// private_nh.param("rolling_window", rolling_window, false);
// private_nh.param("track_unknown_space", track_unknown_space, false);
// private_nh.param("always_send_full_costmap", always_send_full_costmap, false);
// layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
// if (!private_nh.hasParam("plugins"))
// {
// loadOldParameters(private_nh);
// } else {
// warnForOldParameters(private_nh);
// }
// if (private_nh.hasParam("plugins"))
// {
// XmlRpc::XmlRpcValue my_list;
// private_nh.getParam("plugins", my_list);
// for (int32_t i = 0; i < my_list.size(); ++i)
// {
// std::string pname = static_cast<std::string>(my_list[i]["name"]);
// std::string type = static_cast<std::string>(my_list[i]["type"]);
// ROS_INFO("%s: Using plugin \"%s\" with type %s", name_.c_str(), pname.c_str(), type.c_str());
// try
// {
// copyParentParameters(pname, type, private_nh);
// boost::shared_ptr<costmap_2d::Layer> plugin = plugin_loader_.createInstance(type);
// layered_costmap_->addPlugin(plugin);
// plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
// }
// catch (pluginlib::PluginlibException &ex)
// {
// ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", type.c_str(), ex.what());
// }
// }
// }
// // subscribe to the footprint topic
// std::string topic_param, topic;
// if (!private_nh.searchParam("footprint_topic", topic_param))
// {
// topic_param = "footprint_topic";
// }
// private_nh.param(topic_param, topic, std::string("footprint"));
// footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROBOT::setUnpaddedRobotFootprintPolygon, this);
// if (!private_nh.searchParam("published_footprint_topic", topic_param))
// {
// topic_param = "published_footprint";
// }
// private_nh.param(topic_param, topic, std::string("footprint")); // TODO: revert to oriented_footprint in N-turtle
// footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>(topic, 1);
// setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
// publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
// always_send_full_costmap);
// // create a thread to handle updating the map
// stop_updates_ = false;
// initialized_ = true;
// stopped_ = false;
// dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
// dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); };
// dsrv_->setCallback(cb);
} }
void Costmap2DROBOT::getParams() void Costmap2DROBOT::getParams(const std::string& config_file_name)
{ {
YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml"); try
YAML::Node layer = config["static_layer"];
global_frame_ = loadParam(layer, "global_frame", std::string("map"));
robot_base_frame_ = loadParam(layer, "robot_base_frame", std::string("base_link"));
robot::Time last_error = robot::Time::now();
std::string tf_error;
// check if we want a rolling window version of the costmap
bool rolling_window, track_unknown_space, always_send_full_costmap;
rolling_window = loadParam(layer, "rolling_window", false);
track_unknown_space = loadParam(layer, "track_unknown_space", false);
always_send_full_costmap = loadParam(layer, "always_send_full_costmap", false);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
struct PluginInfo { std::string path; std::string name; };
std::vector<PluginInfo> plugins_to_load = {
{"./costmap_2d/libstatic_layer.so", "static_layer"},
{"./costmap_2d/libinflation_layer.so", "inflation_layer"},
{"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
{"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
};
// if (private_nh.hasParam("plugins"))
// {
// my_list = loadParam(layer, "plugins", my_list);
// for (int32_t i = 0; i < my_list.size(); ++i)
// {
// std::string pname = static_cast<std::string>(my_list[i]["name"]);
// std::string type = static_cast<std::string>(my_list[i]["type"]);
// printf("%s: Using plugin \"%s\" with type %s\n", name_.c_str(), pname.c_str(), type.c_str());
for (auto& info : plugins_to_load)
{ {
try std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{ {
// copyParentParameters(pname, type, private_nh); std::cout << "Path source: " << path_source << std::endl;
auto creator = boost::dll::import_alias<PluginLayerPtr()>(
info.path, "create_plugin", boost::dll::load_mode::append_decorations
);
PluginLayerPtr plugin = creator();
std::cout << "Plugin created: " << info.name << std::endl;
plugin->initialize(layered_costmap_, info.name, &tf_);
layered_costmap_->addPlugin(plugin);
} }
catch (std::exception &ex) else
{ {
printf("Failed to create the s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", ex.what()); std::cout << "/cfg folder not found!" << std::endl;
} }
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["static_layer"];
global_frame_ = loadParam(layer, "global_frame", std::string("map"));
robot_base_frame_ = loadParam(layer, "robot_base_frame", std::string("base_link"));
robot::Time last_error = robot::Time::now();
std::string tf_error;
// we need to make sure that the transform between the robot base frame and the global frame is available
while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error))
{
if (last_error + robot::Duration(5.0) < robot::Time::now())
{
printf("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
last_error = robot::Time::now();
}
// The error string will accumulate and errors will typically be the same, so the last
// will do for the warning above. Reset the string here to avoid accumulation.
tf_error.clear();
}
// check if we want a rolling window version of the costmap
bool rolling_window, track_unknown_space, always_send_full_costmap;
rolling_window = loadParam(layer, "rolling_window", false);
track_unknown_space = loadParam(layer, "track_unknown_space", false);
always_send_full_costmap = loadParam(layer, "always_send_full_costmap", false);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
struct PluginInfo { std::string path; std::string name; };
std::vector<PluginInfo> plugins_to_load = {
{"./costmap_2d/libstatic_layer.so", "static_layer"},
{"./costmap_2d/libinflation_layer.so", "inflation_layer"},
{"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
{"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
};
// if (private_nh.hasParam("plugins"))
// {
// my_list = loadParam(layer, "plugins", my_list);
// for (int32_t i = 0; i < my_list.size(); ++i)
// {
// std::string pname = static_cast<std::string>(my_list[i]["name"]);
// std::string type = static_cast<std::string>(my_list[i]["type"]);
// printf("%s: Using plugin \"%s\" with type %s\n", name_.c_str(), pname.c_str(), type.c_str());
for (auto& info : plugins_to_load)
{
try
{
// copyParentParameters(pname, type, private_nh);
auto creator = boost::dll::import_alias<PluginLayerPtr()>(
info.path, "create_plugin", boost::dll::load_mode::append_decorations
);
PluginLayerPtr plugin = creator();
std::cout << "Plugin created: " << info.name << std::endl;
plugin->initialize(layered_costmap_, info.name, &tf_);
layered_costmap_->addPlugin(plugin);
}
catch (std::exception &ex)
{
printf("Failed to create the s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", ex.what());
}
}
// setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
transform_tolerance_ = loadParam(layer, "transform_tolerance", false);
if (map_update_thread_ != NULL)
{
map_update_thread_shutdown_ = true;
map_update_thread_->join();
delete map_update_thread_;
map_update_thread_ = NULL;
}
map_update_thread_shutdown_ = false;
double map_update_frequency = loadParam(layer, "update_frequency", false);
// find size parameters
double map_width_meters = loadParam(layer, "width", false);
double map_height_meters = loadParam(layer, "height", false);
double resolution = loadParam(layer, "resolution", false);
double origin_x = loadParam(layer, "origin_x", false);
double origin_y = loadParam(layer, "origin_y", false);
if (!layered_costmap_->isSizeLocked())
{
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
}
// If the padding has changed, call setUnpaddedRobotFootprint() to
// re-apply the padding.
float footprint_padding = loadParam(layer, "footprint_padding", 0.0);
if (footprint_padding_ != footprint_padding)
{
footprint_padding_ = footprint_padding;
setUnpaddedRobotFootprint(unpadded_footprint_);
}
double robot_radius = loadParam(layer, "robot_radius", 0.0);
std::vector<geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius);
// only construct the thread if the frequency is positive
if(map_update_frequency > 0.0)
map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROBOT::mapUpdateLoop, this, map_update_frequency));
}
catch (const YAML::BadFile& e)
{
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return;
} }
// }
// }
} }
void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint) void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
@ -246,215 +217,40 @@ Costmap2DROBOT::~Costmap2DROBOT()
map_update_thread_->join(); map_update_thread_->join();
delete map_update_thread_; delete map_update_thread_;
} }
// if (publisher_ != NULL)
// delete publisher_;
delete layered_costmap_; delete layered_costmap_;
// delete dsrv_;
} }
// void Costmap2DROBOT::loadOldParameters(ros::NodeHandle& nh)
// {
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::loadOldParameters");
// ROS_WARN("%s: Parameter \"plugins\" not provided, loading pre-Hydro parameters", name_.c_str());
// bool flag;
// std::string s;
// std::vector < XmlRpc::XmlRpcValue > plugins;
// XmlRpc::XmlRpcValue::ValueStruct map; void Costmap2DROBOT::readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
// SuperValue super_map; const std::vector<geometry_msgs::Point> &old_footprint,
// SuperValue super_array; const double &robot_radius)
{
// if (nh.getParam("static_map", flag) && flag) // Only change the footprint if footprint or robot_radius has
// { // changed.
// map["name"] = XmlRpc::XmlRpcValue("static_layer"); if(new_footprint.size() == old_footprint.size())
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer"); {
// super_map.setStruct(&map); for(size_t i = 0; i < new_footprint.size(); i++)
// plugins.push_back(super_map); {
if(new_footprint[i].x != old_footprint[i].x ||
// ros::NodeHandle map_layer(nh, "static_layer"); new_footprint[i].y != old_footprint[i].y ||
// move_parameter(nh, map_layer, "map_topic"); new_footprint[i].z != old_footprint[i].z)
// move_parameter(nh, map_layer, "unknown_cost_value"); {
// move_parameter(nh, map_layer, "lethal_cost_threshold"); break;
// move_parameter(nh, map_layer, "track_unknown_space", false); }
// } if(i == (new_footprint.size()-1)) return;
}
// ros::NodeHandle obstacles(nh, "obstacle_layer"); }
// if (nh.getParam("map_type", s) && s == "voxel") if (!new_footprint.empty())
// { {
// map["name"] = XmlRpc::XmlRpcValue("obstacle_layer"); setUnpaddedRobotFootprint(new_footprint);
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer"); }
// super_map.setStruct(&map); else
// plugins.push_back(super_map); {
// robot_radius may be 0, but that must be intended at this point.
// move_parameter(nh, obstacles, "origin_z"); setUnpaddedRobotFootprint(makeFootprintFromRadius(robot_radius));
// move_parameter(nh, obstacles, "z_resolution"); }
// move_parameter(nh, obstacles, "z_voxels"); }
// move_parameter(nh, obstacles, "mark_threshold");
// move_parameter(nh, obstacles, "unknown_threshold");
// move_parameter(nh, obstacles, "publish_voxel_map");
// }
// else
// {
// map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// }
// move_parameter(nh, obstacles, "max_obstacle_height");
// move_parameter(nh, obstacles, "raytrace_range");
// move_parameter(nh, obstacles, "obstacle_range");
// move_parameter(nh, obstacles, "track_unknown_space", true);
// nh.param("observation_sources", s, std::string(""));
// std::stringstream ss(s);
// std::string source;
// while (ss >> source)
// {
// move_parameter(nh, obstacles, source);
// }
// move_parameter(nh, obstacles, "observation_sources");
// ros::NodeHandle inflation(nh, "inflation_layer");
// move_parameter(nh, inflation, "cost_scaling_factor");
// move_parameter(nh, inflation, "inflation_radius");
// map["name"] = XmlRpc::XmlRpcValue("inflation_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::InflationLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// super_array.setArray(&plugins);
// nh.setParam("plugins", super_array);
// }
// void Costmap2DROBOT::copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh)
// {
// ros::NodeHandle target_layer(nh, plugin_name);
// if(plugin_type == "costmap_2d::StaticLayer")
// {
// move_parameter(nh, target_layer, "map_topic", false);
// move_parameter(nh, target_layer, "unknown_cost_value", false);
// move_parameter(nh, target_layer, "lethal_cost_threshold", false);
// move_parameter(nh, target_layer, "track_unknown_space", false);
// }
// else if(plugin_type == "costmap_2d::VoxelLayer")
// {
// move_parameter(nh, target_layer, "origin_z", false);
// move_parameter(nh, target_layer, "z_resolution", false);
// move_parameter(nh, target_layer, "z_voxels", false);
// move_parameter(nh, target_layer, "mark_threshold", false);
// move_parameter(nh, target_layer, "unknown_threshold", false);
// move_parameter(nh, target_layer, "publish_voxel_map", false);
// }
// else if(plugin_type == "costmap_2d::ObstacleLayer")
// {
// move_parameter(nh, target_layer, "max_obstacle_height", false);
// move_parameter(nh, target_layer, "raytrace_range", false);
// move_parameter(nh, target_layer, "obstacle_range", false);
// move_parameter(nh, target_layer, "track_unknown_space", false);
// }
// else if(plugin_type == "costmap_2d::InflationLayer")
// {
// move_parameter(nh, target_layer, "cost_scaling_factor", false);
// move_parameter(nh, target_layer, "inflation_radius", false);
// }
// }
// void Costmap2DROBOT::warnForOldParameters(ros::NodeHandle& nh)
// {
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::warnForOldParameters");
// checkOldParam(nh, "static_map");
// checkOldParam(nh, "map_type");
// }
// void Costmap2DROBOT::checkOldParam(ros::NodeHandle& nh, const std::string &param_name){
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::checkOldParam");
// if(nh.hasParam(param_name)){
// ROS_WARN("%s: Pre-Hydro parameter \"%s\" unused since \"plugins\" is provided", name_.c_str(), param_name.c_str());
// }
// }
// void Costmap2DROBOT::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
// {
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::reconfigureCB");
// transform_tolerance_ = config.transform_tolerance;
// if (map_update_thread_ != NULL)
// {
// map_update_thread_shutdown_ = true;
// map_update_thread_->join();
// delete map_update_thread_;
// map_update_thread_ = NULL;
// }
// map_update_thread_shutdown_ = false;
// double map_update_frequency = config.update_frequency;
// double map_publish_frequency = config.publish_frequency;
// if (map_publish_frequency > 0)
// publish_cycle = ros::Duration(1 / map_publish_frequency);
// else
// publish_cycle = ros::Duration(-1);
// // find size parameters
// double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =
// config.origin_x,
// origin_y = config.origin_y;
// if (!layered_costmap_->isSizeLocked())
// {
// layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
// (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
// }
// // If the padding has changed, call setUnpaddedRobotFootprint() to
// // re-apply the padding.
// if (footprint_padding_ != config.footprint_padding)
// {
// footprint_padding_ = config.footprint_padding;
// setUnpaddedRobotFootprint(unpadded_footprint_);
// }
// readFootprintFromConfig(config, old_config_);
// old_config_ = config;
// // only construct the thread if the frequency is positive
// if(map_update_frequency > 0.0)
// map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROBOT::mapUpdateLoop, this, map_update_frequency));
// }
// void Costmap2DROBOT::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
// const costmap_2d::Costmap2DConfig &old_config)
// {
// // Only change the footprint if footprint or robot_radius has
// // changed. Otherwise we might overwrite a footprint sent on a
// // topic by a dynamic_reconfigure call which was setting some other
// // variable.
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::readFootprintFromConfig");
// if (new_config.footprint == old_config.footprint &&
// new_config.robot_radius == old_config.robot_radius)
// {
// return;
// }
// if (new_config.footprint != "" && new_config.footprint != "[]")
// {
// std::vector<geometry_msgs::Point> new_footprint;
// if (makeFootprintFromString(new_config.footprint, new_footprint))
// {
// setUnpaddedRobotFootprint(new_footprint);
// }
// else
// {
// ROS_ERROR("Invalid footprint string from dynamic reconfigure");
// }
// }
// else
// {
// // robot_radius may be 0, but that must be intended at this point.
// setUnpaddedRobotFootprint(makeFootprintFromRadius(new_config.robot_radius));
// }
// }
void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points) void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
{ {
@ -465,62 +261,27 @@ void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::
layered_costmap_->setFootprint(padded_footprint_); layered_costmap_->setFootprint(padded_footprint_);
} }
// void Costmap2DROBOT::movementCB(const ros::TimerEvent &event) void Costmap2DROBOT::checkMovement()
// { {
// // don't allow configuration to happen while this check occurs geometry_msgs::PoseStamped new_pose;
// // boost::recursive_mutex::scoped_lock mcl(configuration_mutex_); if (!getRobotPose(new_pose))
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::movementCB"); std::cout << "Cannot get robot pose\n";
// geometry_msgs::PoseStamped new_pose; }
// if (!getRobotPose(new_pose)) void Costmap2DROBOT::mapUpdateLoop(double frequency)
// { {
// ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling reconfiguration"); robot::Rate r(frequency);
// } while (!map_update_thread_shutdown_)
// } {
updateMap();
// void Costmap2DROBOT::mapUpdateLoop(double frequency) r.sleep();
// { // make sure to sleep for the remainder of our cycle time
// ros::NodeHandle nh; if (r.cycleTime() > robot::Duration(1 / frequency))
// ros::Rate r(frequency); printf("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds\n", frequency,
// while (nh.ok() && !map_update_thread_shutdown_) r.cycleTime().toSec());
// { }
// #ifdef HAVE_SYS_TIME_H }
// struct timeval start, end;
// double start_t, end_t, t_diff;
// gettimeofday(&start, NULL);
// #endif
// updateMap();
// #ifdef HAVE_SYS_TIME_H
// gettimeofday(&end, NULL);
// start_t = start.tv_sec + double(start.tv_usec) / 1e6;
// end_t = end.tv_sec + double(end.tv_usec) / 1e6;
// t_diff = end_t - start_t;
// ROS_DEBUG("Map update time: %.9f", t_diff);
// #endif
// if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized())
// {
// unsigned int x0, y0, xn, yn;
// layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
// publisher_->updateBounds(x0, xn, y0, yn);
// ros::Time now = ros::Time::now();
// if (last_publish_ + publish_cycle < now)
// {
// publisher_->publishCostmap();
// last_publish_ = now;
// }
// }
// ros::spinOnce();
// r.sleep();
// // make sure to sleep for the remainder of our cycle time
// if (r.cycleTime() > ros::Duration(1 / frequency))
// ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
// r.cycleTime().toSec());
// }
// }
void Costmap2DROBOT::updateMap() void Costmap2DROBOT::updateMap()
{ {
@ -534,12 +295,10 @@ void Costmap2DROBOT::updateMap()
y = pose.pose.position.y, y = pose.pose.position.y,
yaw = getYaw(pose.pose.orientation); yaw = getYaw(pose.pose.orientation);
layered_costmap_->updateMap(x, y, yaw); layered_costmap_->updateMap(x, y, yaw);
// geometry_msgs::PolygonStamped footprint; geometry_msgs::PolygonStamped footprint;
// footprint.header.frame_id = global_frame_; footprint.header.frame_id = global_frame_;
// footprint.header.stamp = ros::Time::now(); footprint.header.stamp = robot::Time::now();
// transformFootprint(x, y, yaw, padded_footprint_, footprint); transformFootprint(x, y, yaw, padded_footprint_, footprint);
// footprint_pub_.publish(footprint);
initialized_ = true; initialized_ = true;
} }
@ -565,9 +324,17 @@ void Costmap2DROBOT::start()
// block until the costmap is re-initialized.. meaning one update cycle has run // block until the costmap is re-initialized.. meaning one update cycle has run
// note: this does not hold, if the user has disabled map-updates allgother // note: this does not hold, if the user has disabled map-updates allgother
// robot::Rate r(100.0); robot::Time start_time = robot::Time::now();
// while (ros::ok() && !initialized_ && map_update_thread_) robot::Rate r(100.0);
// r.sleep(); while (!initialized_ && map_update_thread_)
{
if (robot::Time::now() - start_time > robot::Duration(5.0))
{
printf("Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
break;
}
r.sleep();
}
} }
void Costmap2DROBOT::stop() void Costmap2DROBOT::stop()
@ -598,9 +365,18 @@ void Costmap2DROBOT::resume()
stop_updates_ = false; stop_updates_ = false;
// block until the costmap is re-initialized.. meaning one update cycle has run // block until the costmap is re-initialized.. meaning one update cycle has run
// robot::Rate r(100.0); robot::Time start_time = robot::Time::now();
// while (!initialized_) robot::Rate r(100.0);
// r.sleep(); while (!initialized_)
{
if (robot::Time::now() - start_time > robot::Duration(5.0))
{
printf("Costmap2DROBOT::resume() timed out waiting for costmap to initialize\n");
break;
}
r.sleep();
}
} }
@ -619,10 +395,11 @@ void Costmap2DROBOT::resetLayers()
bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
{ {
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::getRobotPose");
// tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
geometry_msgs::PoseStamped robot_pose; geometry_msgs::PoseStamped robot_pose;
// tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); geometry_msgs::Pose pose_default;
global_pose.pose = pose_default;
robot_pose.pose = pose_default;
robot_pose.header.frame_id = robot_base_frame_; robot_pose.header.frame_id = robot_base_frame_;
robot_pose.header.stamp = robot::Time(); robot_pose.header.stamp = robot::Time();
robot::Time current_time = robot::Time::now(); // save time for checking tf delay later robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
@ -634,12 +411,18 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
if (tf_.canTransform(global_frame_, robot_base_frame_, convertTime(current_time))) if (tf_.canTransform(global_frame_, robot_base_frame_, convertTime(current_time)))
{ {
tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, convertTime(current_time)); tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, convertTime(current_time));
// tf3::doTransform(robot_pose, global_pose, transform); tf3::doTransform(robot_pose, global_pose, transform);
} }
// use the latest otherwise // use the latest otherwise
else else
{ {
// tf_.transform(robot_pose, global_pose, global_frame_); // tf_.transform(robot_pose, global_pose, global_frame_);
tf3::TransformStampedMsg transform = tf_.lookupTransform(
global_frame_, // frame đích
robot_base_frame_, // frame nguồn
tf3::convertTime(robot_pose.header.stamp)
);
tf3::doTransform(robot_pose, global_pose, transform);
} }
} }
catch (tf3::LookupException& ex) catch (tf3::LookupException& ex)

206
src/footprint.cpp
View File

@ -208,117 +208,117 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh) std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& file_name)
// { {
// std::string full_param_name; std::string full_param_name;
// std::string full_radius_param_name; std::string full_radius_param_name;
// std::vector<geometry_msgs::Point> points; std::vector<geometry_msgs::Point> points;
// if (nh.searchParam("footprint", full_param_name)) // if (nh.searchParam("footprint", full_param_name))
// { // {
// XmlRpc::XmlRpcValue footprint_xmlrpc; // XmlRpc::XmlRpcValue footprint_xmlrpc;
// nh.getParam(full_param_name, footprint_xmlrpc); // nh.getParam(full_param_name, footprint_xmlrpc);
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString && // if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]") // footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
// { // {
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points)) // if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
// { // {
// writeFootprintToParam(nh, points); // writeFootprintToParam(nh, points);
// return points; // return points;
// } // }
// } // }
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) // else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
// { // {
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name); // points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
// writeFootprintToParam(nh, points); // writeFootprintToParam(nh, points);
// return points; // return points;
// } // }
// } // }
// if (nh.searchParam("robot_radius", full_radius_param_name)) // if (nh.searchParam("robot_radius", full_radius_param_name))
// { // {
// double robot_radius; // double robot_radius;
// nh.param(full_radius_param_name, robot_radius, 1.234); // nh.param(full_radius_param_name, robot_radius, 1.234);
// points = makeFootprintFromRadius(robot_radius); // points = makeFootprintFromRadius(robot_radius);
// nh.setParam("robot_radius", robot_radius); // nh.setParam("robot_radius", robot_radius);
// } // }
// // Else neither param was found anywhere this knows about, so // // Else neither param was found anywhere this knows about, so
// // defaults will come from dynamic_reconfigure stuff, set in // // defaults will come from dynamic_reconfigure stuff, set in
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB(). // // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
// return points; // return points;
// } }
// void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint) void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint)
// { {
// std::ostringstream oss; // std::ostringstream oss;
// bool first = true; // bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++) // for (unsigned int i = 0; i < footprint.size(); i++)
// { // {
// geometry_msgs::Point p = footprint[ i ]; // geometry_msgs::Point p = footprint[ i ];
// if (first) // if (first)
// { // {
// oss << "[[" << p.x << "," << p.y << "]"; // oss << "[[" << p.x << "," << p.y << "]";
// first = false; // first = false;
// } // }
// else // else
// { // {
// oss << ",[" << p.x << "," << p.y << "]"; // oss << ",[" << p.x << "," << p.y << "]";
// } // }
// } // }
// oss << "]"; // oss << "]";
// nh.setParam("footprint", oss.str().c_str()); // nh.setParam("footprint", oss.str().c_str());
// } }
// double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name) double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
// { {
// // Make sure that the value we're looking at is either a double or an int. // Make sure that the value we're looking at is either a double or an int.
// if (value.getType() != XmlRpc::XmlRpcValue::TypeInt && if (value.getType() != XmlRpc::XmlRpcValue::TypeInt &&
// value.getType() != XmlRpc::XmlRpcValue::TypeDouble) value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
// { {
// std::string& value_string = value; std::string& value_string = value;
// std::printf("Values in the footprint specification (param %s) must be numbers. Found value %s.", std::printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
// full_param_name.c_str(), value_string.c_str()); full_param_name.c_str(), value_string.c_str());
// throw std::runtime_error("Values in the footprint specification must be numbers"); throw std::runtime_error("Values in the footprint specification must be numbers\n");
// } }
// return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
// } }
// std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc, std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
// const std::string& full_param_name) const std::string& full_param_name)
// { {
// // Make sure we have an array of at least 3 elements. // Make sure we have an array of at least 3 elements.
// if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray || if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
// footprint_xmlrpc.size() < 3) footprint_xmlrpc.size() < 3)
// { {
// std::printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", std::printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
// full_param_name.c_str(), std::string(footprint_xmlrpc).c_str()); full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
// throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least " throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
// "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]\n");
// } }
// std::vector<geometry_msgs::Point> footprint; std::vector<geometry_msgs::Point> footprint;
// geometry_msgs::Point pt; geometry_msgs::Point pt;
// for (int i = 0; i < footprint_xmlrpc.size(); ++i) for (int i = 0; i < footprint_xmlrpc.size(); ++i)
// { {
// // Make sure each element of the list is an array of size 2. (x and y coordinates) // Make sure each element of the list is an array of size 2. (x and y coordinates)
// XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ]; XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
// if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
// point.size() != 2) point.size() != 2)
// { {
// std::printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: " std::printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
// "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.", "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.\n",
// full_param_name.c_str()); full_param_name.c_str());
// throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: " throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "
// "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form\n");
// } }
// pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name); pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);
// pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name); pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name);
// footprint.push_back(pt); footprint.push_back(pt);
// } }
// return footprint; return footprint;
// } }
} // end namespace costmap_2d } // end namespace costmap_2d

540
test/static_layer_test.cpp
View File

@ -1,472 +1,90 @@
#include <boost/dll/import.hpp> #include <costmap_2d/costmap_2d_robot.h>
#include <iostream> #include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_layer.h> #include <gtest/gtest.h>
#include <costmap_2d/layered_costmap.h>
#include <tf3/buffer_core.h> #include <tf3/buffer_core.h>
#include <tf3/time.h>
#include "nav_msgs/OccupancyGrid.h"
#include <map_msgs/OccupancyGridUpdate.h>
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/critical_layer.h>
#include <costmap_2d/preferred_layer.h>
using namespace costmap_2d;
int main(int argc, char* argv[]) { namespace costmap_2d {
std::string global_frame = "map"; class CostmapTester : public testing::Test {
bool rolling_window = false; public:
bool track_unknown = false; CostmapTester(tf3::BufferCore& tf);
LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown); void checkConsistentCosts();
void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
void compareCells(costmap_2d::Costmap2D& costmap,
unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
virtual void TestBody(){}
// ✅ FIX 1: RESIZE costmap TRƯỚC KHI sử dụng private:
// Costmap mặc định có size = 0x0, resolution = 0.0 -> sẽ gây crash costmap_2d::Costmap2DROBOT costmap_ros_;
layered_costmap.resizeMap( };
100, // size_x: số ô theo chiều X
100, // size_y: số ô theo chiều Y
0.05, // resolution: 0.05m/ô (5cm)
0.0, // origin_x
0.0, // origin_y
false // size_locked
);
std::cout << "Costmap resized to: "
<< layered_costmap.getCostmap()->getSizeInCellsX()
<< "x" << layered_costmap.getCostmap()->getSizeInCellsY()
<< ", resolution: " << layered_costmap.getCostmap()->getResolution()
<< "m/cell" << std::endl;
tf3::BufferCore tf_buffer; CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("test_costmap", tf){}
// // ✅ FIX 2: Lưu tất cả plugins để tránh bị destroy void CostmapTester::checkConsistentCosts(){
// std::vector<costmap_2d::PluginLayerPtr> plugin_instances; costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
// // // ✅ FIX 3: Load và add static_layer //get a copy of the costmap contained by our ros wrapper
// // auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>( costmap->saveMap("costmap_test.pgm");
// // "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// // );
// // costmap_2d::PluginLayerPtr plugin1 = creator();
// // std::cout << "Plugin created static_layer successfully" << std::endl;
// // plugin1->initialize(&layered_costmap, "static_layer", &tf_buffer);
// // layered_costmap.addPlugin(plugin1); // ✅ FIX: ADD vào LayeredCostmap
// // plugin_instances.push_back(plugin1); // ✅ FIX: Lưu lại để tránh bị destroy
// // std::cout << "Static layer initialized and added. Use count: " << plugin1.use_count() << std::endl;
//loop through the costmap and check for any unexpected drop-offs in costs
// // // ✅ FIX 5: Load và add obstacle_layer for(unsigned int i = 0; i < costmap->getSizeInCellsX(); ++i){
// // auto creator2 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>( for(unsigned int j = 0; j < costmap->getSizeInCellsY(); ++j){
// // "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations compareCellToNeighbors(*costmap, i, j);
// // );
// // costmap_2d::PluginLayerPtr plugin3 = creator2();
// // std::cout << "Plugin created obstacle_layer successfully" << std::endl;
// // plugin3->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
// // layered_costmap.addPlugin(plugin3); // ✅ FIX: ADD vào LayeredCostmap
// // plugin_instances.push_back(plugin3); // ✅ FIX: Lưu lại
// // std::cout << "Obstacle layer initialized and added. Use count: " << plugin3.use_count() << std::endl;
// // ✅ FIX 4: Load và add inflation_layer
// auto creator1 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// costmap_2d::PluginLayerPtr plugin2 = creator1();
// std::cout << "✅ Plugin created inflation_layer successfully" << std::endl;
// plugin2->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
// plugin2->onFootprintChanged(); // ✅ FIX: Cần gọi cho inflation_layer
// layered_costmap.addPlugin(plugin2); // ✅ FIX: ADD vào LayeredCostmap
// plugin_instances.push_back(plugin2); // ✅ FIX: Lưu lại
// std::cout << "Inflation layer initialized and added. Use count: " << plugin2.use_count() << std::endl;
// auto creator3 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// costmap_2d::PluginLayerPtr plugin4 = creator3();
// std::cout << "Plugin created preferred_layer successfully" << std::endl;
// plugin4->initialize(&layered_costmap, "preferred_layer", &tf_buffer);
// layered_costmap.addPlugin(plugin4); // ✅ FIX: ADD vào LayeredCostmap
// plugin_instances.push_back(plugin4); // ✅ FIX: Lưu lại
// // ✅ Kiểm tra số lượng plugins
// std::cout << "Total plugins added: " << layered_costmap.getPlugins()->size() << std::endl;
// // ✅ Test updateMap (nếu cần)
// // layered_costmap.updateMap(0.0, 0.0, 0.0); // robot_x, robot_y, robot_yaw
// std::cout << "All plugins initialized and added successfully!" << std::endl;
// // ✅ FIX: Xóa reference đến layered_costmap trong các plugins trước khi destroy
// // Để tránh destructor của plugin cố gắng truy cập layered_costmap đã bị destroy
// // for (auto& plugin : plugin_instances) {
// // // Reset pointer để tránh truy cập vào object đã bị destroy
// // // Lưu ý: Cần kiểm tra xem Layer có hàm reset pointer không
// // // Nếu không có, cần thêm vào Layer class
// // }
// // ✅ FIX: Clear plugin_instances TRƯỚC KHI layered_costmap bị destroy
// // Thứ tự destroy: plugin_instances -> plugins (trong shared_ptr) -> destructor của plugin
// plugin_instances.clear();
// std::cout << "Plugin instances cleared" << std::endl;
// // std::cout << "=== Costmap Plugin Test ===" << std::endl;
// // // 1️⃣ Khởi tạo LayeredCostmap
// // std::string global_frame = "map";
// // bool rolling_window = true;
// // bool track_unknown = true;
// // LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
// // // ✅ Resize map trước khi init plugin
// // layered_costmap.resizeMap(
// // 100, // size_x
// // 100, // size_y
// // 0.05, // resolution (m/cell)
// // 0.0, // origin_x
// // 0.0, // origin_y
// // false // size_locked
// // );
// // std::cout << "Costmap resized to: "
// // << layered_costmap.getCostmap()->getSizeInCellsX()
// // << "x" << layered_costmap.getCostmap()->getSizeInCellsY()
// // << ", resolution: " << layered_costmap.getCostmap()->getResolution()
// // << " m/cell" << std::endl;
// // // 2️⃣ TF buffer
// // tf3::BufferCore tf_buffer;
// // // 3️⃣ Thiết lập footprint robot trước khi load inflation_layer
// // std::vector<geometry_msgs::Point> footprint = {
// // {0.1, 0.1, 0.0},
// // {0.1, -0.1, 0.0},
// // {-0.1, -0.1, 0.0},
// // {-0.1, 0.1, 0.0}
// // };
// // // layered_costmap.setUnpaddedRobotFootprint(footprint);
// 4️⃣ Danh sách plugin để load (thứ tự an toàn)
struct PluginInfo { std::string path; std::string name; };
std::vector<PluginInfo> plugins_to_load = {
{"./costmap_2d/libstatic_layer.so", "static_layer"},
{"./costmap_2d/libinflation_layer.so", "inflation_layer"},
{"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
{"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
};
// // // 5️⃣ Vector lưu shared_ptr plugin
std::vector<PluginLayerPtr> plugin_instances;
for (auto& info : plugins_to_load)
{
auto creator = boost::dll::import_alias<PluginLayerPtr()>(
info.path, "create_plugin", boost::dll::load_mode::append_decorations
);
PluginLayerPtr plugin = creator();
std::cout << "Plugin created: " << info.name << std::endl;
plugin->initialize(&layered_costmap, info.name, &tf_buffer);
// Nếu là inflation_layer, cần gọi onFootprintChanged()
if(info.name == "inflation_layer")
{
plugin->onFootprintChanged();
std::cout << "Inflation layer footprint updated." << std::endl;
}
plugin_instances.push_back(plugin);
layered_costmap.addPlugin(plugin);
std::cout << info.name << " initialized, use_count=" << plugin.use_count() << std::endl;
} }
}
std::cout << "All plugins loaded successfully." << std::endl;
// 6️⃣ Test update map với dummy grid
// nav_msgs::OccupancyGrid grid;
// grid.info.width = 10;
// grid.info.height = 10;
// grid.info.resolution = 0.05;
// grid.data.resize(grid.info.width * grid.info.height, 0); // free space
// for(auto& plugin : plugin_instances)
// {
// plugin->dataCallBack(grid, "map");
// }
// std::cout << "Map update simulated." << std::endl;
// // // 7️⃣ Clear plugin_instances trước khi LayeredCostmap bị destroy
// // plugin_instances.clear();
// // std::cout << "Plugin instances cleared safely." << std::endl;
// // return 0;
// auto creator1 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// costmap_2d::PluginLayerPtr plugin1 = creator1();
// std::cout << "Plugin created obstacle_layer successfully" << std::endl;
// plugin1->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
// layered_costmap.addPlugin(plugin1);
// auto creator2 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// costmap_2d::PluginLayerPtr plugin2 = creator2();
// std::cout << "Plugin created static_layer successfully" << std::endl;
// plugin2->initialize(&layered_costmap, "static_layer", &tf_buffer);
// layered_costmap.addPlugin(plugin2);
// auto creator3 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// costmap_2d::PluginLayerPtr plugin3 = creator3();
// std::cout << "Plugin created inflation_layer successfully" << std::endl;
// plugin3->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
// layered_costmap.addPlugin(plugin3);
// auto creator4 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// costmap_2d::PluginLayerPtr plugin4 = creator4();
// std::cout << "Plugin created preferred_layer successfully" << std::endl;
// plugin4->initialize(&layered_costmap, "preferred_layer", &tf_buffer);
// layered_costmap.addPlugin(plugin4);
// while(true)
// {
robot::Duration(10).sleep();
// }
return 0;
} }
void CostmapTester::compareCellToNeighbors(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){
int nx = x + offset_x;
int ny = y + offset_y;
// auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations //check to make sure that the neighbor cell is a legal one
// ); if(nx >= 0 && nx < (int)costmap.getSizeInCellsX() && ny >=0 && ny < (int)costmap.getSizeInCellsY()){
// costmap_2d::PluginLayerPtr plugin = creator(); compareCells(costmap, x, y, nx, ny);
// std::cout << "Plugin created obstacle_layer successfully" << std::endl; }
// plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer); }
}
}
// creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations //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){
// plugin = creator(); double cell_distance = hypot(static_cast<int>(x-nx), static_cast<int>(y-ny));
// std::cout << "Plugin created static_layer successfully" << std::endl;
// plugin->initialize(&layered_costmap, "static_layer", &tf_buffer); unsigned char cell_cost = costmap.getCost(x, y);
unsigned char neighbor_cost = costmap.getCost(nx, ny);
// creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>( if(cell_cost == costmap_2d::LETHAL_OBSTACLE){
// "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations //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);
// plugin = creator(); EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == costmap_2d::FREE_SPACE));
// std::cout << "Plugin created inflation_layer successfully" << std::endl; }
// plugin->initialize(&layered_costmap, "inflation_layer", &tf_buffer); else if(cell_cost == 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;
// creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>( unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
// "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations if(neighbor_cost < expected_lowest_cost){
// ); printf("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
// plugin = creator(); x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
// std::cout << "Plugin created preferred_layer successfully" << std::endl; printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
// plugin->initialize(&layered_costmap, "preferred_layer", &tf_buffer); 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));
}
}
};
// #include <boost/dll/import.hpp> costmap_2d::CostmapTester* map_tester = NULL;
// #include <iostream>
// #include <costmap_2d/costmap_layer.h>
// #include <costmap_2d/layered_costmap.h> int main(int argc, char** argv){
// #include <tf3/buffer_core.h>
// #include <tf3/time.h> tf3::BufferCore tf(tf3::Duration(10));
// #include "nav_msgs/OccupancyGrid.h"
// #include <map_msgs/OccupancyGridUpdate.h> map_tester = new costmap_2d::CostmapTester(tf);
// #include <costmap_2d/obstacle_layer.h>
// #include <costmap_2d/inflation_layer.h>
// #include <costmap_2d/critical_layer.h> return(0);
// #include <costmap_2d/preferred_layer.h> }
// using namespace costmap_2d;
// int main(int argc, char* argv[]) {
// auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// costmap_2d::PluginLayerPtr plugin = creator();
// std::cout << "Plugin created static_layer successfully" << std::endl;
// std::string global_frame = "map";
// bool rolling_window = true;
// bool track_unknown = true;
// LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
// tf3::BufferCore tf_buffer;
// // tf3::Duration cache_time(10.0);
// // tf3::BufferCore tf_buffer(cache_time);
// plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);
// std::cout << "Plugin initialized" << std::endl;
// // plugin->activate();
// nav_msgs::OccupancyGrid grid;
// grid.info.resolution = 0.05f;
// grid.info.width = 2;
// grid.info.height = 2;
// grid.data.resize(grid.info.width * grid.info.height, -1);
// grid.data[0] = 0;
// grid.data[1] = 100;
// grid.data[2] = 10;
// grid.data[3] = 0;
// plugin->dataCallBack(grid, "map");
// map_msgs::OccupancyGridUpdate update;
// update.x = 1;
// update.y = 1;
// update.width = 2;
// update.height = 2;
// update.data.resize(update.width * update.height, -1);
// update.data[0] = 0;
// update.data[1] = 100;
// update.data[2] = 10;
// update.data[3] = 0;
// plugin->dataCallBack(update, "map_update");
// // creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// // "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// // );
// // plugin = creator();
// // std::cout << "Plugin created obstacle_layer successfully" << std::endl;
// // plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
// // sensor_msgs::LaserScan scan;
// // // --- Header ---
// // scan.header.seq = 1;
// // scan.header.stamp = robot::Time::now(); // nếu bạn chưa có Time class, gán tạm số
// // scan.header.frame_id = "laser_frame";
// // // --- Cấu hình góc ---
// // scan.angle_min = -M_PI; // -180 độ
// // scan.angle_max = M_PI; // +180 độ
// // scan.angle_increment = M_PI / 180; // 1 độ mỗi tia
// // // --- Cấu hình thời gian ---
// // scan.time_increment = 0.0001f; // mỗi tia cách nhau 0.1 ms
// // scan.scan_time = 0.04f; // tổng thời gian quét 40 ms (~25Hz)
// // // --- Giới hạn đo ---
// // scan.range_min = 0.1f;
// // scan.range_max = 10.0f;
// // // --- Tạo dữ liệu ---
// // int num_readings = static_cast<int>((scan.angle_max - scan.angle_min) / scan.angle_increment);
// // scan.ranges.resize(num_readings);
// // scan.intensities.resize(num_readings);
// // for (int i = 0; i < num_readings; ++i)
// // {
// // float angle = scan.angle_min + i * scan.angle_increment;
// // // Tạo dữ liệu giả lập: vật thể cách 2m, dao động nhẹ theo góc
// // scan.ranges[i] = 2.0f + 0.2f * std::sin(3.0f * angle);
// // // Giả lập cường độ phản xạ
// // scan.intensities[i] = 100.0f + 20.0f * std::cos(angle);
// // }
// // // --- In thử 10 giá trị đầu ---
// // // for (int i = 0; i < 10; ++i)
// // // {
// // // std::cout << "Angle " << (scan.angle_min + i * scan.angle_increment)
// // // << " rad | Range " << scan.ranges[i]
// // // << " m | Intensity " << scan.intensities[i]
// // // << std::endl;
// // // }
// // // plugin->deactivate();
// // plugin->dataCallBack(scan, "laser_valid");
// // sensor_msgs::PointCloud cloud;
// // // 2. Thiết lập header
// // cloud.header.stamp.sec = 1234567890; // hoặc ros::Time::now().sec
// // cloud.header.stamp.nsec = 0;
// // cloud.header.frame_id = "laser_frame";
// // // 3. Thêm một vài điểm
// // geometry_msgs::Point32 pt1;
// // pt1.x = 1.0f; pt1.y = 0.0f; pt1.z = 0.0f;
// // geometry_msgs::Point32 pt2;
// // pt2.x = 0.0f; pt2.y = 1.0f; pt2.z = 0.0f;
// // geometry_msgs::Point32 pt3;
// // pt3.x = 1.0f; pt3.y = 1.0f; pt3.z = 0.0f;
// // cloud.points.push_back(pt1);
// // cloud.points.push_back(pt2);
// // cloud.points.push_back(pt3);
// // // 4. Thêm dữ liệu channels (tùy chọn)
// // cloud.channels.resize(1);
// // cloud.channels[0].name = "intensity";
// // cloud.channels[0].values.push_back(0.5f);
// // cloud.channels[0].values.push_back(1.0f);
// // cloud.channels[0].values.push_back(0.8f);
// // plugin->dataCallBack(cloud, "pcl_cb");
// std::cout << "Plugin activated successfully" << std::endl;
// creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libinflation_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// plugin = creator();
// std::cout << "Plugin created inflation_layer successfully" << std::endl;
// plugin->initialize(&layered_costmap, "inflation_layer", &tf_buffer);
// plugin->onFootprintChanged();
// std::cout << "Plugin initialized" << std::endl;
// creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libpreferred_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// plugin = creator();
// std::cout << "Plugin created preferred_layer successfully" << std::endl;
// plugin->initialize(&layered_costmap, "preferred_layer", &tf_buffer);
// plugin->dataCallBack(grid, "map");
// creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// plugin = creator();
// std::cout << "Plugin created obstacle_layer successfully" << std::endl;
// plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer);
// std::cout << "Plugin initialized" << std::endl;
// }