sitaw merge

asv_setup/config/sitaw/land_polygon.yaml

@@ -0,0 +1,8 @@
+63.437946025438066 10.507175268806458
+63.43787563495969 10.507121157686353
+63.4378360402396 10.50719002638467
+63.43763366637102 10.50697358190425
+63.43760726967404 10.507101480915406
+63.43713432473124 10.506752218231092
+63.437239913116464 10.505876601923934
+63.438056010214204 10.506461985859616

asv_setup/config/sitaw/landmark_server_params.yaml

@@ -0,0 +1,4 @@
+landmark_server_node:
+  ros__parameters:
+    fixed_frame: map
+    target_frame: base_link

asv_setup/config/sitaw/map_manager_params.yaml

@@ -0,0 +1,8 @@
+map_manager_node:
+  ros__parameters:
+    use_predef_landmask: true
+    landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon.yaml"
+    map_resolution: 0.2
+    map_width: 1000
+    map_height: 1000
+    frame_id: "map"

asv_setup/config/sitaw/seapath_params.yaml

@@ -0,0 +1,7 @@
+seapath_ros_driver_node:
+  ros__parameters:
+    UDP_IP: "10.0.1.10"  # Source IP of the host sender
+    UDP_PORT: 31421  # Port at which to receive UDP semgents
+    use_predef_map_origin: false
+    map_origin_lat: 0.0
+    map_origin_lon: 0.0

asv_setup/launch/sitaw.launch.py

@@ -0,0 +1,51 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    enable_tf = LaunchConfiguration('enable_tf')
+    enable_tf_arg = DeclareLaunchArgument(
+        'enable_tf',
+        default_value='True',
+        description='enable tf launch file',
+    )
+
+    seapath_ros_driver_node = Node(
+            package='seapath_ros_driver',
+            executable='seapath_ros_driver_node',
+            name='seapath_ros_driver_node',
+            parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','sitaw','seapath_params.yaml')],
+            output='screen',
+        )
+    map_manager_node = Node(
+            package='map_manager',
+            executable='map_manager_node',
+            name='map_manager_node',
+            parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','sitaw','map_manager_params.yaml')],
+            output='screen',
+        )
+    landmark_server_node = Node(
+            package='landmark_server',
+            executable='landmark_server_node',
+            name='landmark_server_node',
+            parameters=[os.path.join(get_package_share_directory('asv_setup'),'config','sitaw','landmark_server_params.yaml')],
+            # arguments=['--ros-args', '--log-level', 'DEBUG'],
+            output='screen',
+        )
+    tf_launch = IncludeLaunchDescription(
+        launch_description_source=PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('asv_setup'), 'launch', 'tf.launch.py')),
+        condition=IfCondition(enable_tf),
+    )
+    return LaunchDescription([
+        enable_tf_arg,
+        tf_launch,
+        seapath_ros_driver_node,
+        map_manager_node,
+        landmark_server_node
+    ])

asv_setup/launch/tf.launch.py

@@ -36,9 +36,9 @@ def generate_launch_description():
     )
 
     # base_link (NED) to seapath_frame (NED) tf.
