Basic action server and grid_visualization class

mission/landmarks/CMakeLists.txt

@@ -21,13 +21,16 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(vortex_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
 
 include_directories(include)
 
 # Create an executable
 add_executable(landmarks_node 
     src/landmarks.cpp
-    src/landmarks_main.cpp)
+    src/landmarks_main.cpp
+    src/grid_visualization.cpp)
 
 target_link_libraries( ${PROJECT_NAME}_node
     pcl_common)
@@ -36,7 +39,9 @@ ament_target_dependencies(landmarks_node
     rclcpp
     std_msgs
     vortex_msgs
-    nav_msgs)
+    nav_msgs
+    rclcpp_action
+    rclcpp_components)
 
     install(TARGETS ${PROJECT_NAME}_node
     DESTINATION lib/${PROJECT_NAME}

mission/landmarks/include/landmarks/grid_visualization.hpp

@@ -0,0 +1,33 @@
+#ifndef GRID_VISUALIZATION_HPP
+#define GRID_VISUALIZATION_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <vortex_msgs/msg/landmark_array.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+
+namespace grid_visualization {
+
+class GridVisualization  {
+public:
+    explicit GridVisualization();
+
+    ~GridVisualization() {};
+
+    /**
+     * @brief Creates an occupancy grid.
+     * 
+     * @return The created occupancy grid.
+     */
+    nav_msgs::msg::OccupancyGrid createGrid(const vortex_msgs::msg::LandmarkArray &landmarkArray);
+
+    geometry_msgs::msg::PoseArray poseArrayCreater(vortex_msgs::msg::LandmarkArray landmarks);
+
+
+
+
+};
+
+}  // namespace GRID_VISUALIZATION
+
+#endif // GRID_VISUALIZATION_HPP

mission/landmarks/include/landmarks/landmarks.hpp

@@ -5,9 +5,15 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <vortex_msgs/msg/landmark.hpp>
 #include <vortex_msgs/msg/landmark_array.hpp>
-#include <vector>  
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
+#include <landmarks/grid_visualization.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <vortex_msgs/action/filtered_landmarks.hpp>
+#include "rclcpp_components/register_node_macro.hpp"
+#include <vortex_msgs/msg/odometry_array.hpp>
+#include <thread>
+
 
 namespace landmarks {
 
@@ -23,16 +29,16 @@ class LandmarksNode : public rclcpp::Node {
     rclcpp::Publisher<vortex_msgs::msg::LandmarkArray>::SharedPtr landmarkPublisher_;
     rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr gridPublisher_;
     rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr posePublisher_;
-    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twistPublisher_;
     std::shared_ptr<vortex_msgs::msg::LandmarkArray> storedLandmarks_;
-    nav_msgs::msg::OccupancyGrid createGrid(vortex_msgs::msg::LandmarkArray landmarks);
-    nav_msgs::msg::OccupancyGrid grid_;
-    // maybe use timer based publishing?
-    // void publishLandmarks();
-    void updateGrid(vortex_msgs::msg::Landmark landmark,int number);
-    geometry_msgs::msg::PoseArray poseArrayCreater(vortex_msgs::msg::LandmarkArray landmarks);
-    void twistCreater(vortex_msgs::msg::LandmarkArray landmarks);
-
+    std::shared_ptr<grid_visualization::GridVisualization> gridVisualization_;
+
+    rclcpp_action::Server<vortex_msgs::action::FilteredLandmarks>::SharedPtr action_server_;
+    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const vortex_msgs::action::FilteredLandmarks::Goal> goal);
+    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<vortex_msgs::action::FilteredLandmarks>> goal_handle);
+    void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<vortex_msgs::action::FilteredLandmarks>> goal_handle);
+    void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<vortex_msgs::action::FilteredLandmarks>> goal_handle);
+
+
 };
 
 }  // namespace landmarks

mission/landmarks/src/grid_visualization.cpp

@@ -0,0 +1,48 @@
+#include <landmarks/grid_visualization.hpp>
+
+namespace grid_visualization {
+
+GridVisualization::GridVisualization() {}
+
+nav_msgs::msg::OccupancyGrid GridVisualization::createGrid(
+    const vortex_msgs::msg::LandmarkArray &landmarkArray) {
+  // Assuming some parameters for the occupancy grid
+  int grid_width = 100;
+  int grid_height = 100;
+
+  // Initialize the OccupancyGrid message
+  nav_msgs::msg::OccupancyGrid grid;
+  grid.header.frame_id = "map";
+  grid.info.width = grid_width;
+  grid.info.height = grid_height;
+  grid.info.resolution = 0.1; // physical distance between two cells in meters
+
+  // Initialize the data array
+  grid.data.resize(grid_width * grid_height,
+                   0); // Initialize unoccupied grid
+  for (const auto &landmark : landmarkArray.landmarks) {
+
+    int x = landmark.odom.pose.pose.position.x / grid.info.resolution;
+    int y = landmark.odom.pose.pose.position.y / grid.info.resolution;
+
+    // Check bounds
+    if (x >= 0 && x < grid.info.width && y >= 0 && y < grid.info.height) {
+      // Increment the grid index
+      grid.data[x + y * grid.info.width]++;
+    }
+  }
+
+  return grid;
+}
+
+geometry_msgs::msg::PoseArray GridVisualization::poseArrayCreater(vortex_msgs::msg::LandmarkArray landmarks) {
+  geometry_msgs::msg::PoseArray poseArray;
+  poseArray.header.frame_id = "map";
+  for (const auto &landmark : landmarks.landmarks) {
+    // Convert landmark to pose here...
+    poseArray.poses.push_back(landmark.odom.pose.pose);
+  }
+  return poseArray;
+}
+
+} // namespace grid_visualization