update

fix bugs
update
2026-03-03 07:26:47 +00:00 · 2026-02-26 14:54:23 +07:00 · 2026-02-26 14:52:33 +07:00 · 2026-02-26 14:49:45 +07:00 · 2026-02-26 14:43:17 +07:00 · 2026-02-07 11:00:46 +07:00
3 changed files with 99 additions and 90 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,7 +49,7 @@ if (NOT BUILDING_WITH_CATKIN)
    robot_cpp
    robot_time
  )
-
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 else()

 # ========================================================
@@ -64,7 +64,6 @@ else()
    robot_laser_geometry
    robot_visualization_msgs
    robot_voxel_grid
-    tf3
    robot_tf3_geometry_msgs
    robot_tf3_sensor_msgs
    data_convert
@@ -73,10 +72,12 @@ else()
    robot_time
  )

+  find_library(TF3_LIBRARY NAMES tf3)
+  
  catkin_package(
    INCLUDE_DIRS include
    LIBRARIES robot_costmap_2d plugins
-    CATKIN_DEPENDS robot_std_msgs robot_sensor_msgs geometry_msgs robot_nav_msgs robot_map_msgs robot_laser_geometry robot_visualization_msgs robot_voxel_grid tf3 robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time
+    CATKIN_DEPENDS robot_std_msgs robot_sensor_msgs geometry_msgs robot_nav_msgs robot_map_msgs robot_laser_geometry robot_visualization_msgs robot_voxel_grid robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time
    DEPENDS  PCL Boost 
  )

@@ -87,6 +88,7 @@ else()
    ${Boost_INCLUDE_DIRS}
    ${GTEST_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
+    ${TF3_INCLUDE_DIR}
  )
  link_directories(${PCL_LIBRARY_DIRS})
 endif()
@@ -122,6 +124,7 @@ if(BUILDING_WITH_CATKIN)
      ${EIGEN3_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
      ${PCL_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
  )

  target_link_libraries(robot_costmap_2d
@@ -130,6 +133,7 @@ if(BUILDING_WITH_CATKIN)
    PRIVATE yaml-cpp
    PRIVATE dl
    PRIVATE ${PCL_LIBRARIES}
+    PRIVATE ${TF3_LIBRARY}
  )

 else()
@@ -141,6 +145,7 @@ else()
      ${EIGEN3_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
      ${PCL_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
  )

  target_link_libraries(robot_costmap_2d
@@ -151,6 +156,7 @@ else()
      yaml-cpp
      dl
      ${PCL_LIBRARIES}
+      ${TF3_LIBRARY}
  )

  set_target_properties(robot_costmap_2d PROPERTIES
@@ -189,6 +195,7 @@ if(BUILDING_WITH_CATKIN)
    PRIVATE ${catkin_LIBRARIES}
    PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem
    PRIVATE yaml-cpp
+    PRIVATE ${TF3_LIBRARY}
  )

 else()
@@ -205,6 +212,7 @@ else()
    PRIVATE yaml-cpp
    PRIVATE robot_time
    PRIVATE robot_cpp
+    PRIVATE ${TF3_LIBRARY}
  )

  set_target_properties(plugins PROPERTIES
@@ -270,41 +278,47 @@ endif()
 # ========================================================
 # Test executables
 # ========================================================
-# option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" OFF)
+option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" ON)

-# if(BUILD_COSTMAP_TESTS)
-#   if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/array_parser_test.cpp)
-#     add_executable(test_array_parser test/array_parser_test.cpp)
-#     target_link_libraries(test_array_parser PRIVATE
-#       robot_costmap_2d
-#       GTest::GTest
-#       GTest::Main
-#       Threads::Threads
-#     )
-#   endif()
+if(BUILD_COSTMAP_TESTS)
+  find_package(GTest REQUIRED)
+  find_package(Boost REQUIRED COMPONENTS system thread filesystem)
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)

-#   if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp)
-#     add_executable(test_costmap test/coordinates_test.cpp)
-#     target_link_libraries(test_costmap PRIVATE
-#       robot_costmap_2d
-#       GTest::GTest
-#       GTest::Main
-#       Threads::Threads
-#     )
-#   endif()
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/array_parser_test.cpp)
+    add_executable(test_array_parser test/array_parser_test.cpp)
+    target_link_libraries(test_array_parser PRIVATE 
+      robot_costmap_2d
+      GTest::GTest
+      GTest::Main
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
+    )
+  endif()

