add file nav_msgs

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()

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 <cstdint>
+#include <string>
+#include <vector>
+#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<geometry_msgs::Point> cells;
+};
+
+}
+
+#endif //GRID_CELLS_H

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 <cstdint>
+#include <string>
+#include <vector>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Point.h"
+#include <geometry_msgs/PoseWithCovariance.h>
+#include <geometry_msgs/TwistWithCovariance.h>
+
+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

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 <cstdint>
+#include <string>
+#include <vector>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/PoseStamped.h"
+
+namespace nav_msgs
+{
+
+struct Path
+{
+    std_msgs::Header header;
+    std::vector<geometry_msgs::PoseStamped> poses;
+};
+
+}
+
+#endif //PATH_H
+

sensor_msgs/cfg/config.yaml

@@ -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

sensor_msgs/test/main.cpp

@@ -2,38 +2,6 @@
 #include "sensor_msgs/JoyFeedbackArray.h"
 #include "sensor_msgs/PointCloud2.h"
 #include <iostream>
-// #include <yaml-cpp/yaml.h>
-
-// bool getParams()
-// {
-//     try {
-//         YAML::Node config = YAML::LoadFile("../cfg/config.yaml");
-
-//         double cost_scaling_factor = config["inflation_layer"]["cost_scaling_factor"].as<double>();
-//         double inflation_radius = config["inflation_layer"]["inflation_radius"].as<double>();
-
-//         bool enabled = config["inflation_layer"]["enabled"].as<bool>();
-//         bool inflate_unknown = config["inflation_layer"]["inflate_unknown"].as<bool>();
-
-//         double distance = config["obstacle_layer"]["distance"].as<double>();
-
-//         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()
 {