Resolved comments

asv_setup/launch/tf.launch.py

@@ -69,7 +69,7 @@ def generate_launch_description():
                    '--pitch'          , '0',
                    '--yaw'            , '0',
                    '--frame-id'       , 'base_link',
-                   '--child-frame-id' , 'seapath_frame_pose'],
+                   '--child-frame-id' , 'seapath_frame'],
     )
 
     return LaunchDescription([

mission/landmark_server/CMakeLists.txt

@@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD)
   set(CMAKE_C_STANDARD 99)
 endif()
 
-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

mission/landmark_server/include/landmark_server/landmark_server.hpp

@@ -4,6 +4,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <thread>
+#include <sstream>
 
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <vortex_msgs/action/filtered_landmarks.hpp>
@@ -137,15 +138,6 @@ class LandmarkServerNode : public rclcpp::Node {
                    vortex_msgs::action::FilteredLandmarks>>
                    goal_handle);
 
-  /**
-   * Calculates the distance between the landmark and the drone.
-   *
-   * @param pose The pose of the landmark.
-   * @param target_frame The frame to which data should be transformed.
-   * @param source_frame The frame where the data originated
-   * @return The distance between the landmark and the drone.
-   */
-
   /**
    * @brief Logs messages based on request.
    *

mission/landmark_server/package.xml

@@ -4,7 +4,7 @@
   <name>landmark_server</name>
   <version>0.0.1</version>
   <description>ROS2 action server that handles the storage of landmarks and publishing based on the action request</description>
-  <maintainer email="[email protected]">jorgen Fjermedal</maintainer>
+  <maintainer email="[email protected]">Jorgen Fjermedal</maintainer>
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

mission/landmark_server/src/landmark_server.cpp

@@ -47,7 +47,7 @@ void LandmarkServerNode::landmarksRecievedCallback(
     "Received empty landmark array");
     return;
   }
-  const auto &pose = msg->landmarks[0].odom.pose.pose;
+
   for (const auto &landmark : msg->landmarks) {
     // RCLCPP_INFO(this->get_logger(), "Landmarks received");
 
@@ -63,7 +63,7 @@ void LandmarkServerNode::landmarksRecievedCallback(
                          }),
           storedLandmarks_->landmarks.end());
     } else if (landmark.action == 1) {
-      // Find if the landmark already exists
+      // Find the landmark if it already exists
       auto it = std::find_if(
           storedLandmarks_->landmarks.begin(),
           storedLandmarks_->landmarks.end(), [&](const auto &storedLandmark) {
@@ -225,38 +225,35 @@ vortex_msgs::msg::OdometryArray LandmarkServerNode::filterLandmarks(
   return filteredLandmarksOdoms;
 }
 
-void LandmarkServerNode::requestLogger(
-    const std::shared_ptr<
-        rclcpp_action::ServerGoalHandle<vortex_msgs::action::FilteredLandmarks>>
-        goal_handle) {
-  const auto goal = goal_handle->get_goal();
-  double distance = goal->distance;
-
-  if (distance == 0.0 && goal->landmark_types.empty()) {
-    RCLCPP_INFO(this->get_logger(), "Received request to return all landmarks");
-  } else if (goal->landmark_types.empty()) {
-    RCLCPP_INFO(this->get_logger(),
-                "Received request to return all landmarks within distance %f",
-                distance);
-  } else {
-    // Log the request to return landmarks by type filter
-    std::string types_log =
-        "Received request to return landmarks by type filter: [";
-
-    for (const auto &type : goal->landmark_types) {
-      types_log += type + ", ";
-    }
-
-    // Remove the trailing comma and space
-    if (!goal->landmark_types.empty()) {
-      types_log = types_log.substr(0, types_log.size() - 2);
-    }
-
-    types_log += "]";
-
-    RCLCPP_INFO(this->get_logger(), types_log.c_str());
-  }
-}
+void LandmarkServerNode::requestLogger(  
+    const std::shared_ptr<  
+        rclcpp_action::ServerGoalHandle<vortex_msgs::action::FilteredLandmarks>>  
+        goal_handle) {  
+  const auto goal = goal_handle->get_goal();  
+  double distance = goal->distance;  
+
+  if (distance == 0.0 && goal->landmark_types.empty()) {  
+    RCLCPP_INFO_STREAM(this->get_logger(), "Received request to return all landmarks.");  
+    return;  
+  }  
+
+  if (goal->landmark_types.empty()) {  
+    RCLCPP_INFO_STREAM(this->get_logger(),  
+                "Received request to return all landmarks within distance " << distance << ".");  
+    return;  
+  }  
+
+  std::stringstream types_log;  
+  types_log << "Received request to return landmarks by type filter: [";  
+  for (auto it = goal->landmark_types.begin(); it != goal->landmark_types.end(); it++) {  
+    types_log << *it;  
+    if (std::next(it) != goal->landmark_types.end()) {  
+      types_log << ", ";  
+    }  
+  }  
+  types_log << "].";  
+  RCLCPP_INFO_STREAM(this->get_logger(), types_log.str());  
+}  
 
 double
 LandmarkServerNode::calculateDistance(const geometry_msgs::msg::Point &point,