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

60
include/costmap_2d/costmap_2d_robot.h
View File

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

21
include/costmap_2d/data_convert.h
View File

@ -1,5 +1,5 @@
#ifndef DATA_CONVERT_H
#define DATA_CONVERT_H
#ifndef COSTMAP_2D_DATA_CONVERT_H
#define COSTMAP_2D_DATA_CONVERT_H
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
@ -56,7 +56,7 @@ namespace costmap_2d
return q;
}
robot::Time convertTime(tf3::Time time)
inline robot::Time convertTime(tf3::Time time)
{
robot::Time time_tmp;
time_tmp.sec = time.sec;
@ -64,7 +64,7 @@ namespace costmap_2d
return time_tmp;
}
tf3::Time convertTime(robot::Time time)
inline tf3::Time convertTime(robot::Time time)
{
tf3::Time time_tmp;
time_tmp.sec = time.sec;
@ -72,18 +72,18 @@ namespace costmap_2d
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);
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());
}
tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
inline tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
{
tf3::Quaternion q(
msg.rotation.x,
@ -102,7 +102,7 @@ namespace costmap_2d
return tf;
}
tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg)
inline tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg)
{
tf3::Quaternion q(
msg.rotation.x,
@ -121,7 +121,7 @@ namespace costmap_2d
return tf;
}
tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
{
tf3::TransformStampedMsg out;
out.header.seq = msg.header.seq;
@ -137,7 +137,8 @@ namespace costmap_2d
out.transform.rotation.w = msg.transform.rotation.w;
return out;
}
geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
{
geometry_msgs::TransformStamped out;
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/PolygonStamped.h>
#include <xmlrpcpp/XmlRpcValue.h>
#include<string.h>
#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
// * the given NodeHandle using searchParam() to go up the tree.
// */
// std::vector<geometry_msgs::Point> makeFootprintFromParams(std::string& nh);
/**
* @brief Read the ros-params "footprint" and/or "robot_radius" from
* the given NodeHandle using searchParam() to go up the tree.
*/
std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& file_name);
// /**
// * @brief Create the footprint from the given XmlRpcValue.
// *
// * @param footprint_xmlrpc should be an array of arrays, where the
// * top-level array should have 3 or more elements, and the
// * sub-arrays should all have exactly 2 elements (x and y
// * coordinates).
// *
// * @param full_param_name this is the full name of the rosparam from
// * which the footprint_xmlrpc value came. It is used only for
// * reporting errors. */
// std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc,
// const std::string& full_param_name);
/**
* @brief Create the footprint from the given XmlRpcValue.
*
* @param footprint_xmlrpc should be an array of arrays, where the
* top-level array should have 3 or more elements, and the
* sub-arrays should all have exactly 2 elements (x and y
* coordinates).
*
* @param full_param_name this is the full name of the rosparam from
* which the footprint_xmlrpc value came. It is used only for
* reporting errors. */
std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name);
// /** @brief Write the current unpadded_footprint_ to the "footprint"
// * parameter of the given NodeHandle so that dynamic_reconfigure
// * will see the new value. */
// void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint);
/** @brief Write the current unpadded_footprint_ to the "footprint"
* parameter of the given NodeHandle so that dynamic_reconfigure
* will see the new value. */
void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint);
} // end namespace costmap_2d

2
plugins/static_layer.cpp
View File

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

647
src/costmap_2d_robot.cpp
View File