-#   if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/static_layer_test.cpp)
-#     add_executable(test_plugin test/static_layer_test.cpp)
-#     target_link_libraries(test_plugin PRIVATE
-#       robot_costmap_2d
-#       Boost::boost Boost::filesystem Boost::system
-#       yaml-cpp
-#       dl
-#       Threads::Threads
-#       tf3
-#       robot_time
-#       GTest::GTest
-#       GTest::Main
-#     )
-#   endif()
-# endif()
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp)
+    add_executable(test_costmap test/coordinates_test.cpp)
+    target_link_libraries(test_costmap PRIVATE
+      robot_costmap_2d
+      GTest::GTest
+      GTest::Main
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
+    )
+  endif()
+
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/static_layer_test.cpp)
+    add_executable(test_plugin test/static_layer_test.cpp)
+    target_link_libraries(test_plugin PRIVATE
+      robot_costmap_2d
+      Boost::boost Boost::filesystem Boost::system
+      yaml-cpp
+      dl
+      Boost::system Boost::thread
+      robot_time
+      GTest::GTest
+      GTest::Main
+      ${TF3_LIBRARY}
+    )
+  endif()
+endif()
--- a/package.xml
+++ b/package.xml
@@ -46,9 +46,6 @@
  <build_depend>robot_voxel_grid</build_depend>
  <run_depend>robot_voxel_grid</run_depend>

-  <build_depend>tf3</build_depend>
-  <run_depend>tf3</run_depend>
-
  <build_depend>robot_tf3_geometry_msgs</build_depend>
  <run_depend>robot_tf3_geometry_msgs</run_depend>

--- a/src/costmap_2d_robot.cpp
+++ b/src/costmap_2d_robot.cpp
@@ -82,7 +82,7 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
  name_ = name;
  std::string config_file_name = "costmap_params.yaml";
  getParams(config_file_name, name_, nh);
-
+ 
  // create a thread to handle updating the map
  stop_updates_ = false;
  initialized_ = true;