-    tf_base_link_to_seapath = Node(
+    tf_seapath_to_base_link = Node(
         package='tf2_ros',
-        name='base_link_ned_to_seapath_frame',
+        name='seapath_to_base_link',
         executable='static_transform_publisher',
         arguments=['--x'              , '0',
                    '--y'              , '0',
@@ -50,8 +50,23 @@ def generate_launch_description():
                    '--child-frame-id' , 'base_link'],
     )
 
+    os_sensor_to_os_lidar = Node(
+        package='tf2_ros',
+        name='os_sensor_to_os_lidar',
+        executable='static_transform_publisher',
+        arguments=['--x'              , '0',
+                   '--y'              , '0',
+                   '--z'              , '0.036180000000000004',
+                   '--roll'           , '0',
+                   '--pitch'          , '0',
+                   '--yaw'            , str(math.pi),
+                   '--frame-id'       , 'os_sensor',
+                   '--child-frame-id' , 'os_lidar'],
+    )
+
     return LaunchDescription([
         tf_base_link_to_lidar,
         tf_base_link_to_zed_camera_link,
-        tf_base_link_to_seapath,
+        tf_seapath_to_base_link,
+        os_sensor_to_os_lidar,
     ])

guidance/dp_guidance/launch/dp_guidance.launch.py

@@ -14,3 +14,4 @@ def generate_launch_description():
     return LaunchDescription([
         dp_guidance_node
     ])
+

mission/landmark_server/CMakeLists.txt

@@ -22,24 +22,34 @@ find_package(std_msgs REQUIRED)
 find_package(vortex_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(rclcpp_action REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED)
 
 include_directories(include)
+include_directories(${EIGEN3_INCLUDE_DIR})
 
 # Create an executable
 add_executable(landmark_server_node 
     src/landmark_server.cpp
-    src/landmark_server_main.cpp)
+    src/landmark_server_main.cpp
+    src/grid_manager.cpp)
 
 target_link_libraries( ${PROJECT_NAME}_node
-    pcl_common)
+    pcl_common
+    ${PCL_LIBRARIES}
+    Eigen3::Eigen)
 
 ament_target_dependencies(landmark_server_node
     rclcpp
     std_msgs
     vortex_msgs
+    sensor_msgs
+    pcl_conversions
     nav_msgs
     rclcpp_action
     rclcpp_components
@@ -52,6 +62,8 @@ ament_target_dependencies(landmark_server_node
   )
 
   install(DIRECTORY
+    launch
+    params
     DESTINATION share/${PROJECT_NAME}/
   )
 

mission/landmark_server/include/landmark_server/grid_manager.hpp

@@ -0,0 +1,96 @@
+#include <vector>
+#include <cstdint>
+#include <tuple>
+#include <Eigen/Dense>
+
+namespace landmark_server
+{
+
+enum class ShapeType : uint8_t
+{
+    SPHERE = 0,
+    PRISM = 1
+};
+
+struct Circle
+{
+    float radius;
+    float x_centre;
+    float y_centre;
+};
+
+struct Point
+{
+    float x;
+    float y;
+    float z;
+};
+
+struct Polygon
+{
+    std::vector<Point> vertices;
+};
+
+struct Pose
+{
+    float x;
+    float y;
+    float yaw;
+};
+
+
+struct ShapeInfo
+{
+    ShapeType type;
+    Circle circle;
+    Polygon polygon;
+    Pose pose;
+
+};
+
+
+
+
+class GridManager
+{
+
+    public:
+        using Grid = Eigen::Array<int8_t, Eigen::Dynamic, Eigen::Dynamic>;
+
+        // GridManager(std::vector<int8_t> grid, float resolution, uint32_t height, uint32_t width);
+        GridManager(float resolution, uint32_t height, uint32_t width);
+        ~GridManager() = default;
+
+        const Grid& get_grid() const;
+
+
+
+        void update_grid(int8_t* grid, const Eigen::Array<float, 2, Eigen::Dynamic>& polygon, int value);
+
+        std::tuple<int, int> world_to_grid(float x, float y);
+
+
+        void handle_circle(int xc, int yc, int r, int value);
+
+        void draw_circle(int xc, int yc, int x, int y, int value, Eigen::Block<landmark_server::GridManager::Grid, -1, -1, false>& block, int xmin, int ymin);
+
+
+        void handle_polygon(int8_t* grid, const Eigen::Array<float, 2, Eigen::Dynamic>& vertices, int value);
+
+        void draw_polygon(int x0, int y0, int x1, int y1, Eigen::Block<landmark_server::GridManager::Grid, -1, -1, false>& block, int value);
+        void draw_line(int x0, int y0, int x1, int y1, int8_t* grid, int value);
+
+
+
+
+    private:
+        Grid grid_;
+        float resolution_;
+        uint32_t height_;
+        uint32_t width_;
+
+
+};
+
+
+}

mission/landmark_server/include/landmark_server/landmark_server.hpp

@@ -2,9 +2,14 @@
 #define LANDMARK_SERVER_HPP
 
 #include <rclcpp/rclcpp.hpp>
+#include <landmark_server/grid_manager.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <sstream>
 #include <thread>
+#include <cstdint>
+#include <pcl/surface/convex_hull.h>
+#include <pcl_conversions/pcl_conversions.h>
+
 
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <vortex_msgs/action/filtered_landmarks.hpp>
@@ -17,6 +22,10 @@
 
 #include <tf2/transform_datatypes.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include "nav_msgs/srv/get_map.hpp"
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
 
 namespace landmark_server {
 
@@ -63,6 +72,11 @@ class LandmarkServerNode : public rclcpp::Node {
   void landmarksRecievedCallback(
       const vortex_msgs::msg::LandmarkArray::SharedPtr msg);
 
+
+Eigen::Array<float, 2, Eigen::Dynamic> get_convex_hull(const shape_msgs::msg::SolidPrimitive& solid_primitive);
+
+void get_grid();
+
   /**
    * @brief A shared pointer to a publisher for the LandmarkArray message type.
    * Publishes all landmarks currently stored in the server.
@@ -76,19 +90,32 @@ class LandmarkServerNode : public rclcpp::Node {
    */
   rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr posePublisher_;
 
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr gridPublisher_;
+
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr convex_hull_publisher_;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cluster_publisher_;
+
+
   /**
    * @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_;
 
+
+  nav_msgs::msg::OccupancyGrid grid_msg_;
+
   /**
    * @brief A shared pointer to an rclcpp_action server for handling filtered
    * landmarks.
    */
   rclcpp_action::Server<vortex_msgs::action::FilteredLandmarks>::SharedPtr
       action_server_;
 
+  rclcpp::Client<nav_msgs::srv::GetMap>::SharedPtr grid_client_;
+
+  std::unique_ptr<GridManager> grid_manager_ = nullptr;
+
   /**
    * @brief Handles the goal request for the `handle_goal` function.
    *

mission/landmark_server/launch/landmark_server_launch.py

@@ -0,0 +1,26 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import (DeclareLaunchArgument)
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+    default_params_file = os.path.join(get_package_share_directory('landmark_server'),'params','landmark_server_params.yaml')
+    params_file = LaunchConfiguration('params_file')
+    params_file_arg = DeclareLaunchArgument('params_file',
+                                            default_value=str(
+                                                default_params_file),
+                                            description='name or path to the parameters file to use.')
+    landmark_server_node = Node(
+            package='landmark_server',
+            executable='landmark_server_node',
+            name='landmark_server_node',
+            parameters=[params_file],
+            # arguments=['--ros-args', '--log-level', 'DEBUG'],
+            output='screen',
+        )
+    return LaunchDescription([
+        params_file_arg,
+        landmark_server_node
+    ])

mission/landmark_server/package.xml

@@ -18,6 +18,9 @@
   <depend>nav_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>pcl_conversions</depend>
+
 
 
 

mission/landmark_server/params/landmark_server_params.yaml

@@ -0,0 +1,4 @@
+landmark_server_node:
+  ros__parameters:
+    fixed_frame: world
+    target_frame: base_link