docking task

control logic for the asv to navigate through the initial buoy formation of the docking task

mission/docking_task/CMakeLists.txt

@@ -0,0 +1,77 @@
+cmake_minimum_required(VERSION 3.8)
+project(docking_task)
+
+# 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(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+include_directories(include)
+
+add_executable(docking_task_node
+  src/docking_task_node.cpp
+  src/docking_task_ros.cpp)
+
+# target_include_directories(dock_localization_node PUBLIC
+#   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#   $<INSTALL_INTERFACE:include>)
+
+# target_compile_features(dock_localization_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+target_link_libraries(docking_task_node
+  tf2::tf2
+  tf2_ros::tf2_ros
+  tf2_geometry_msgs::tf2_geometry_msgs
+)
+
+ament_target_dependencies(docking_task_node 
+  rclcpp
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  vortex_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+
+)
+
+install(TARGETS
+  ${PROJECT_NAME}_node
+  DESTINATION lib/${PROJECT_NAME}/
+  )
+
+install(DIRECTORY
+  launch
+  params
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+
+ament_package()

mission/docking_task/include/docking_task/docking_task_ros.hpp

@@ -0,0 +1,158 @@
+#ifndef DOCKING_TASK_ROS_HPP
+#define DOCKING_TASK_ROS_HPP
+
+#include <Eigen/Dense>
+#include <thread>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <vortex_msgs/msg/landmark_array.hpp>
+#include <vortex_msgs/srv/waypoint.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2/LinearMath/Matrix3x3.h> 
+
+namespace docking_task {
+
+struct LandmarkWithID {
+      geometry_msgs::msg::Pose pose_map_frame;
+      geometry_msgs::msg::Pose pose_base_link_frame;
+      uint32_t id;
+    };
+
+/**
+ * @class DockingTaskNode
+ * @brief A class representing a node for handling dock localization in a ROS 2 system.
+ *
+ * This class inherits from rclcpp::Node and provides functionality for
+ * localizing the dock in the map.
+ */
+class DockingTaskNode : public rclcpp::Node {
+public:
+  /**
+   * @brief Constructor for the DockingTaskNode class.
+   *
+   * @param options The options for configuring the node.
+   */
+  explicit DockingTaskNode(
+      const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
+
+  /**
+   * @brief Destructor for the DockLocalizationNode class.
+   */
+  ~DockingTaskNode(){};
+
+  /**
+   * @brief Main task for the DockingTaskNode class.
+   */
+  void main_task();
+
+  /**
+   * @brief Detect the closest buoy pair and set a waypoint between them
+   *
+   * @return A pair of landmarks representing the closest buoy pair
+   */
+  std::pair<LandmarkWithID, LandmarkWithID> initial_waypoint();
+
+  /**
+   * @brief Predict the 6-tuple formation of buoys
+   *
+   * @param landmark1 The first landmark/buoy used for initial waypoint
+   * @param landmark2 The second landmark/buoy used for initial waypoint
+   * 
+   * @return The predicted posistion of the 6-tuple formation of buoys in odom frame
+   */
+  Eigen::Array<double, 2, 6> predict_buoy_formation(const LandmarkWithID &buoy1, const LandmarkWithID &buoy2) const;
+
+  /**
+   * @brief Generate the reward matrix for the auction algorithm
+   *
+   * @param predicted_positions The predicted positions of the buoys
+   * @param measured_positions The measured positions of landmarks
+   * 
+   * @return The reward matrix for the auction algorithm
+   */
+  Eigen::MatrixXd generate_reward_matrix(const Eigen::Array<double, 2, 6>& predicted_positions, const Eigen::MatrixXd& measured_positions);
+
+  /**
+   * @brief The auction algorithm for the assignment problem
+   *
+   * @param reward_matrix The reward matrix for the auction algorithm
+   * 
+   * @return The assignment of landmarks to buoys
+   */
+  Eigen::VectorXi auction_algorithm(const Eigen::MatrixXd &reward_matrix);
+
+  /**
+   * @brief Navigate the ASV through the formation of buoys. First travels to waypoint between second pair of buoys,
+   * then navigates through the formation of buoys and returns control when asv is 0.2m away from crossing the last buoy pair.
+   *
+   * @param predicted_positions The predicted positions of the buoys
+   */
+  void navigate_formation(const Eigen::Array<double, 2, 6>& predicted_positions);
+
+  /**
+   * @brief Calculate the coordinates of a gps point in the map frame
+   *
+   * @param lat The latitude of the gps point
+   * @param lon The longitude of the gps point
+   * 
+   * @return The coordinates of the gps point in the map frame
+   */
+  std::pair<double, double> lla2flat(double lat, double lon) const;
+
+  /**
+   * @brief Utility function to get the heading of the ASV assuming identical orientation between odom and map frame
+   *
+   * @param msg The quat message derived from the odom message published by the seapath
+   */
+  double get_freya_heading(const geometry_msgs::msg::Quaternion msg) const;
+
+  /**
+   * @brief Utility function to set the position of the GPS coordinates in the map frame
+   */
+  void set_gps_frame_coords();
+
+  /**
+   * @brief Utility function that runs until waypoint is reached.
+   * Function returns when within distance_threshold of the waypoint.
+   * 
+   * @param distance_threshold The distance threshold for reaching the waypoint
+   */
+  void reach_waypoint(const double distance_threshold);
+
+
+
+private:
+  mutable std::mutex odom_mutex_;
+  mutable std::mutex grid_mutex_;
+  mutable std::mutex landmark_mutex_;
+
+  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr map_origin_sub_;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr grid_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Subscription<vortex_msgs::msg::LandmarkArray>::SharedPtr landmarks_sub_;
+
+  rclcpp::Client<vortex_msgs::srv::Waypoint>::SharedPtr waypoint_client_;
+
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr waypoint_visualization_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr gps_map_coord_visualization_pub_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_;
+  nav_msgs::msg::Odometry::SharedPtr odom_msg_;
+  vortex_msgs::msg::LandmarkArray::SharedPtr landmarks_msg_;
+
+  geometry_msgs::msg::Point previous_waypoint_odom_frame_;
+
+};
+
+} // namespace docking_task
+
+#endif // DOCKING_TASK_ROS_HPP

mission/docking_task/launch/docking_task_launch.py

@@ -0,0 +1,17 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    docking_task_node = Node(
+            package='docking_task',
+            executable='docking_task_node',
+            name='docking_task_node',
+            parameters=[os.path.join(get_package_share_directory('docking_task'),'params','docking_task_params.yaml')],
+            # arguments=['--ros-args', '--log-level', 'DEBUG'],
+            output='screen',
+        )
+    return LaunchDescription([
+        docking_task_node
+    ])

mission/docking_task/package.xml

@@ -0,0 +1,28 @@
+<?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>docking_task</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">jorgen</maintainer>
+  <license>TODO: License declaration</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>nav_msgs</depend> 
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mission/docking_task/params/docking_task_params.yaml

@@ -0,0 +1,32 @@
+docking_task_node:
+  ros__parameters:
+    map_origin_lat: 63.4411585867919
+    map_origin_lon: 10.419400373255625
+    map_origin_set: true
+    gps_start_lat: 63.44097054434422
+    gps_start_lon: 10.419997767413607
+    gps_end_lat: 63.44125901804796
+    gps_end_lon: 10.41857835889424
+    gps_start_x: 0.0
+    gps_start_y: 0.0
+    gps_end_x: 0.0
+    gps_end_y: 0.0
+    gps_frame_coords_set: false
+    dock_width: 4.13 # width of available docking space
+    dock_width_tolerance: 0.5
+    dock_length: 2.0  # length of available docking space
+    dock_length_tolerance: 0.5
+    dock_edge_width: 0.13   # width of the dock edge
+    dock_edge_width_tolerance: 0.05
+    dock_search_offset: 0.2   # offset in forward direction to begin search for dock
+    task_nr: 2 # possible values 1 and 2 corresponding to task 4.1 and 4.2
+    models:
+      dynmod_stddev: 0.01
+      senmod_stddev: 0.01
+
+
+    map_origin_topic: "/map/origin"
+    grid_topic: "grid"
+    odom_topic: "/seapath/odom/ned"
+    landmark_pose_topic: "landmarks_out"
+

mission/docking_task/src/docking_task_node.cpp

@@ -0,0 +1,14 @@
+#include <docking_task/docking_task_ros.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+
+  auto node = std::make_shared<docking_task::DockingTaskNode>();
+
+  rclcpp::spin(node);
+
+  rclcpp::shutdown();
+
+  return 0;
+}