@@ -129,6 +129,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::st
    {
      if (last_error + robot::Duration(5.0) < robot::Time::now())
      {
+        std::string all_frames_string = tf_.allFramesAsString();
+        robot::log_info("[%s:%d]\n INFO: tf allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str());
        // std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl;
        robot::log_warning("[%s:%d] %0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
                __FILE__, __LINE__, robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
@@ -332,21 +334,19 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
                                          const std::string& plugin_type, 
                                          robot::NodeHandle& nh)
 {
-  robot::NodeHandle priv_nh_1(nh, costmap_name);
-  robot::NodeHandle target_layer(priv_nh_1, plugin_name);
-  robot::NodeHandle priv_nh_2(nh, plugin_name);
-  // robot::log_error("TEST: %s",priv_nh_2.getNamespace().c_str());
-  
+  robot::NodeHandle costmap_nh(nh, costmap_name);
+  robot::NodeHandle costmap_plugin_nh(costmap_nh, plugin_name);
+  robot::NodeHandle plugin_nh(nh, plugin_name);
  if(plugin_type == "StaticLayer")
  {
    std::string map_topic;
    int unknown_cost_value;
    int lethal_cost_threshold;
    bool track_unknown_space;
-    move_parameter(priv_nh_2, target_layer, "map_topic", map_topic);
-    move_parameter(priv_nh_2, target_layer, "unknown_cost_value", unknown_cost_value);
-    move_parameter(priv_nh_2, target_layer, "lethal_cost_threshold", lethal_cost_threshold);
-    move_parameter(priv_nh_2, target_layer, "track_unknown_space", track_unknown_space);
+    move_parameter(plugin_nh, costmap_plugin_nh, "map_topic", map_topic);
+    move_parameter(plugin_nh, costmap_plugin_nh, "unknown_cost_value", unknown_cost_value);
+    move_parameter(plugin_nh, costmap_plugin_nh, "lethal_cost_threshold", lethal_cost_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
  }
  else if(plugin_type == "VoxelLayer")
  {
@@ -356,24 +356,23 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
    int mark_threshold;
    int unknown_threshold;
    bool publish_voxel_map;
-    move_parameter(priv_nh_2, target_layer, "origin_z", origin_z);
-    move_parameter(priv_nh_2, target_layer, "z_resolution", z_resolution);
-    move_parameter(priv_nh_2, target_layer, "z_voxels", z_voxels);
-    move_parameter(priv_nh_2, target_layer, "mark_threshold", mark_threshold);
-    move_parameter(priv_nh_2, target_layer, "unknown_threshold", unknown_threshold);
-    move_parameter(priv_nh_2, target_layer, "publish_voxel_map", publish_voxel_map);
-    if(priv_nh_2.hasParam("observation_sources"))
+    move_parameter(plugin_nh, costmap_plugin_nh, "origin_z", origin_z);
+    move_parameter(plugin_nh, costmap_plugin_nh, "z_resolution", z_resolution);
+    move_parameter(plugin_nh, costmap_plugin_nh, "z_voxels", z_voxels);
+    move_parameter(plugin_nh, costmap_plugin_nh, "mark_threshold", mark_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "unknown_threshold", unknown_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "publish_voxel_map", publish_voxel_map);
+    if(plugin_nh.hasParam("observation_sources"))
    {
      std::string topics_string;
-      move_parameter(priv_nh_2, target_layer, "observation_sources", topics_string);
+      move_parameter(plugin_nh, costmap_plugin_nh, "observation_sources", topics_string);
      robot::log_error("topics_string: %s", topics_string.c_str());
      std::stringstream ss(topics_string);
      std::string source;
      while (ss >> source)
      {
-        robot::NodeHandle priv_nh_3(priv_nh_2, source);
-        robot::NodeHandle target_layer_2(target_layer, source);
-        robot::log_warning("priv_nh_3: %s",priv_nh_3.getNamespace().c_str());
+        robot::NodeHandle plugin_nh_element(plugin_nh, source);
+        robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
        std::string topic;
        std::string data_type;
        bool clearing;
@@ -381,15 +380,14 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
        bool inf_is_valid;
        double min_obstacle_height;
        double max_obstacle_height;
-        move_parameter(priv_nh_2, target_layer_2, "topic", topic);
-        robot::log_error("topic: %s", topic.c_str());
-        move_parameter(priv_nh_2, target_layer_2, "data_type", data_type);
-        move_parameter(priv_nh_2, target_layer_2, "clearing", clearing);
-        move_parameter(priv_nh_2, target_layer_2, "marking", marking);
-        move_parameter(priv_nh_2, target_layer_2, "inf_is_valid", inf_is_valid);
-        move_parameter(priv_nh_2, target_layer_2, "min_obstacle_height", min_obstacle_height);
-        move_parameter(priv_nh_2, target_layer_2, "max_obstacle_height", max_obstacle_height);
-        target_layer_2.printParams();
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
+        robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
      }
    }
  }
@@ -399,21 +397,21 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
    double raytrace_range;
    double obstacle_range;
    bool track_unknown_space;
-    move_parameter(priv_nh_2, target_layer, "max_obstacle_height", max_obstacle_height);
-    move_parameter(priv_nh_2, target_layer, "raytrace_range", raytrace_range);
-    move_parameter(priv_nh_2, target_layer, "obstacle_range", obstacle_range);
-    move_parameter(priv_nh_2, target_layer, "track_unknown_space", track_unknown_space);
-    if(priv_nh_2.hasParam("observation_sources"))
+    move_parameter(plugin_nh, costmap_plugin_nh, "max_obstacle_height", max_obstacle_height);
+    move_parameter(plugin_nh, costmap_plugin_nh, "raytrace_range", raytrace_range);
+    move_parameter(plugin_nh, costmap_plugin_nh, "obstacle_range", obstacle_range);
+    move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
+    if(plugin_nh.hasParam("observation_sources"))
    {
      std::string topics_string;
-      move_parameter(priv_nh_2, target_layer, "observation_sources", topics_string);
+      move_parameter(plugin_nh, costmap_plugin_nh, "observation_sources", topics_string);
      robot::log_error("topics_string: %s", topics_string.c_str());
      std::stringstream ss(topics_string);
      std::string source;
      while (ss >> source)
      {
-        robot::NodeHandle priv_nh_3(priv_nh_2, source);
-        robot::NodeHandle target_layer_2(target_layer, source);
+        robot::NodeHandle plugin_nh_element(plugin_nh, source);
+        robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
        std::string topic;
        std::string data_type;
        bool clearing;
@@ -421,23 +419,23 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
        bool inf_is_valid;
        double min_obstacle_height;
        double max_obstacle_height;
-        move_parameter(priv_nh_2, target_layer_2, "topic", topic);
-        move_parameter(priv_nh_2, target_layer_2, "data_type", data_type);
-        move_parameter(priv_nh_2, target_layer_2, "clearing", clearing);
-        move_parameter(priv_nh_2, target_layer_2, "marking", marking);
-        move_parameter(priv_nh_2, target_layer_2, "inf_is_valid", inf_is_valid);
-        move_parameter(priv_nh_2, target_layer_2, "min_obstacle_height", min_obstacle_height);
-        move_parameter(priv_nh_2, target_layer_2, "max_obstacle_height", max_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
+        robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
      }
    }
-
  }
  else if(plugin_type == "InflationLayer")
  {
    double cost_scaling_factor;
    double inflation_radius;
-    move_parameter(priv_nh_2, target_layer, "cost_scaling_factor", cost_scaling_factor);
-    move_parameter(priv_nh_2, target_layer, "inflation_radius", inflation_radius);
+    move_parameter(plugin_nh, costmap_plugin_nh, "cost_scaling_factor", cost_scaling_factor);
+    move_parameter(plugin_nh, costmap_plugin_nh, "inflation_radius", inflation_radius);
  }

 }
@@ -696,7 +694,7 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
  }
  catch (tf3::ExtrapolationException& ex)
  {
-    robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
+    // robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
    return false;
  }
  // ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);
Author	SHA1	Message	Date
hoangson02	2fcd211ccf	update	2026-03-03 07:26:47 +00:00
HiepLM	3d621de809	fix bugs	2026-02-26 14:54:23 +07:00
HiepLM	1f9e9f1398	update	2026-02-26 14:52:33 +07:00
duongtd	9208c8bcdc	WIP: fix costmap_2d_robot	2026-02-26 14:49:45 +07:00
duongtd	6c6e5b44f8	update copyParentParameters	2026-02-26 14:43:17 +07:00
HiepLM	eb52edc6e8	update tf3	2026-02-07 11:00:46 +07:00