@ -40,7 +40,7 @@
#include <algorithm>
#include <vector>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/utils.h>
@ -48,24 +48,10 @@
#include <tf3/utils.h>
#include <exception>
// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
using namespace std;
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) :
layered_costmap_(NULL),
name_(name),
@ -78,159 +64,144 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
map_update_thread_(NULL),
footprint_padding_(0.0)
{
getParams("config.yaml");
// ros::NodeHandle private_nh("~/" + name);
// ros::NodeHandle g_nh;
// // 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);
// create a thread to handle updating the map
stop_updates_ = false;
initialized_ = true;
stopped_ = false;
}
void Costmap2DROBOT::getParams()
void Costmap2DROBOT::getParams(const std::string& config_file_name)
{
YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml");
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
{
try
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{
// 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);
std::cout << "Path source: " << path_source << std::endl;
}
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)
@ -246,215 +217,40 @@ Costmap2DROBOT::~Costmap2DROBOT()
map_update_thread_->join();
delete map_update_thread_;
}
// if (publisher_ != NULL)
// delete publisher_;
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;
// SuperValue super_map;
// SuperValue super_array;
// if (nh.getParam("static_map", flag) && flag)
// {
// map["name"] = XmlRpc::XmlRpcValue("static_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// ros::NodeHandle map_layer(nh, "static_layer");
// move_parameter(nh, map_layer, "map_topic");
// move_parameter(nh, map_layer, "unknown_cost_value");
// move_parameter(nh, map_layer, "lethal_cost_threshold");
// move_parameter(nh, map_layer, "track_unknown_space", false);
// }
// ros::NodeHandle obstacles(nh, "obstacle_layer");
// if (nh.getParam("map_type", s) && s == "voxel")
// {
// map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// move_parameter(nh, obstacles, "origin_z");
// 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::readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
const std::vector<geometry_msgs::Point> &old_footprint,
const double &robot_radius)
{
// Only change the footprint if footprint or robot_radius has
// changed.
if(new_footprint.size() == old_footprint.size())
{
for(size_t i = 0; i < new_footprint.size(); i++)
{
if(new_footprint[i].x != old_footprint[i].x ||
new_footprint[i].y != old_footprint[i].y ||
new_footprint[i].z != old_footprint[i].z)
{
break;
}
if(i == (new_footprint.size()-1)) return;
}
}
if (!new_footprint.empty())
{
setUnpaddedRobotFootprint(new_footprint);
}
else
{
// robot_radius may be 0, but that must be intended at this point.
setUnpaddedRobotFootprint(makeFootprintFromRadius(robot_radius));
}
}
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_);
}
// void Costmap2DROBOT::movementCB(const ros::TimerEvent &event)
// {
// // don't allow configuration to happen while this check occurs
// // boost::recursive_mutex::scoped_lock mcl(configuration_mutex_);
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::movementCB");
// geometry_msgs::PoseStamped new_pose;
void Costmap2DROBOT::checkMovement()
{
geometry_msgs::PoseStamped new_pose;
if (!getRobotPose(new_pose))
std::cout << "Cannot get robot pose\n";
}
// if (!getRobotPose(new_pose))
// {
// ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling reconfiguration");
// }
// }
void Costmap2DROBOT::mapUpdateLoop(double frequency)
{
robot::Rate r(frequency);
while (!map_update_thread_shutdown_)
{
updateMap();
// void Costmap2DROBOT::mapUpdateLoop(double frequency)
// {
// ros::NodeHandle nh;
// ros::Rate r(frequency);
// while (nh.ok() && !map_update_thread_shutdown_)
// {
// #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());
// }
// }
r.sleep();
// make sure to sleep for the remainder of our cycle time
if (r.cycleTime() > robot::Duration(1 / frequency))
printf("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds\n", frequency,
r.cycleTime().toSec());
}
}
void Costmap2DROBOT::updateMap()
{
@ -534,12 +295,10 @@ void Costmap2DROBOT::updateMap()
y = pose.pose.position.y,
yaw = getYaw(pose.pose.orientation);
layered_costmap_->updateMap(x, y, yaw);
// geometry_msgs::PolygonStamped footprint;
// footprint.header.frame_id = global_frame_;
// footprint.header.stamp = ros::Time::now();
// transformFootprint(x, y, yaw, padded_footprint_, footprint);
// footprint_pub_.publish(footprint);
geometry_msgs::PolygonStamped footprint;
footprint.header.frame_id = global_frame_;
footprint.header.stamp = robot::Time::now();
transformFootprint(x, y, yaw, padded_footprint_, footprint);
initialized_ = true;
}
@ -565,9 +324,17 @@ void Costmap2DROBOT::start()
// 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
// robot::Rate r(100.0);
// while (ros::ok() && !initialized_ && map_update_thread_)
// r.sleep();
robot::Time start_time = robot::Time::now();
robot::Rate r(100.0);
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()
@ -598,9 +365,18 @@ void Costmap2DROBOT::resume()
stop_updates_ = false;
// block until the costmap is re-initialized.. meaning one update cycle has run
// robot::Rate r(100.0);
// while (!initialized_)
// r.sleep();
robot::Time start_time = robot::Time::now();
robot::Rate r(100.0);
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
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::getRobotPose");
// tf3::toMsg(tf3::Transform::getIdentity(), global_pose.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.stamp = robot::Time();
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)))
{
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
else
{
// 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)

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

532
test/static_layer_test.cpp
View File

@ -1,472 +1,90 @@
#include <boost/dll/import.hpp>
#include <iostream>
#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/cost_values.h>
#include <gtest/gtest.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";
bool rolling_window = false;
bool track_unknown = false;
LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
// ✅ FIX 1: RESIZE costmap TRƯỚC KHI sử dụng
// Costmap mặc định có size = 0x0, resolution = 0.0 -> sẽ gây crash
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;
// // ✅ FIX 2: Lưu tất cả plugins để tránh bị destroy
// std::vector<costmap_2d::PluginLayerPtr> plugin_instances;
// // // ✅ FIX 3: Load và add static_layer
// // 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 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;
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,
unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
virtual void TestBody(){}
private:
costmap_2d::Costmap2DROBOT costmap_ros_;
};
// // // ✅ FIX 5: Load và add obstacle_layer
// // auto creator2 = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// // "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// // );
// // 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;
CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("test_costmap", tf){}
void CostmapTester::checkConsistentCosts(){
costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
// // ✅ 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;
//get a copy of the costmap contained by our ros wrapper
costmap->saveMap("costmap_test.pgm");
// 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;
//loop through the costmap and check for any unexpected drop-offs in costs
for(unsigned int i = 0; i < costmap->getSizeInCellsX(); ++i){
for(unsigned int j = 0; j < costmap->getSizeInCellsY(); ++j){
compareCellToNeighbors(*costmap, i, j);
}
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;
//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()){
compareCells(costmap, x, y, nx, ny);
}
}
}
}
//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){
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 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));
}
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;
unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
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",
x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
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));
}
}
};
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);
// auto creator = boost::dll::import_alias<costmap_2d::PluginLayerPtr()>(
// "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// );
// costmap_2d::PluginLayerPtr plugin = creator();
// 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
// );
// plugin = creator();
// std::cout << "Plugin created static_layer successfully" << std::endl;
// plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);
// 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);
// 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);
// #include <boost/dll/import.hpp>
// #include <iostream>
// #include <costmap_2d/costmap_layer.h>
// #include <costmap_2d/layered_costmap.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[]) {
// 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;
// }
return(0);
}