add file costmap_2d_robot
This commit is contained in:
parent
0c61984d3e
commit
59ec58a018
|
|
@ -12,12 +12,12 @@ set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
|
|||
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
|
||||
|
||||
# --- Dependencies ---
|
||||
# find_package(tf2 REQUIRED)
|
||||
# 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(TF2_LIBRARY /usr/lib/libtf2.so)
|
||||
# set(tf3_LIBRARY /usr/lib/libtf3.so)
|
||||
|
||||
# --- Include other message packages if needed ---
|
||||
# if (NOT TARGET sensor_msgs)
|
||||
|
|
@ -39,6 +39,8 @@ find_package(PCL REQUIRED COMPONENTS common io)
|
|||
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../voxel_grid ${CMAKE_BINARY_DIR}/voxel_grid_build)
|
||||
# endif()
|
||||
|
||||
add_definitions(-DCOSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
|
|
@ -51,7 +53,8 @@ link_directories(${PCL_LIBRARY_DIRS})
|
|||
add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})
|
||||
|
||||
# --- Core library ---
|
||||
add_library(costmap_2d_new
|
||||
add_library(costmap_2d
|
||||
src/costmap_2d_robot.cpp
|
||||
src/array_parser.cpp
|
||||
src/costmap_2d.cpp
|
||||
src/observation_buffer.cpp
|
||||
|
|
@ -62,7 +65,7 @@ add_library(costmap_2d_new
|
|||
src/costmap_layer.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(costmap_2d_new
|
||||
target_link_libraries(costmap_2d
|
||||
${Boost_LIBRARIES}
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
|
|
@ -71,17 +74,22 @@ target_link_libraries(costmap_2d_new
|
|||
map_msgs
|
||||
laser_geometry
|
||||
voxel_grid
|
||||
# ${TF2_LIBRARY}
|
||||
# ${tf3_LIBRARY}
|
||||
tf3
|
||||
)
|
||||
target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS})
|
||||
target_include_directories(costmap_2d
|
||||
PUBLIC ${Boost_INCLUDE_DIRS}
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
|
||||
)
|
||||
|
||||
|
||||
# --- Plugin libraries ---
|
||||
add_library(static_layer SHARED
|
||||
plugins/static_layer.cpp
|
||||
)
|
||||
target_link_libraries(static_layer
|
||||
costmap_2d_new
|
||||
costmap_2d
|
||||
${Boost_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
|
@ -90,7 +98,7 @@ add_library(obstacle_layer SHARED
|
|||
plugins/obstacle_layer.cpp
|
||||
)
|
||||
target_link_libraries(obstacle_layer
|
||||
costmap_2d_new
|
||||
costmap_2d
|
||||
${Boost_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
|
@ -99,7 +107,7 @@ add_library(inflation_layer SHARED
|
|||
plugins/inflation_layer.cpp
|
||||
)
|
||||
target_link_libraries(inflation_layer
|
||||
costmap_2d_new
|
||||
costmap_2d
|
||||
${Boost_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
|
@ -108,7 +116,7 @@ add_library(voxel_layer SHARED
|
|||
plugins/voxel_layer.cpp
|
||||
)
|
||||
target_link_libraries(voxel_layer
|
||||
costmap_2d_new
|
||||
costmap_2d
|
||||
${Boost_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
|
@ -117,7 +125,7 @@ add_library(critical_layer SHARED
|
|||
plugins/critical_layer.cpp
|
||||
)
|
||||
target_link_libraries(critical_layer
|
||||
costmap_2d_new
|
||||
costmap_2d
|
||||
static_layer
|
||||
${Boost_LIBRARIES}
|
||||
yaml-cpp
|
||||
|
|
@ -127,7 +135,7 @@ add_library(directional_layer SHARED
|
|||
plugins/directional_layer.cpp
|
||||
)
|
||||
target_link_libraries(directional_layer
|
||||
costmap_2d_new
|
||||
costmap_2d
|
||||
static_layer
|
||||
${Boost_LIBRARIES}
|
||||
yaml-cpp
|
||||
|
|
@ -137,7 +145,7 @@ add_library(preferred_layer SHARED
|
|||
plugins/preferred_layer.cpp
|
||||
)
|
||||
target_link_libraries(preferred_layer
|
||||
costmap_2d_new
|
||||
costmap_2d
|
||||
static_layer
|
||||
${Boost_LIBRARIES}
|
||||
yaml-cpp
|
||||
|
|
@ -147,20 +155,20 @@ add_library(unpreferred_layer SHARED
|
|||
plugins/unpreferred_layer.cpp
|
||||
)
|
||||
target_link_libraries(unpreferred_layer
|
||||
costmap_2d_new
|
||||
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_new
|
||||
${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)
|
||||
|
|
@ -168,22 +176,22 @@ add_executable(test_costmap test/coordinates_test.cpp)
|
|||
add_executable(test_plugin test/static_layer_test.cpp)
|
||||
|
||||
target_link_libraries(test_array_parser PRIVATE
|
||||
costmap_2d_new
|
||||
costmap_2d
|
||||
GTest::GTest
|
||||
GTest::Main
|
||||
pthread
|
||||
)
|
||||
|
||||
target_link_libraries(test_costmap PRIVATE
|
||||
costmap_2d_new
|
||||
costmap_2d
|
||||
GTest::GTest
|
||||
GTest::Main
|
||||
pthread
|
||||
)
|
||||
|
||||
target_link_libraries(test_plugin PRIVATE
|
||||
${TF2_LIBRARY}
|
||||
costmap_2d_new
|
||||
${tf3_LIBRARY}
|
||||
costmap_2d
|
||||
static_layer
|
||||
obstacle_layer
|
||||
inflation_layer
|
||||
|
|
|
|||
|
|
@ -329,23 +329,23 @@ bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int&
|
|||
|
||||
## 🟠 **VẤN ĐỀ TRONG CMAKELISTS.TXT**
|
||||
|
||||
### 9. **Hardcoded path đến tf2 library**
|
||||
### 9. **Hardcoded path đến tf3 library**
|
||||
|
||||
**File:** `CMakeLists.txt:135`
|
||||
|
||||
```cmake
|
||||
set(TF2_LIBRARY /usr/lib/libtf2.so) # ❌ Hardcoded path
|
||||
set(tf3_LIBRARY /usr/lib/libtf3.so) # ❌ Hardcoded path
|
||||
```
|
||||
|
||||
**Vấn đề:**
|
||||
- Không portable, sẽ fail trên các hệ thống khác
|
||||
- Nên dùng `find_package(tf2)` hoặc `find_library()`
|
||||
- Nên dùng `find_package(tf3)` hoặc `find_library()`
|
||||
|
||||
**Giải pháp:**
|
||||
```cmake
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf3 REQUIRED)
|
||||
# hoặc
|
||||
find_library(TF2_LIBRARY tf2 PATHS /usr/lib /usr/local/lib)
|
||||
find_library(tf3_LIBRARY tf3 PATHS /usr/lib /usr/local/lib)
|
||||
```
|
||||
|
||||
---
|
||||
|
|
|
|||
|
|
@ -364,7 +364,7 @@ boost_thread_DIR:PATH=/usr/lib/x86_64-linux-gnu/cmake/boost_thread-1.71.0
|
|||
costmap_2d_BINARY_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d/build
|
||||
|
||||
//Dependencies for the target
|
||||
costmap_2d_LIB_DEPENDS:STATIC=general;/usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0;general;/usr/lib/x86_64-linux-gnu/libboost_filesystem.so;general;/usr/lib/x86_64-linux-gnu/libboost_date_time.so;general;/usr/lib/x86_64-linux-gnu/libboost_iostreams.so;general;/usr/lib/x86_64-linux-gnu/libboost_regex.so;general;tf2;
|
||||
costmap_2d_LIB_DEPENDS:STATIC=general;/usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0;general;/usr/lib/x86_64-linux-gnu/libboost_filesystem.so;general;/usr/lib/x86_64-linux-gnu/libboost_date_time.so;general;/usr/lib/x86_64-linux-gnu/libboost_iostreams.so;general;/usr/lib/x86_64-linux-gnu/libboost_regex.so;general;tf3;
|
||||
|
||||
//Value Computed by CMake
|
||||
costmap_2d_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d
|
||||
|
|
|
|||
|
|
@ -99,7 +99,7 @@ costmap_2d/layered_costmap.h
|
|||
-
|
||||
string
|
||||
-
|
||||
tf2/buffer_core.h
|
||||
tf3/buffer_core.h
|
||||
-
|
||||
|
||||
../include/costmap_2d/layered_costmap.h
|
||||
|
|
@ -131,7 +131,7 @@ chrono
|
|||
-
|
||||
costmap_2d/observation.h
|
||||
-
|
||||
tf2/buffer_core.h
|
||||
tf3/buffer_core.h
|
||||
-
|
||||
sensor_msgs/PointCloud2.h
|
||||
-
|
||||
|
|
@ -249,7 +249,7 @@ vector
|
|||
/home/duongtd/robotics_core/costmap_2d/src/observation_buffer.cpp
|
||||
costmap_2d/observation_buffer.h
|
||||
-
|
||||
tf2/convert.h
|
||||
tf3/convert.h
|
||||
-
|
||||
sensor_msgs/point_cloud2_iterator.h
|
||||
-
|
||||
|
|
|
|||
|
|
@ -87,9 +87,9 @@ costmap_2d/static_layer.h
|
|||
../include/costmap_2d/data_convert.h
|
||||
geometry_msgs/TransformStamped.h
|
||||
-
|
||||
tf2/utils.h
|
||||
tf3/utils.h
|
||||
-
|
||||
tf2/compat.h
|
||||
tf3/compat.h
|
||||
-
|
||||
|
||||
../include/costmap_2d/directional_layer.h
|
||||
|
|
@ -127,7 +127,7 @@ costmap_2d/layered_costmap.h
|
|||
-
|
||||
string
|
||||
-
|
||||
tf2/buffer_core.h
|
||||
tf3/buffer_core.h
|
||||
-
|
||||
|
||||
../include/costmap_2d/layered_costmap.h
|
||||
|
|
@ -159,7 +159,7 @@ chrono
|
|||
-
|
||||
costmap_2d/observation.h
|
||||
-
|
||||
tf2/buffer_core.h
|
||||
tf3/buffer_core.h
|
||||
-
|
||||
sensor_msgs/PointCloud2.h
|
||||
-
|
||||
|
|
@ -441,7 +441,7 @@ costmap_2d/costmap_math.h
|
|||
-
|
||||
sensor_msgs/point_cloud2_iterator.h
|
||||
-
|
||||
tf2/utils.h
|
||||
tf3/utils.h
|
||||
-
|
||||
boost/dll/alias.hpp
|
||||
-
|
||||
|
|
@ -461,9 +461,9 @@ costmap_2d/data_convert.h
|
|||
-
|
||||
costmap_2d/utils.h
|
||||
-
|
||||
tf2/convert.h
|
||||
tf3/convert.h
|
||||
-
|
||||
tf2/utils.h
|
||||
tf3/utils.h
|
||||
-
|
||||
boost/dll/alias.hpp
|
||||
-
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
/usr/bin/c++ CMakeFiles/test_array_parser.dir/test/array_parser_test.cpp.o -o test_array_parser libcostmap_2d.a -lpthread /usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0 /usr/lib/x86_64-linux-gnu/libboost_filesystem.so /usr/lib/x86_64-linux-gnu/libboost_date_time.so /usr/lib/x86_64-linux-gnu/libboost_iostreams.so /usr/lib/x86_64-linux-gnu/libboost_regex.so -lyaml-cpp robot_time_build/librobot_time.a -ltf2 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread
|
||||
/usr/bin/c++ CMakeFiles/test_array_parser.dir/test/array_parser_test.cpp.o -o test_array_parser libcostmap_2d.a -lpthread /usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0 /usr/lib/x86_64-linux-gnu/libboost_filesystem.so /usr/lib/x86_64-linux-gnu/libboost_date_time.so /usr/lib/x86_64-linux-gnu/libboost_iostreams.so /usr/lib/x86_64-linux-gnu/libboost_regex.so -lyaml-cpp robot_time_build/librobot_time.a -ltf3 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
/usr/bin/c++ CMakeFiles/test_costmap.dir/test/coordinates_test.cpp.o -o test_costmap libcostmap_2d.a -lpthread /usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0 /usr/lib/x86_64-linux-gnu/libboost_filesystem.so /usr/lib/x86_64-linux-gnu/libboost_date_time.so /usr/lib/x86_64-linux-gnu/libboost_iostreams.so /usr/lib/x86_64-linux-gnu/libboost_regex.so -lyaml-cpp robot_time_build/librobot_time.a -ltf2 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread
|
||||
/usr/bin/c++ CMakeFiles/test_costmap.dir/test/coordinates_test.cpp.o -o test_costmap libcostmap_2d.a -lpthread /usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0 /usr/lib/x86_64-linux-gnu/libboost_filesystem.so /usr/lib/x86_64-linux-gnu/libboost_date_time.so /usr/lib/x86_64-linux-gnu/libboost_iostreams.so /usr/lib/x86_64-linux-gnu/libboost_regex.so -lyaml-cpp robot_time_build/librobot_time.a -ltf3 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread
|
||||
|
|
|
|||
|
|
@ -40,14 +40,10 @@
|
|||
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/layer.h>
|
||||
// #include <costmap_2d/costmap_2d_publisher.h>
|
||||
// #include <costmap_2d/Costmap2DConfig.h>
|
||||
#include <costmap_2d/footprint.h>
|
||||
#include <geometry_msgs/Polygon.h>
|
||||
#include <geometry_msgs/PolygonStamped.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
// #include <dynamic_reconfigure/server.h>
|
||||
// #include <pluginlib/class_loader.hpp>
|
||||
#include <tf3/LinearMath/Transform.h>
|
||||
#include <robot/rate.h>
|
||||
#include <costmap_2d/data_convert.h>
|
||||
|
|
|
|||
|
|
@ -83,7 +83,7 @@ namespace costmap_2d
|
|||
return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
|
||||
}
|
||||
|
||||
tf3::Transform convertToTf2Transform(const geometry_msgs::Transform& msg)
|
||||
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 convertToTf2Transform(const tf3::TransformMsg& msg)
|
||||
tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg)
|
||||
{
|
||||
tf3::Quaternion q(
|
||||
msg.rotation.x,
|
||||
|
|
|
|||
|
|
@ -37,14 +37,14 @@ public:
|
|||
* @param max_obstacle_height The minimum height of a hitpoint to be considered legal
|
||||
* @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
|
||||
* @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
|
||||
* @param tf2_buffer A reference to a tf3 Buffer
|
||||
* @param tf3_buffer A reference to a tf3 Buffer
|
||||
* @param global_frame The frame to transform PointClouds into
|
||||
* @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
|
||||
* @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
|
||||
*/
|
||||
ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
|
||||
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
|
||||
double raytrace_range, tf3::BufferCore& tf2_buffer, std::string global_frame,
|
||||
double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
|
||||
std::string sensor_frame, double tf_tolerance);
|
||||
|
||||
/**
|
||||
|
|
@ -107,7 +107,7 @@ private:
|
|||
*/
|
||||
void purgeStaleObservations();
|
||||
|
||||
tf3::BufferCore& tf2_buffer_;
|
||||
tf3::BufferCore& tf3_buffer_;
|
||||
// const ros::Duration observation_keep_time_;
|
||||
// const ros::Duration expected_update_rate_;
|
||||
// ros::Time last_updated_;
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@
|
|||
#include <sensor_msgs/point_cloud_conversion.h>
|
||||
|
||||
#include <laser_geometry/laser_geometry.hpp>
|
||||
// #include <tf2_ros/message_filter.h>
|
||||
// #include <tf3_ros/message_filter.h>
|
||||
// #include <message_filters/subscriber.h>
|
||||
|
||||
namespace costmap_2d
|
||||
|
|
|
|||
|
|
@ -49,14 +49,14 @@ unsigned int countValues(costmap_2d::Costmap2D& costmap, unsigned char value, bo
|
|||
return count;
|
||||
}
|
||||
|
||||
void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
|
||||
void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
|
||||
{
|
||||
costmap_2d::StaticLayer* slayer = new costmap_2d::StaticLayer();
|
||||
layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
|
||||
slayer->initialize(&layers, "static", &tf);
|
||||
}
|
||||
|
||||
costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
|
||||
costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
|
||||
{
|
||||
costmap_2d::ObstacleLayer* olayer = new costmap_2d::ObstacleLayer();
|
||||
olayer->initialize(&layers, "obstacles", &tf);
|
||||
|
|
@ -86,7 +86,7 @@ void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, doubl
|
|||
olayer->addStaticObservation(obs, true, true);
|
||||
}
|
||||
|
||||
costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
|
||||
costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
|
||||
{
|
||||
costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer();
|
||||
ilayer->initialize(&layers, "inflation", &tf);
|
||||
|
|
|
|||
|
|
@ -1,6 +1,9 @@
|
|||
#ifndef COSTMAP_2D_UTILS_H
|
||||
#define COSTMAP_2D_UTILS_H
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <filesystem>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
|
@ -12,6 +15,29 @@ namespace costmap_2d
|
|||
return default_value;
|
||||
}
|
||||
|
||||
std::string getSourceFile(const std::string& root, const std::string& config_file_name)
|
||||
{
|
||||
std::string path_source = " ";
|
||||
std::string sub_folder = "config";
|
||||
namespace fs = std::filesystem;
|
||||
|
||||
fs::path cfg_dir = fs::path(root) / sub_folder;
|
||||
|
||||
if (fs::exists(cfg_dir) && fs::is_directory(cfg_dir))
|
||||
{
|
||||
for (const auto& entry : fs::recursive_directory_iterator(cfg_dir))
|
||||
{
|
||||
if (entry.is_regular_file() && entry.path().filename() == config_file_name)
|
||||
{
|
||||
path_source = entry.path().string();
|
||||
break; // tìm thấy thì dừng
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return path_source;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // COSTMAP_2D_UTILS_H
|
||||
|
|
@ -412,12 +412,12 @@ namespace costmap_2d
|
|||
return false;
|
||||
}
|
||||
// Copy map data given proper transformations
|
||||
tf3::Transform tf2_transform;
|
||||
tf2_transform = convertToTf2Transform(transformMsg.transform);
|
||||
x = tf2_transform.getOrigin().x();
|
||||
y = tf2_transform.getOrigin().y();
|
||||
tf3::Transform tf3_transform;
|
||||
tf3_transform = convertTotf3Transform(transformMsg.transform);
|
||||
x = tf3_transform.getOrigin().x();
|
||||
y = tf3_transform.getOrigin().y();
|
||||
// Extract the rotation as a quaternion from the transform
|
||||
tf3::Quaternion rotation = tf2_transform.getRotation();
|
||||
tf3::Quaternion rotation = tf3_transform.getRotation();
|
||||
// Convert the quaternion to a yaw angle (in radians)
|
||||
yaw = tf3::getYaw(rotation);
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -6,7 +6,9 @@
|
|||
#include <tf3/convert.h>
|
||||
#include <tf3/utils.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
|
||||
#include <filesystem>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
|
||||
using costmap_2d::NO_INFORMATION;
|
||||
using costmap_2d::LETHAL_OBSTACLE;
|
||||
|
|
@ -35,7 +37,22 @@ void StaticLayer::onInitialize()
|
|||
bool StaticLayer::getParams()
|
||||
{
|
||||
try {
|
||||
YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml");
|
||||
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 != " ")
|
||||
{
|
||||
std::cout << "Path source: " << path_source << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "/cfg folder not found!" << std::endl;
|
||||
}
|
||||
|
||||
|
||||
YAML::Node config = YAML::LoadFile(path_source);
|
||||
YAML::Node layer = config["static_layer"];
|
||||
|
||||
|
||||
|
|
@ -299,9 +316,9 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
|
|||
return;
|
||||
}
|
||||
// Copy map data given proper transformations
|
||||
tf3::Transform tf2_transform;
|
||||
tf2_transform = convertToTf2Transform(transformMsg.transform);
|
||||
// tf3::convert(transformMsg.transform, tf2_transform);
|
||||
tf3::Transform tf3_transform;
|
||||
tf3_transform = convertTotf3Transform(transformMsg.transform);
|
||||
// tf3::convert(transformMsg.transform, tf3_transform);
|
||||
for (unsigned int i = min_i; i < max_i; ++i)
|
||||
{
|
||||
for (unsigned int j = min_j; j < max_j; ++j)
|
||||
|
|
@ -310,7 +327,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
|
|||
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
|
||||
// Transform from global_frame_ to map_frame_
|
||||
tf3::Vector3 p(wx, wy, 0);
|
||||
p = tf2_transform*p;
|
||||
p = tf3_transform*p;
|
||||
// Set master_grid with cell from map
|
||||
if (worldToMap(p.x(), p.y(), mx, my))
|
||||
{
|
||||
|
|
|
|||
|
|
@ -65,15 +65,15 @@ bool VoxelLayer::getParams()
|
|||
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
|
||||
YAML::Node layer = config["voxel_layer"];
|
||||
|
||||
publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
|
||||
// publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
|
||||
enabled_ = loadParam(layer, "enabled", true);
|
||||
footprint_clearing_enabled_ = loadParam(layer, "footprint_clearing_enabled", true);
|
||||
max_obstacle_height_ = loadParam(layer, "max_obstacle_height", true);
|
||||
size_z_ = loadParam(layer, "z_voxels", true);
|
||||
origin_z_ = loadParam(layer, "origin_z", true);
|
||||
z_resolution_ = loadParam(layer, "z_resolution", true);
|
||||
unknown_threshold_ = loadParam(layer, "max_obstacle_height", true);
|
||||
mark_threshold_ = loadParam(layer, "mark_threshold", true);
|
||||
max_obstacle_height_ = loadParam(layer, "max_obstacle_height", 2.0);
|
||||
size_z_ = loadParam(layer, "z_voxels", 10);
|
||||
origin_z_ = loadParam(layer, "origin_z", 0);
|
||||
z_resolution_ = loadParam(layer, "z_resolution", 0.2);
|
||||
unknown_threshold_ = loadParam(layer, "max_obstacle_height", 15);
|
||||
mark_threshold_ = loadParam(layer, "mark_threshold", 0);
|
||||
combination_method_ = loadParam(layer, "combination_method", true);
|
||||
this->matchSize();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,15 +35,20 @@
|
|||
* Author: Eitan Marder-Eppstein
|
||||
* David V. Lu!!
|
||||
*********************************************************************/
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <costmap_2d/utils.h>
|
||||
|
||||
#include <tf3/convert.h>
|
||||
#include <tf3/utils.h>
|
||||
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <exception>
|
||||
|
||||
// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -85,7 +90,7 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
|
|||
// 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
|
||||
// // // 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))
|
||||
// {
|
||||
|
|
@ -174,8 +179,58 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
|
|||
|
||||
void Costmap2DROBOT::getParams()
|
||||
{
|
||||
// private_nh.param("global_frame", global_frame_, std::string("map"));
|
||||
// private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
|
||||
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
|
||||
{
|
||||
// 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());
|
||||
}
|
||||
}
|
||||
// }
|
||||
// }
|
||||
}
|
||||
|
||||
void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
|
||||
|
|
@ -575,17 +630,17 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
|
|||
// get the global pose of the robot
|
||||
try
|
||||
{
|
||||
// // use current time if possible (makes sure it's not in the future)
|
||||
// if (tf_.canTransform(global_frame_, robot_base_frame_, current_time))
|
||||
// {
|
||||
// geometry_msgs::TransformStamped transform = tf_.lookupTransform(global_frame_, robot_base_frame_, current_time);
|
||||
// tf3::doTransform(robot_pose, global_pose, transform);
|
||||
// }
|
||||
// // use the latest otherwise
|
||||
// else
|
||||
// {
|
||||
// tf_.transform(robot_pose, global_pose, global_frame_);
|
||||
// }
|
||||
// use current time if possible (makes sure it's not in the future)
|
||||
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);
|
||||
}
|
||||
// use the latest otherwise
|
||||
else
|
||||
{
|
||||
// tf_.transform(robot_pose, global_pose, global_frame_);
|
||||
}
|
||||
}
|
||||
catch (tf3::LookupException& ex)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#include <costmap_2d/observation_buffer.h>
|
||||
#include <costmap_2d/data_convert.h>
|
||||
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
// #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
|
||||
// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
// #include <tf3_sensor_msgs/tf3_sensor_msgs.h>
|
||||
#include<tf3/convert.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <cstdint>
|
||||
|
|
@ -16,9 +16,9 @@ namespace costmap_2d
|
|||
{
|
||||
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
|
||||
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
|
||||
double raytrace_range, tf3::BufferCore& tf2_buffer, string global_frame,
|
||||
double raytrace_range, tf3::BufferCore& tf3_buffer, string global_frame,
|
||||
string sensor_frame, double tf_tolerance) :
|
||||
tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
|
||||
tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
|
||||
last_updated_(robot::Time::now()),
|
||||
global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
|
||||
min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
|
||||
|
|
@ -36,7 +36,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
|||
|
||||
std::string tf_error;
|
||||
|
||||
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time::now(), &tf_error))
|
||||
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time::now(), &tf_error))
|
||||
{
|
||||
printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
|
||||
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
|
||||
|
|
@ -57,14 +57,14 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
|||
|
||||
// we need to transform the origin of the observation to the new global frame
|
||||
tf3::doTransform(origin, origin,
|
||||
tf2_buffer_.lookupTransform(new_global_frame,
|
||||
tf3_buffer_.lookupTransform(new_global_frame,
|
||||
origin.header.frame_id,
|
||||
convertTime(origin.header.stamp)));
|
||||
obs.origin_ = origin.point;
|
||||
|
||||
// we also need to transform the cloud of the observation to the new global frame
|
||||
tf3::doTransform(*(obs.cloud_), *(obs.cloud_),
|
||||
tf2_buffer_.lookupTransform(new_global_frame,
|
||||
tf3_buffer_.lookupTransform(new_global_frame,
|
||||
obs.cloud_->header.frame_id,
|
||||
convertTime(obs.cloud_->header.stamp)));
|
||||
}
|
||||
|
|
@ -101,7 +101,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
|||
local_origin.point.y = 0;
|
||||
local_origin.point.z = 0;
|
||||
tf3::doTransform(local_origin, global_origin,
|
||||
tf2_buffer_.lookupTransform(global_frame_,
|
||||
tf3_buffer_.lookupTransform(global_frame_,
|
||||
local_origin.header.frame_id,
|
||||
convertTime(local_origin.header.stamp)));
|
||||
tf3::convert(global_origin.point, observation_list_.front().origin_);
|
||||
|
|
@ -114,7 +114,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
|||
|
||||
// transform the point cloud
|
||||
tf3::doTransform(cloud, global_frame_cloud,
|
||||
tf2_buffer_.lookupTransform(global_frame_,
|
||||
tf3_buffer_.lookupTransform(global_frame_,
|
||||
(cloud.header.frame_id),
|
||||
convertTime(cloud.header.stamp)));
|
||||
global_frame_cloud.header.stamp = cloud.header.stamp;
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user