114 task landmark server v2 (#132)

* basic landmarkServer to handle landmarks and publish * saved changed, reimplementing with array-msg * basic publishers run, working on velocity visualization * Basic action server and grid_visualization class * removed OccupancyGrid related stuff and added transform for landmarks * Committing clang-format changes * renamed from landmarks to landmark_server. Added README.md * added functionality for multiple requests * Committing clang-format changes * added debugger logging * Committing clang-format changes * Fixed Logger messages and made it so server ignores distance filter when transform fails * Resolved comments * Committing clang-format changes * Helped jorgen * Committing clang-format changes --------- Co-authored-by: Clang Robot <[email protected]> Co-authored-by: etfroeland <[email protected]>
vortexntnu · Mar 3, 2024 · c69f871 · c69f871
1 parent 449ea70
commit c69f871
Show file tree

Hide file tree

Showing 7 changed files with 633 additions and 0 deletions.
diff --git a/asv_setup/launch/tf.launch.py b/asv_setup/launch/tf.launch.py
@@ -57,6 +57,21 @@ def generate_launch_description():
                    '--child-frame-id' , 'zed2i_camera_center'],
     )
 
+    # base_link (NED) to seapath_frame_pose (NED?) tf.
+    tf_base_link_ned_to_seapath_frame_pose = Node(
+        package='tf2_ros',
+        name='base_link_ned_to_seapath_frame_pose',
+        executable='static_transform_publisher',
+        arguments=['--x'              , '0',
+                   '--y'              , '0',
+                   '--z'              , '0',
+                   '--roll'           , '0',
+                   '--pitch'          , '0',
+                   '--yaw'            , '0',
+                   '--frame-id'       , 'base_link',
+                   '--child-frame-id' , 'seapath_frame'],
+    )
+
     return LaunchDescription([
         tf_base_link_ned_to_base_link_seu,
         tf_base_link_to_lidar,

diff --git a/mission/landmark_server/CMakeLists.txt b/mission/landmark_server/CMakeLists.txt
@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.8)
+project(landmark_server)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+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)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+include_directories(include)
+
+# Create an executable
+add_executable(landmark_server_node 
+    src/landmark_server.cpp
+    src/landmark_server_main.cpp)
+
+target_link_libraries( ${PROJECT_NAME}_node
+    pcl_common)
+
+ament_target_dependencies(landmark_server_node
+    rclcpp
+    std_msgs
+    vortex_msgs
+    nav_msgs
+    rclcpp_action
+    rclcpp_components
+    tf2
+    tf2_geometry_msgs
+  )
+
+    install(TARGETS ${PROJECT_NAME}_node
+    DESTINATION lib/${PROJECT_NAME}
+  )
+
+  install(DIRECTORY
+    DESTINATION share/${PROJECT_NAME}/
+  )
+
+
+
+
+ament_package()
diff --git a/mission/landmark_server/README.md b/mission/landmark_server/README.md
@@ -0,0 +1,32 @@
+# Landmark Server
+this ROS-node serves as an interface between the perception system and the control system. The node receives messages of type landmarkArray defined `vortex-msgs/msg/LandmarkArray.msg`. 
+The landmarks are recieved through a topic published by a node in the `vortex-target-tracking`repo. This server then publishes the odometries of these landmarks on a topic the control system suscribes to. 
+The landmark server node is configured as an action server allowing the control systems to filter what landmarks they want to recieve.
+
+## Storing Landmarks
+The landmark server receives the landmarks and stores them in the class member `storedLandmarks_`. The server handles logic for adding, updating and removing landmarks.
+
+## Publishers
+The landmark server supports publising of all the landmarks currently stored by the server over the `landmarks_out` topic.
+The server also supports publishing the poses for all the currently stored landmarks over the `landmark_poses_out`. These poses are published as a PoseArray message that can be visualized in the foxglove 3D-panel.
+
+## Action
+The control systems can send action request for the FilteredLandmarks action defined in `vortex-msgs/action/FilteredLandmarks.action`. The control systems can filter landmarks by type and distance in the action request.
+
+```yaml
+float32 IGNORE_DISTANCE =0.0
+
+# Define the action request
+string[] landmark_types
+float32  distance
+string  frame_id
+```
+The server returns the odometries of landmarks that have a landmark type that matches one of the elements in `string[] landmark_types` and is withing the distance specified in the request.
+The distance is the relative distance between the landmark and the drone. 
+If `string[] landmark_types` is empty then it will accept all types within the specified distance.
+If distance is set to `0.0` the distance is ignored and the server returns all landmarks matching the `string[] landmark_types` filter.
+`string  frame_id` is used to specify which frame is to be used on the transformation to calculate the relative distance between the landmark and the drone.
+
+
+
+
diff --git a/mission/landmark_server/include/landmark_server/landmark_server.hpp b/mission/landmark_server/include/landmark_server/landmark_server.hpp
@@ -0,0 +1,182 @@
+#ifndef LANDMARK_SERVER_HPP
+#define LANDMARK_SERVER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <sstream>
+#include <thread>
+
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <vortex_msgs/action/filtered_landmarks.hpp>
+#include <vortex_msgs/msg/landmark.hpp>
+#include <vortex_msgs/msg/landmark_array.hpp>
+#include <vortex_msgs/msg/odometry_array.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <tf2/transform_datatypes.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace landmark_server {
+
+/**
+ * @class LandmarkServerNode
+ * @brief A class representing a node for handling landmarks in a ROS 2 system.
+ *
+ * This class inherits from rclcpp::Node and provides functionality for
+ * receiving landmark array messages, publishing the poses of the landmarks,
+ * handling filtered landmarks using an action server, and calculating the
+ * distance between the poses and the drone.
+ */
+class LandmarkServerNode : public rclcpp::Node {
+public:
+  /**
+   * @brief Constructor for the LandmarkServerNode class.
+   *
+   * @param options The options for configuring the node.
+   */
+  explicit LandmarkServerNode(
+      const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
+
+  /**
+   * @brief Destructor for the LandmarkServerNode class.
+   */
+  ~LandmarkServerNode(){};
+
+protected:
+  /**
+   * @brief A shared pointer to a subscription object for receiving
+   * vortex_msgs::msg::LandmarkArray messages.
+   */
+  rclcpp::Subscription<vortex_msgs::msg::LandmarkArray>::SharedPtr
+      landmark_sub_;
+
+  /**
+   * @brief Callback function for receiving landmark array messages.
+   *
+   * This function is called when a landmark array message of type
+   * vortex_msgs::msg::LandmarkArray::SharedPtr is received.
+   *
+   * @param msg The shared pointer to the received landmark array message.
+   */
+  void landmarksRecievedCallback(
+      const vortex_msgs::msg::LandmarkArray::SharedPtr msg);
+
+  /**
+   * @brief A shared pointer to a publisher for the LandmarkArray message type.
+   * Publishes all landmarks currently stored in the server.
+   */
+  rclcpp::Publisher<vortex_msgs::msg::LandmarkArray>::SharedPtr
+      landmarkPublisher_;
+
+  /**
+   * @brief A shared pointer to a publisher for geometry_msgs::msg::PoseArray.
+   * Publishes the pose of all landmarks currently stored in the server.
+   */
+  rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr posePublisher_;
+
+  /**
+   * @brief A shared pointer to a LandmarkArray message.
+   * The array contains all landmarks currently stored by the server.
+   */
+  std::shared_ptr<vortex_msgs::msg::LandmarkArray> storedLandmarks_;
+
+  /**
+   * @brief A shared pointer to an rclcpp_action server for handling filtered
+   * landmarks.
+   */
+  rclcpp_action::Server<vortex_msgs::action::FilteredLandmarks>::SharedPtr
+      action_server_;
+
+  /**
+   * @brief Handles the goal request for the `handle_goal` function.
+   *
+   * This function is responsible for processing the goal request for the
+   * `handle_goal` action.
+   *
+   * @param uuid The unique identifier of the goal request.
+   * @param goal A shared pointer to the goal message containing the requested
+   * goal.
+   * @return The response to the goal request.
+   */
+  rclcpp_action::GoalResponse handle_goal(
+      const rclcpp_action::GoalUUID &uuid,
+      std::shared_ptr<const vortex_msgs::action::FilteredLandmarks::Goal> goal);
+
+  /**
+   * @brief Handles the cancellation of a goal.
+   *
+   * This function is called when a goal is cancelled by the client.
+   *
+   * @param goal_handle The goal handle associated with the cancelled goal.
+   * @return The response indicating the result of the cancellation.
+   */
+  rclcpp_action::CancelResponse
+  handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+                    vortex_msgs::action::FilteredLandmarks>>
+                    goal_handle);
+
+  /**
+   * @brief Handles the accepted goal for the FilteredLandmarks action server.
+   *
+   * This function is called when a goal is accepted by the action server.
+   *
+   * @param goal_handle The goal handle for the accepted goal.
+   */
+  void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+                           vortex_msgs::action::FilteredLandmarks>>
+                           goal_handle);
+
+  /**
+   * @brief Executes the action server goal handle for the FilteredLandmarks
+   * action.
+   *
+   * @param goal_handle The goal handle for the FilteredLandmarks action.
+   */
+  void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+                   vortex_msgs::action::FilteredLandmarks>>
+                   goal_handle);
+
+  /**
+   * @brief Logs messages based on request.
+   *
+   * This function is responsible for logging messages based on the request
+   *
+   * @param goal_handle A shared pointer to the goal handle.
+   */
+  void requestLogger(const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+                         vortex_msgs::action::FilteredLandmarks>>
+                         goal_handle);
+
+  /**
+   * Calculates the distance between a landmark and the drone.
+   *
+   * @param point The point to calculate the distance from.
+   * @param header The header to calculate the distance to.
+   * @return The distance between the landmark and the drone.
+   */
+  double calculateDistance(const geometry_msgs::msg::Point &point,
+                           const std_msgs::msg::Header &header);
+
+  /**
+   * @brief Creates a pose array from a landmark array.
+   * @param landmarks The landmark array.
+   * @return The pose array.
+   */
+  geometry_msgs::msg::PoseArray
+  poseArrayCreater(vortex_msgs::msg::LandmarkArray landmarks);
+
+  // Declare tf_buffer_ and tf_listener_ as class members
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+
+  vortex_msgs::msg::OdometryArray
+  filterLandmarks(const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+                      vortex_msgs::action::FilteredLandmarks>>
+                      goal_handle);
+};
+
+} // namespace landmark_server
+
+#endif // LANDMARK_SERVER_HPP
diff --git a/mission/landmark_server/package.xml b/mission/landmark_server/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <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>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+
+
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>