From 2e0a4348ddd73d7e7c72d1167e86de49224b5082 Mon Sep 17 00:00:00 2001 From: duongtd Date: Wed, 26 Nov 2025 15:00:26 +0700 Subject: [PATCH] update file costmap_2d_robot --- .gitignore | 4 + CMakeLists.txt | 233 ++++------ include/costmap_2d/costmap_2d_robot.h | 60 ++- include/costmap_2d/data_convert.h | 21 +- include/costmap_2d/footprint.h | 46 +- plugins/static_layer.cpp | 2 - src/costmap_2d_robot.cpp | 647 +++++++++----------------- src/footprint.cpp | 206 ++++---- test/static_layer_test.cpp | 532 +++------------------ 9 files changed, 551 insertions(+), 1200 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..64c5965 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +# Bỏ qua thư mục build/ +build/ + +CODE_REVIEW.md \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 0975d9f..339b498 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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} - $ - $ + PUBLIC + ${Boost_INCLUDE_DIRS} # Boost headers + $ # Khi build từ source + $ # 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() diff --git a/include/costmap_2d/costmap_2d_robot.h b/include/costmap_2d/costmap_2d_robot.h index 53c3250..f633869 100644 --- a/include/costmap_2d/costmap_2d_robot.h +++ b/include/costmap_2d/costmap_2d_robot.h @@ -41,27 +41,31 @@ #include #include #include + #include #include #include + #include #include #include +#include -// 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(*a); -// } -// }; +#include +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(*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 &new_footprint, + const std::vector &old_footprint, + const double &robot_radius); - // void loadOldParameters(ros::NodeHandle& nh); - // void warnForOldParameters(ros::NodeHandle& nh); - // void checkOldParam(ros::NodeHandle& nh, const std::string ¶m_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 plugin_loader_; - // Costmap2DPublisher* publisher_; - // dynamic_reconfigure::Server *dsrv_; boost::recursive_mutex configuration_mutex_; - // ros::Subscriber footprint_sub_; - // ros::Publisher footprint_pub_; std::vector unpadded_footprint_; std::vector 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 diff --git a/include/costmap_2d/data_convert.h b/include/costmap_2d/data_convert.h index c6ed5d7..2604ad7 100644 --- a/include/costmap_2d/data_convert.h +++ b/include/costmap_2d/data_convert.h @@ -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 #include @@ -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; diff --git a/include/costmap_2d/footprint.h b/include/costmap_2d/footprint.h index 503954d..9547cec 100644 --- a/include/costmap_2d/footprint.h +++ b/include/costmap_2d/footprint.h @@ -41,6 +41,8 @@ #include #include +#include + #include #include @@ -134,30 +136,30 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector 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 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 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 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& 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& footprint); } // end namespace costmap_2d diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index 2e73d60..f45958d 100644 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -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 != " ") { diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 4001b32..485c064 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -40,7 +40,7 @@ #include #include - #include +#include #include #include @@ -48,24 +48,10 @@ #include #include -// #include - 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(my_list[i]["name"]); -// std::string type = static_cast(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 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(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(ros::NodeHandle("~/" + name)); -// dynamic_reconfigure::Server::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 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(my_list[i]["name"]); - // std::string type = static_cast(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( - 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 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(my_list[i]["name"]); + // std::string type = static_cast(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( + 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 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 ¶m_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 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 &new_footprint, + const std::vector &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& points) { @@ -465,62 +261,27 @@ void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vectorsetFootprint(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) diff --git a/src/footprint.cpp b/src/footprint.cpp index 3a313e0..de2c0be 100644 --- a/src/footprint.cpp +++ b/src/footprint.cpp @@ -208,117 +208,117 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector makeFootprintFromParams(ros::NodeHandle& nh) -// { -// std::string full_param_name; -// std::string full_radius_param_name; -// std::vector points; +std::vector makeFootprintFromParams(const std::string& file_name) +{ + std::string full_param_name; + std::string full_radius_param_name; + std::vector 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& 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& 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 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 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 footprint; -// geometry_msgs::Point pt; + std::vector 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 diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp index 3c9513a..11ad1e4 100644 --- a/test/static_layer_test.cpp +++ b/test/static_layer_test.cpp @@ -1,472 +1,90 @@ -#include -#include -#include -#include +#include +#include +#include #include -#include -#include "nav_msgs/OccupancyGrid.h" -#include -#include -#include -#include -#include -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 plugin_instances; - - // // // ✅ FIX 3: Load và add static_layer - // // auto creator = boost::dll::import_alias( - // // "./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/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/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/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 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 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 plugin_instances; - - for (auto& info : plugins_to_load) - { - auto creator = boost::dll::import_alias( - 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/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/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/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/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(x-nx), static_cast(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/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/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/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/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 -// #include -// #include -// #include -// #include -// #include -// #include "nav_msgs/OccupancyGrid.h" -// #include -// #include -// #include -// #include -// #include -// using namespace costmap_2d; - -// int main(int argc, char* argv[]) { - -// auto creator = boost::dll::import_alias( -// "./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/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((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/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/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/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; -// } \ No newline at end of file + return(0); +}