From 4c8e350dac1527efcb54aee83bab34d59a3c692f Mon Sep 17 00:00:00 2001 From: duongtd Date: Thu, 13 Nov 2025 16:51:49 +0700 Subject: [PATCH] add file nav_msgs --- CMakeLists.txt | 6 ++--- nav_msgs/include/nav_msgs/GridCells.h | 28 ++++++++++++++++++++++ nav_msgs/include/nav_msgs/MapMetaData.h | 10 ++++---- nav_msgs/include/nav_msgs/Odometry.h | 32 +++++++++++++++++++++++++ nav_msgs/include/nav_msgs/Path.h | 25 +++++++++++++++++++ sensor_msgs/cfg/config.yaml | 7 ------ sensor_msgs/test/main.cpp | 32 ------------------------- 7 files changed, 93 insertions(+), 47 deletions(-) create mode 100644 nav_msgs/include/nav_msgs/GridCells.h create mode 100644 nav_msgs/include/nav_msgs/Odometry.h create mode 100644 nav_msgs/include/nav_msgs/Path.h delete mode 100644 sensor_msgs/cfg/config.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 16d90ef..0258689 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,12 +6,12 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) if (NOT TARGET std_msgs) add_subdirectory(std_msgs) endif() -if (NOT TARGET sensor_msgs) - add_subdirectory(sensor_msgs) -endif() if (NOT TARGET geometry_msgs) add_subdirectory(geometry_msgs) endif() +if (NOT TARGET sensor_msgs) + add_subdirectory(sensor_msgs) +endif() if (NOT TARGET nav_msgs) add_subdirectory(nav_msgs) endif() \ No newline at end of file diff --git a/nav_msgs/include/nav_msgs/GridCells.h b/nav_msgs/include/nav_msgs/GridCells.h new file mode 100644 index 0000000..6115595 --- /dev/null +++ b/nav_msgs/include/nav_msgs/GridCells.h @@ -0,0 +1,28 @@ +// #an array of cells in a 2D grid +// Header header +// float32 cell_width +// float32 cell_height +// geometry_msgs/Point[] cells +#ifndef GRID_CELLS_H +#define GRID_CELLS_H + +#include +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Point.h" + +namespace nav_msgs +{ + +struct GridCells +{ + std_msgs::Header header; + float cell_width; + float cell_height; + std::vector cells; +}; + +} + +#endif //GRID_CELLS_H \ No newline at end of file diff --git a/nav_msgs/include/nav_msgs/MapMetaData.h b/nav_msgs/include/nav_msgs/MapMetaData.h index 7504af4..10ba6a3 100644 --- a/nav_msgs/include/nav_msgs/MapMetaData.h +++ b/nav_msgs/include/nav_msgs/MapMetaData.h @@ -11,11 +11,11 @@ namespace nav_msgs struct MapMetaData { -double map_load_time; -float resolution; -uint32_t width; -uint32_t height; -geometry_msgs::Pose origin; + double map_load_time; + float resolution; + uint32_t width; + uint32_t height; + geometry_msgs::Pose origin; }; } diff --git a/nav_msgs/include/nav_msgs/Odometry.h b/nav_msgs/include/nav_msgs/Odometry.h new file mode 100644 index 0000000..79dd904 --- /dev/null +++ b/nav_msgs/include/nav_msgs/Odometry.h @@ -0,0 +1,32 @@ +// # This represents an estimate of a position and velocity in free space. +// # The pose in this message should be specified in the coordinate frame given by header.frame_id. +// # The twist in this message should be specified in the coordinate frame given by the child_frame_id +// Header header +// string child_frame_id +// geometry_msgs/PoseWithCovariance pose +// geometry_msgs/TwistWithCovariance twist +#ifndef ODOMETRY_H +#define ODOMETRY_H + +#include +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Point.h" +#include +#include + +namespace nav_msgs +{ + +struct Odometry +{ + std_msgs::Header header; + std::string child_frame_id; + geometry_msgs::PoseWithCovariance pose; + geometry_msgs::TwistWithCovariance twist; +}; + +} + +#endif //ODOMETRY_H \ No newline at end of file diff --git a/nav_msgs/include/nav_msgs/Path.h b/nav_msgs/include/nav_msgs/Path.h new file mode 100644 index 0000000..6b8bbf6 --- /dev/null +++ b/nav_msgs/include/nav_msgs/Path.h @@ -0,0 +1,25 @@ +// #An array of poses that represents a Path for a robot to follow +// Header header +// geometry_msgs/PoseStamped[] poses +#ifndef PATH_H +#define PATH_H + +#include +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/PoseStamped.h" + +namespace nav_msgs +{ + +struct Path +{ + std_msgs::Header header; + std::vector poses; +}; + +} + +#endif //PATH_H + diff --git a/sensor_msgs/cfg/config.yaml b/sensor_msgs/cfg/config.yaml deleted file mode 100644 index fea614d..0000000 --- a/sensor_msgs/cfg/config.yaml +++ /dev/null @@ -1,7 +0,0 @@ -inflation_layer: - enabled: true - inflate_unknown: false - cost_scaling_factor: 15.0 - inflation_radius: 0.55 -obstacle_layer: - distance: 0.15 \ No newline at end of file diff --git a/sensor_msgs/test/main.cpp b/sensor_msgs/test/main.cpp index b054655..f52a646 100644 --- a/sensor_msgs/test/main.cpp +++ b/sensor_msgs/test/main.cpp @@ -2,38 +2,6 @@ #include "sensor_msgs/JoyFeedbackArray.h" #include "sensor_msgs/PointCloud2.h" #include -// #include - -// bool getParams() -// { -// try { -// YAML::Node config = YAML::LoadFile("../cfg/config.yaml"); - -// double cost_scaling_factor = config["inflation_layer"]["cost_scaling_factor"].as(); -// double inflation_radius = config["inflation_layer"]["inflation_radius"].as(); - -// bool enabled = config["inflation_layer"]["enabled"].as(); -// bool inflate_unknown = config["inflation_layer"]["inflate_unknown"].as(); - -// double distance = config["obstacle_layer"]["distance"].as(); - -// std::cout << "Inflation Layer Parameters:" << std::endl; -// std::cout << " Cost Scaling Factor: " << cost_scaling_factor << std::endl; -// std::cout << " Inflation Radius: " << inflation_radius << std::endl; -// std::cout << " Enabled: " << (enabled ? "true" : "false") << std::endl; -// std::cout << " Inflate Unknown: " << (inflate_unknown ? "true" : "false") << std::endl; -// std::cout << "Obstacle Layer Parameters:" << std::endl; -// std::cout << " Distance: " << distance << std::endl; - -// } -// catch (const YAML::BadFile& e) { -// std::cerr << "Cannot open YAML file: " << e.what() << std::endl; -// return false; -// } - -// return true; - -// } int main() {