From b461023de5e5bf43ec6d4497487ae9960778c10d Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Mon, 5 Aug 2024 19:09:14 +0200 Subject: [PATCH] visualization fix --- .../sitaw/land_polygon_sim_navigation.yaml | 5 + .../config/sitaw/map_manager_params.yaml | 11 +- .../njord_tasks/docking_task/CMakeLists.txt | 4 + .../include/docking_task/docking_task_ros.hpp | 12 +- mission/njord_tasks/docking_task/package.xml | 1 + .../params/docking_task_params.yaml | 10 +- .../docking_task/src/docking_task_ros.cpp | 225 ++++-- .../maneuvering_task/CMakeLists.txt | 4 + .../maneuvering_task/maneuvering_task_ros.hpp | 14 +- .../njord_tasks/maneuvering_task/package.xml | 1 + .../src/maneuvering_task_ros.cpp | 317 ++++++-- .../navigation_task/CMakeLists.txt | 5 + .../navigation_task/navigation_task_ros.hpp | 49 +- .../njord_tasks/navigation_task/package.xml | 1 + .../params/navigation_task_params.yaml | 18 +- .../src/navigation_task_ros.cpp | 676 ++++++++++++++---- .../njord_task_base/njord_task_base_ros.hpp | 9 + .../src/njord_task_base_ros.cpp | 39 +- 18 files changed, 1083 insertions(+), 318 deletions(-) create mode 100644 asv_setup/config/sitaw/land_polygon_sim_navigation.yaml diff --git a/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml b/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml new file mode 100644 index 0000000..63c9117 --- /dev/null +++ b/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml @@ -0,0 +1,5 @@ +-33.722576743831986 150.67377683501326 +-33.72213785615945 150.6737273159938 +-33.722119332734295 150.6744244880215 +-33.72265441348879 150.6743192836287 + \ No newline at end of file diff --git a/asv_setup/config/sitaw/map_manager_params.yaml b/asv_setup/config/sitaw/map_manager_params.yaml index b23c80e..036e3b2 100644 --- a/asv_setup/config/sitaw/map_manager_params.yaml +++ b/asv_setup/config/sitaw/map_manager_params.yaml @@ -1,7 +1,8 @@ map_manager_node: ros__parameters: - use_predef_landmask: false - landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml" + use_predef_landmask: true + landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_sim_navigation.yaml" + # landmask_file: "src/vortex-asv/asv_setup/config/sitaw/land_polygon_office.yaml" map_resolution: 0.2 map_width: 1000 map_height: 1000 @@ -10,6 +11,6 @@ map_manager_node: # map_origin_lat: 63.414660884931976 # map_origin_lon: 10.398554661537544 # use_predef_map_origin: true - map_origin_lat: 0.0 - map_origin_lon: 0.0 - use_predef_map_origin: false \ No newline at end of file + map_origin_lat: -33.72242824301795 + map_origin_lon: 150.6740063854522 + use_predef_map_origin: true \ No newline at end of file diff --git a/mission/njord_tasks/docking_task/CMakeLists.txt b/mission/njord_tasks/docking_task/CMakeLists.txt index c9298cc..2ecfb3b 100644 --- a/mission/njord_tasks/docking_task/CMakeLists.txt +++ b/mission/njord_tasks/docking_task/CMakeLists.txt @@ -27,6 +27,8 @@ find_package(vortex_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) include_directories(include) @@ -38,6 +40,7 @@ target_link_libraries(docking_task_node tf2::tf2 tf2_ros::tf2_ros tf2_geometry_msgs::tf2_geometry_msgs + pcl_common ) ament_target_dependencies(docking_task_node @@ -50,6 +53,7 @@ ament_target_dependencies(docking_task_node tf2 tf2_ros tf2_geometry_msgs + pcl_conversions ) install( diff --git a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp index 7e3ac3c..ebd943d 100644 --- a/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp +++ b/mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp @@ -4,10 +4,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace docking_task { @@ -41,11 +41,11 @@ class DockingTaskNode : public NjordTaskBaseNode { Eigen::Array predict_first_buoy_pair(); - Eigen::Array + Eigen::Array predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); - Eigen::Array + Eigen::Array predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, const geometry_msgs::msg::Point &buoy_2, diff --git a/mission/njord_tasks/docking_task/package.xml b/mission/njord_tasks/docking_task/package.xml index e9d0a45..e3dc286 100644 --- a/mission/njord_tasks/docking_task/package.xml +++ b/mission/njord_tasks/docking_task/package.xml @@ -21,6 +21,7 @@ tf2 tf2_ros tf2_geometry_msgs + pcl_conversions diff --git a/mission/njord_tasks/docking_task/params/docking_task_params.yaml b/mission/njord_tasks/docking_task/params/docking_task_params.yaml index 919a27a..4b67518 100644 --- a/mission/njord_tasks/docking_task/params/docking_task_params.yaml +++ b/mission/njord_tasks/docking_task/params/docking_task_params.yaml @@ -1,10 +1,12 @@ docking_task_node: ros__parameters: - map_origin_lat: 63.4411585867919 - map_origin_lon: 10.419400373255625 + # map_origin_lat: 63.4411585867919 + # map_origin_lon: 10.419400373255625 + map_origin_lat: -33.72242824301795 + map_origin_lon: 150.6740063854522 map_origin_set: true - gps_start_lat: 63.44097054434422 - gps_start_lon: 10.419997767413607 + gps_start_lat: -33.72255346763595 + gps_start_lon: 150.67394210459224 gps_end_lat: 63.44125901804796 gps_end_lon: 10.41857835889424 gps_start_x: 0.0 diff --git a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp index e524e67..0a15d3f 100644 --- a/mission/njord_tasks/docking_task/src/docking_task_ros.cpp +++ b/mission/njord_tasks/docking_task/src/docking_task_ros.cpp @@ -19,7 +19,6 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) declare_parameter("models.dynmod_stddev", 0.0); declare_parameter("models.sen_stddev", 0.0); - initialize_grid_sub(); std::thread(&DockingTaskNode::main_task, this).detach(); } @@ -27,7 +26,32 @@ DockingTaskNode::DockingTaskNode(const rclcpp::NodeOptions &options) void DockingTaskNode::main_task() { navigation_ready(); // Starting docking task + odom_start_point_ = get_odom()->pose.pose.position; Eigen::Array predicted_first_pair = predict_first_buoy_pair(); + + sensor_msgs::msg::PointCloud2 buoy_vis_msg; + pcl::PointCloud buoy_vis; + pcl::PointXYZRGB buoy_red_0; + buoy_red_0.x = predicted_first_pair(0, 0); + buoy_red_0.y = predicted_first_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + pcl::PointXYZRGB buoy_green_1; + buoy_green_1.x = predicted_first_pair(0, 1); + buoy_green_1.y = predicted_first_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + double distance_to_first_buoy_pair = this->get_parameter("distance_to_first_buoy_pair").as_double(); if (distance_to_first_buoy_pair > 6.0) { @@ -38,11 +62,13 @@ void DockingTaskNode::main_task() { waypoint_to_approach_first_pair_base_link.z = 0.0; try { auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)); + "odom", "base_link", tf2::TimePointZero); geometry_msgs::msg::Point waypoint_to_approach_first_pair_odom; tf2::doTransform(waypoint_to_approach_first_pair_base_link, waypoint_to_approach_first_pair_odom, transform); send_waypoint(waypoint_to_approach_first_pair_odom); + set_desired_heading(odom_start_point_, + waypoint_to_approach_first_pair_odom); reach_waypoint(1.0); } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); @@ -53,6 +79,30 @@ void DockingTaskNode::main_task() { if (buoy_landmarks_0_to_1.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = buoy_landmarks_0_to_1[0].pose_odom_frame.position.x; + buoy_red_0.y = buoy_landmarks_0_to_1[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = buoy_landmarks_0_to_1[1].pose_odom_frame.position.x; + buoy_green_1.y = buoy_landmarks_0_to_1[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_first_pair; waypoint_first_pair.x = (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + @@ -64,55 +114,156 @@ void DockingTaskNode::main_task() { 2; waypoint_first_pair.z = 0.0; send_waypoint(waypoint_first_pair); + set_desired_heading(odom_start_point_, waypoint_first_pair); reach_waypoint(1.0); // Second pair of buoys - Eigen::Array predicted_first_and_second_pair = + Eigen::Array predicted_first_and_second_pair = predict_second_buoy_pair( buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position); - std::vector buoy_landmarks_0_to_3 = + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_first_and_second_pair(0, 0); + buoy_red_0.y = predicted_first_and_second_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_first_and_second_pair(0, 1); + buoy_green_1.y = predicted_first_and_second_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_2_to_3 = get_buoy_landmarks(predicted_first_and_second_pair); - if (buoy_landmarks_0_to_3.size() != 4) { + if (buoy_landmarks_2_to_3.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = buoy_landmarks_2_to_3[0].pose_odom_frame.position.x; + buoy_red_0.y = buoy_landmarks_2_to_3[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = buoy_landmarks_2_to_3[1].pose_odom_frame.position.x; + buoy_green_1.y = buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_second_pair; waypoint_second_pair.x = - (buoy_landmarks_0_to_3[2].pose_odom_frame.position.x + - buoy_landmarks_0_to_3[3].pose_odom_frame.position.x) / + (buoy_landmarks_2_to_3[0].pose_odom_frame.position.x + + buoy_landmarks_2_to_3[1].pose_odom_frame.position.x) / 2; waypoint_second_pair.y = - (buoy_landmarks_0_to_3[2].pose_odom_frame.position.y + - buoy_landmarks_0_to_3[3].pose_odom_frame.position.y) / + (buoy_landmarks_2_to_3[0].pose_odom_frame.position.y + + buoy_landmarks_2_to_3[1].pose_odom_frame.position.y) / 2; waypoint_second_pair.z = 0.0; send_waypoint(waypoint_second_pair); + set_desired_heading(odom_start_point_, waypoint_second_pair); reach_waypoint(1.0); // Third pair of buoys - Eigen::Array predicted_second_and_third_pair = + Eigen::Array predicted_third_pair = predict_third_buoy_pair( buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position, - buoy_landmarks_0_to_3[2].pose_odom_frame.position, - buoy_landmarks_0_to_3[3].pose_odom_frame.position); - std::vector buoy_landmarks_2_to_5 = - get_buoy_landmarks(predicted_second_and_third_pair); - if (buoy_landmarks_2_to_5.size() != 4) { + buoy_landmarks_2_to_3[2].pose_odom_frame.position, + buoy_landmarks_2_to_3[3].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_third_pair(0, 0); + buoy_red_0.y = predicted_third_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_third_pair(0, 1); + buoy_green_1.y = predicted_third_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_4_to_5 = + get_buoy_landmarks(predicted_third_pair); + if (buoy_landmarks_4_to_5.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = buoy_landmarks_4_to_5[0].pose_odom_frame.position.x; + buoy_red_0.y = buoy_landmarks_4_to_5[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = buoy_landmarks_4_to_5[1].pose_odom_frame.position.x; + buoy_green_1.y = buoy_landmarks_4_to_5[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_third_pair; waypoint_third_pair.x = - (buoy_landmarks_2_to_5[2].pose_odom_frame.position.x + - buoy_landmarks_2_to_5[3].pose_odom_frame.position.x) / + (buoy_landmarks_4_to_5[0].pose_odom_frame.position.x + + buoy_landmarks_4_to_5[1].pose_odom_frame.position.x) / 2; waypoint_third_pair.y = - (buoy_landmarks_2_to_5[2].pose_odom_frame.position.y + - buoy_landmarks_2_to_5[3].pose_odom_frame.position.y) / + (buoy_landmarks_4_to_5[0].pose_odom_frame.position.y + + buoy_landmarks_4_to_5[1].pose_odom_frame.position.y) / 2; waypoint_third_pair.z = 0.0; send_waypoint(waypoint_third_pair); + set_desired_heading(odom_start_point_, waypoint_third_pair); reach_waypoint(1.0); + + std::thread(&DockingTaskNode::initialize_grid_sub, this).detach(); } Eigen::Array DockingTaskNode::predict_first_buoy_pair() { @@ -159,30 +310,26 @@ Eigen::Array DockingTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array DockingTaskNode::predict_second_buoy_pair( +Eigen::Array DockingTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; direction_vector << previous_waypoint_odom_frame_.x - - this->get_parameter("gps_start_x").as_double(), + odom_start_point_.x, previous_waypoint_odom_frame_.y - - this->get_parameter("gps_start_y").as_double(); + odom_start_point_.y; direction_vector.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x; - predicted_positions(1, 0) = buoy_0.y; - predicted_positions(0, 1) = buoy_1.x; - predicted_positions(1, 1) = buoy_1.y; - predicted_positions(0, 2) = buoy_0.x + direction_vector(0) * 5; - predicted_positions(1, 2) = buoy_0.y + direction_vector(1) * 5; - predicted_positions(0, 3) = buoy_1.x + direction_vector(0) * 5; - predicted_positions(1, 3) = buoy_1.y + direction_vector(1) * 5; + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 5; + predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 5; + predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 5; + predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 5; return predicted_positions; } -Eigen::Array DockingTaskNode::predict_third_buoy_pair( +Eigen::Array DockingTaskNode::predict_third_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, const geometry_msgs::msg::Point &buoy_2, @@ -193,18 +340,14 @@ Eigen::Array DockingTaskNode::predict_third_buoy_pair( (buoy_2.y + buoy_3.y) / 2 - (buoy_0.y + buoy_1.y) / 2; direction_vector_first_to_second_pair.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_2.x; - predicted_positions(1, 0) = buoy_2.y; - predicted_positions(0, 1) = buoy_3.x; - predicted_positions(1, 1) = buoy_3.y; - predicted_positions(0, 2) = + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_2.x + direction_vector_first_to_second_pair(0) * 5; - predicted_positions(1, 2) = + predicted_positions(1, 0) = buoy_2.y + direction_vector_first_to_second_pair(1) * 5; - predicted_positions(0, 3) = + predicted_positions(0, 1) = buoy_3.x + direction_vector_first_to_second_pair(0) * 5; - predicted_positions(1, 3) = + predicted_positions(1, 1) = buoy_3.y + direction_vector_first_to_second_pair(1) * 5; return predicted_positions; diff --git a/mission/njord_tasks/maneuvering_task/CMakeLists.txt b/mission/njord_tasks/maneuvering_task/CMakeLists.txt index c502753..6f3e7a2 100644 --- a/mission/njord_tasks/maneuvering_task/CMakeLists.txt +++ b/mission/njord_tasks/maneuvering_task/CMakeLists.txt @@ -27,6 +27,8 @@ find_package(vortex_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) include_directories(include) @@ -38,6 +40,7 @@ target_link_libraries(maneuvering_task_node tf2::tf2 tf2_ros::tf2_ros tf2_geometry_msgs::tf2_geometry_msgs + pcl_common ) ament_target_dependencies(maneuvering_task_node @@ -50,6 +53,7 @@ ament_target_dependencies(maneuvering_task_node tf2_ros tf2_geometry_msgs njord_task_base + pcl_conversions ) install( diff --git a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp index 45423aa..731d592 100644 --- a/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp +++ b/mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp @@ -2,6 +2,10 @@ #define MANEUVERING_TASK_ROS_HPP #include +#include +#include +#include +#include namespace maneuvering_task { @@ -19,17 +23,11 @@ class ManeuveringTaskNode : public NjordTaskBaseNode { Eigen::Array predict_first_buoy_pair(); - Eigen::Array + Eigen::Array predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); - Eigen::Array - predict_third_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1, - const geometry_msgs::msg::Point &buoy_2, - const geometry_msgs::msg::Point &buoy_3); - - Eigen::Array + Eigen::Array predict_next_pair_in_formation(const geometry_msgs::msg::Point &buoy_red, const geometry_msgs::msg::Point &buoy_green, Eigen::Vector2d direction_vector); diff --git a/mission/njord_tasks/maneuvering_task/package.xml b/mission/njord_tasks/maneuvering_task/package.xml index 52812f8..4703ead 100644 --- a/mission/njord_tasks/maneuvering_task/package.xml +++ b/mission/njord_tasks/maneuvering_task/package.xml @@ -21,6 +21,7 @@ tf2 tf2_ros tf2_geometry_msgs + pcl_conversions diff --git a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp index e5f1a0a..a1106fb 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -15,16 +15,39 @@ ManeuveringTaskNode::ManeuveringTaskNode(const rclcpp::NodeOptions &options) void ManeuveringTaskNode::main_task() { navigation_ready(); RCLCPP_INFO(this->get_logger(), "Maneuvering task started"); + odom_start_point_ = get_odom()->pose.pose.position; // First buoy pair Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); + + sensor_msgs::msg::PointCloud2 buoy_vis_msg; + pcl::PointCloud buoy_vis; + pcl::PointXYZRGB buoy_red_0; + buoy_red_0.x = predicted_first_buoy_pair(0, 0); + buoy_red_0.y = predicted_first_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + pcl::PointXYZRGB buoy_green_1; + buoy_green_1.x = predicted_first_buoy_pair(0, 1); + buoy_green_1.y = predicted_first_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + // First first buoy pair is far away, should be closer before trying to // measure it. - double gps_start_x = this->get_parameter("gps_start_x").as_double(); - double gps_start_y = this->get_parameter("gps_start_y").as_double(); double gps_end_x = this->get_parameter("gps_end_x").as_double(); double gps_end_y = this->get_parameter("gps_end_y").as_double(); Eigen::Vector2d direction_vector_to_end; - direction_vector_to_end << gps_end_x - gps_start_x, gps_end_y - gps_start_y; + direction_vector_to_end << gps_end_x - odom_start_point_.x, gps_end_y - odom_start_point_.y; direction_vector_to_end.normalize(); double distance = @@ -43,6 +66,30 @@ void ManeuveringTaskNode::main_task() { std::vector measured_first_buoy_pair = get_buoy_landmarks(predicted_first_buoy_pair); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_first_buoy_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_first_buoy_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_first_buoy_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_first_buoy_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_first_pair; waypoint_first_pair.x = (measured_first_buoy_pair[0].pose_odom_frame.position.x + @@ -54,6 +101,7 @@ void ManeuveringTaskNode::main_task() { 2; waypoint_first_pair.z = 0.0; send_waypoint(waypoint_first_pair); + set_desired_heading(odom_start_point_, waypoint_first_pair); reach_waypoint(0.5); // Second buoy pair is FAR away, should be closer before trying to measure it. @@ -64,51 +112,91 @@ void ManeuveringTaskNode::main_task() { waypoint_first_pair.y + direction_vector_to_end(1) * 15; waypoint_approach_formation.z = 0.0; send_waypoint(waypoint_approach_formation); + set_desired_heading(waypoint_first_pair, waypoint_approach_formation); reach_waypoint(1.0); // Second buoy pair - Eigen::Array predicted_second_buoy_pair = + Eigen::Array predicted_second_buoy_pair = predict_second_buoy_pair( measured_first_buoy_pair[0].pose_odom_frame.position, measured_first_buoy_pair[1].pose_odom_frame.position); - std::vector measured_buoy_0_to_3 = + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_second_buoy_pair(0, 0); + buoy_red_0.y = predicted_second_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_second_buoy_pair(0, 1); + buoy_green_1.y = predicted_second_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector measured_buoy_2_to_3 = get_buoy_landmarks(predicted_second_buoy_pair); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_buoy_2_to_3[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_buoy_2_to_3[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_buoy_2_to_3[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_buoy_2_to_3[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_second_pair; waypoint_second_pair.x = - (measured_buoy_0_to_3[2].pose_odom_frame.position.x + - measured_buoy_0_to_3[3].pose_odom_frame.position.x) / + (measured_buoy_2_to_3[0].pose_odom_frame.position.x + + measured_buoy_2_to_3[1].pose_odom_frame.position.x) / 2; waypoint_second_pair.y = - (measured_buoy_0_to_3[2].pose_odom_frame.position.y + - measured_buoy_0_to_3[3].pose_odom_frame.position.y) / + (measured_buoy_2_to_3[0].pose_odom_frame.position.y + + measured_buoy_2_to_3[1].pose_odom_frame.position.y) / 2; waypoint_second_pair.z = 0.0; send_waypoint(waypoint_second_pair); + set_desired_heading(waypoint_approach_formation, waypoint_second_pair); reach_waypoint(0.5); // Setup variable to navigate formation Eigen::Vector2d direction_vector_forwards; direction_vector_forwards - << (measured_buoy_0_to_3[2].pose_odom_frame.position.x + - measured_buoy_0_to_3[3].pose_odom_frame.position.x) / - 2 - - (measured_buoy_0_to_3[0].pose_odom_frame.position.x + - measured_buoy_0_to_3[1].pose_odom_frame.position.x) / - 2, - (measured_buoy_0_to_3[2].pose_odom_frame.position.y + - measured_buoy_0_to_3[3].pose_odom_frame.position.y) / - 2 - - (measured_buoy_0_to_3[0].pose_odom_frame.position.y + - measured_buoy_0_to_3[1].pose_odom_frame.position.y) / - 2; + << (waypoint_second_pair.x - waypoint_first_pair.x), + (waypoint_second_pair.y - waypoint_first_pair.y); direction_vector_forwards.normalize(); Eigen::Vector2d direction_vector_downwards; direction_vector_downwards - << measured_buoy_0_to_3[3].pose_odom_frame.position.x - - measured_buoy_0_to_3[2].pose_odom_frame.position.x, - measured_buoy_0_to_3[3].pose_odom_frame.position.y - - measured_buoy_0_to_3[2].pose_odom_frame.position.y; + << measured_buoy_2_to_3[1].pose_odom_frame.position.x - + measured_buoy_2_to_3[0].pose_odom_frame.position.x, + measured_buoy_2_to_3[1].pose_odom_frame.position.y - + measured_buoy_2_to_3[0].pose_odom_frame.position.y; direction_vector_downwards.normalize(); Eigen::Vector2d vector_to_next_pair; @@ -121,46 +209,86 @@ void ManeuveringTaskNode::main_task() { this->get_parameter("initial_offset").as_double(); geometry_msgs::msg::Point red_buoy = - measured_buoy_0_to_3[2].pose_odom_frame.position; + measured_buoy_2_to_3[0].pose_odom_frame.position; geometry_msgs::msg::Point green_buoy = - measured_buoy_0_to_3[3].pose_odom_frame.position; - + measured_buoy_2_to_3[1].pose_odom_frame.position; + geometry_msgs::msg::Point waypoint_prev_pair = waypoint_second_pair; // ASV is at first buoy pair in formation. // Run loop for the rest of the formation excluding the first pair. int num_iterations = this->get_parameter("nr_of_pair_in_formation").as_int() - 1; for (int _ = 0; _ < num_iterations; ++_) { - Eigen::Array predicted_next_pair = + Eigen::Array predicted_next_pair = predict_next_pair_in_formation(red_buoy, green_buoy, vector_to_next_pair); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_next_pair(0, 0); + buoy_red_0.y = predicted_next_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_next_pair(0, 1); + buoy_green_1.y = predicted_next_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + std::vector measured_next_pair = get_buoy_landmarks(predicted_next_pair); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_next_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_next_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_next_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_next_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_next_pair; - waypoint_next_pair.x = (measured_next_pair[2].pose_odom_frame.position.x + - measured_next_pair[3].pose_odom_frame.position.x) / + waypoint_next_pair.x = (measured_next_pair[0].pose_odom_frame.position.x + + measured_next_pair[1].pose_odom_frame.position.x) / 2; - waypoint_next_pair.y = (measured_next_pair[2].pose_odom_frame.position.y + - measured_next_pair[3].pose_odom_frame.position.y) / + waypoint_next_pair.y = (measured_next_pair[0].pose_odom_frame.position.y + + measured_next_pair[1].pose_odom_frame.position.y) / 2; waypoint_next_pair.z = 0.0; send_waypoint(waypoint_next_pair); + set_desired_heading(waypoint_prev_pair, waypoint_next_pair); reach_waypoint(1.0); - red_buoy = measured_next_pair[2].pose_odom_frame.position; - green_buoy = measured_next_pair[3].pose_odom_frame.position; + red_buoy = measured_next_pair[0].pose_odom_frame.position; + green_buoy = measured_next_pair[1].pose_odom_frame.position; vector_to_next_pair - << (measured_next_pair[2].pose_odom_frame.position.x + - measured_next_pair[3].pose_odom_frame.position.x) / - 2 - - (measured_next_pair[1].pose_odom_frame.position.x + - measured_next_pair[0].pose_odom_frame.position.x) / - 2, - (measured_next_pair[2].pose_odom_frame.position.y + - measured_next_pair[3].pose_odom_frame.position.y) / - 2 - - (measured_next_pair[1].pose_odom_frame.position.y + - measured_next_pair[0].pose_odom_frame.position.y) / - 2; + << waypoint_next_pair.x - waypoint_prev_pair.x, + waypoint_next_pair.y - waypoint_prev_pair.y; + waypoint_prev_pair = waypoint_next_pair; } // ASV is now at the last pair of the buoy formation @@ -169,17 +297,17 @@ void ManeuveringTaskNode::main_task() { double distance_to_end = std::sqrt(std::pow(odom->pose.pose.position.x - gps_end_x, 2) + std::pow(odom->pose.pose.position.y - gps_end_y, 2)); - if (distance_to_end > 6.0) { - auto odom = get_odom(); + if (distance_to_end > 7.0) { geometry_msgs::msg::Point waypoint_toward_end; waypoint_toward_end.x = odom->pose.pose.position.x + - direction_vector_to_end(0) * (distance_to_end - 3.0); + direction_vector_to_end(0) * (distance_to_end - 4.0); waypoint_toward_end.y = odom->pose.pose.position.y + - direction_vector_to_end(1) * (distance_to_end - 3.0); + direction_vector_to_end(1) * (distance_to_end - 4.0); waypoint_toward_end.z = 0.0; send_waypoint(waypoint_toward_end); + set_desired_heading(waypoint_prev_pair, waypoint_toward_end); reach_waypoint(2.0); } Eigen::Array predicted_last_buoy_pair; @@ -191,8 +319,56 @@ void ManeuveringTaskNode::main_task() { gps_end_x + direction_vector_downwards(0) * 2.5; predicted_last_buoy_pair(1, 1) = gps_end_y + direction_vector_downwards(1) * 2.5; + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = predicted_last_buoy_pair(0, 0); + buoy_red_0.y = predicted_last_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = predicted_last_buoy_pair(0, 1); + buoy_green_1.y = predicted_last_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + std::vector measured_last_buoy_pair = get_buoy_landmarks(predicted_last_buoy_pair); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = measured_last_buoy_pair[0].pose_odom_frame.position.x; + buoy_red_0.y = measured_last_buoy_pair[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = measured_last_buoy_pair[1].pose_odom_frame.position.x; + buoy_green_1.y = measured_last_buoy_pair[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_last_pair; waypoint_last_pair.x = (measured_last_buoy_pair[0].pose_odom_frame.position.x + @@ -204,6 +380,8 @@ void ManeuveringTaskNode::main_task() { 2; waypoint_last_pair.z = 0.0; send_waypoint(waypoint_last_pair); + set_desired_heading(waypoint_prev_pair, waypoint_last_pair); + reach_waypoint(1.0); // Also send waypoint 2m through the last pair geometry_msgs::msg::Point waypoint_through_last_pair; waypoint_through_last_pair.x = @@ -212,6 +390,7 @@ void ManeuveringTaskNode::main_task() { waypoint_last_pair.y + direction_vector_to_end(1) * 2; waypoint_through_last_pair.z = 0.0; send_waypoint(waypoint_through_last_pair); + set_desired_heading(waypoint_last_pair, waypoint_through_last_pair); // Task is now finished } @@ -257,42 +436,34 @@ Eigen::Array ManeuveringTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array ManeuveringTaskNode::predict_second_buoy_pair( +Eigen::Array ManeuveringTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; direction_vector << previous_waypoint_odom_frame_.x - - this->get_parameter("gps_start_x").as_double(), + odom_start_point_.x, previous_waypoint_odom_frame_.y - - this->get_parameter("gps_start_y").as_double(); + odom_start_point_.y; direction_vector.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x; - predicted_positions(1, 0) = buoy_0.y; - predicted_positions(0, 1) = buoy_1.x; - predicted_positions(1, 1) = buoy_1.y; - predicted_positions(0, 2) = buoy_0.x + direction_vector(0) * 20; - predicted_positions(1, 2) = buoy_0.y + direction_vector(1) * 20; - predicted_positions(0, 3) = buoy_1.x + direction_vector(0) * 20; - predicted_positions(1, 3) = buoy_1.y + direction_vector(1) * 20; + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 20; + predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 20; + predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 20; + predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 20; return predicted_positions; } -Eigen::Array ManeuveringTaskNode::predict_next_pair_in_formation( +Eigen::Array ManeuveringTaskNode::predict_next_pair_in_formation( const geometry_msgs::msg::Point &buoy_red, const geometry_msgs::msg::Point &buoy_green, Eigen::Vector2d vector_to_next_pair) { - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_red.x; - predicted_positions(1, 0) = buoy_red.y; - predicted_positions(0, 1) = buoy_green.x; - predicted_positions(1, 1) = buoy_green.y; - predicted_positions(0, 2) = buoy_red.x + vector_to_next_pair(0); - predicted_positions(1, 2) = buoy_red.y + vector_to_next_pair(1); - predicted_positions(0, 3) = buoy_green.x + vector_to_next_pair(0); - predicted_positions(1, 3) = buoy_green.y + vector_to_next_pair(1); + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_red.x + vector_to_next_pair(0); + predicted_positions(1, 0) = buoy_red.y + vector_to_next_pair(1); + predicted_positions(0, 1) = buoy_green.x + vector_to_next_pair(0); + predicted_positions(1, 1) = buoy_green.y + vector_to_next_pair(1); return predicted_positions; } diff --git a/mission/njord_tasks/navigation_task/CMakeLists.txt b/mission/njord_tasks/navigation_task/CMakeLists.txt index 1fe4eba..9d46268 100644 --- a/mission/njord_tasks/navigation_task/CMakeLists.txt +++ b/mission/njord_tasks/navigation_task/CMakeLists.txt @@ -26,6 +26,9 @@ find_package(vortex_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) + include_directories(include) @@ -38,6 +41,7 @@ target_link_libraries(navigation_task_node tf2::tf2 tf2_ros::tf2_ros tf2_geometry_msgs::tf2_geometry_msgs + pcl_common ) ament_target_dependencies(navigation_task_node @@ -50,6 +54,7 @@ ament_target_dependencies(navigation_task_node tf2_ros tf2_geometry_msgs njord_task_base + pcl_conversions ) install( diff --git a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp index 7e87816..0185283 100644 --- a/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp +++ b/mission/njord_tasks/navigation_task/include/navigation_task/navigation_task_ros.hpp @@ -2,6 +2,11 @@ #define NAVIGATION_TASK_ROS_HPP #include +#include +#include +#include +#include + namespace navigation_task { @@ -26,23 +31,23 @@ class NavigationTaskNode : public NjordTaskBaseNode { Eigen::Array predict_first_buoy_pair(); /** - * @brief Predict the positions of the first two buoys pairs + * @brief Predict the positions of the second buoy pair * - * @return An Eigen::Array representing the predicted positions - * of the first two buoy pairs + * @return An Eigen::Array representing the predicted positions + * of the second buoy pair */ - Eigen::Array - predict_first_and_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, + Eigen::Array + predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); /** * @brief Predict the positions of the west buoy by using the two first buoy * pairs * - * @return An Eigen::Array representing the predicted positions - * of the second buoy pair and the west buoy + * @return An Eigen::Array representing the predicted position + * of the west buoy */ - Eigen::Array + Eigen::Array predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, const geometry_msgs::msg::Point &buoy_2, @@ -65,10 +70,10 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @brief Predict the positions of the north buoy by using the two first buoy * pairs * - * @return An Eigen::Array representing the predicted positions - * of the third buoy pair and the north buoy + * @return An Eigen::Array representing the predicted position + * of the north buoy */ - Eigen::Array predict_north_buoy( + Eigen::Array predict_north_buoy( const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, const Eigen::Vector2d &direction_vector_second_to_third_pair); @@ -80,10 +85,9 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @return An Eigen::Array representing the predicted positions * of the north and south buoys */ - Eigen::Array predict_south_buoy( + Eigen::Array predict_south_buoy( const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, - const geometry_msgs::msg::Point &buoy_7, const Eigen::Vector2d &direction_vector_second_to_third_pair); /** @@ -101,10 +105,10 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @brief Predict the position of the east buoy by using the fourth buoy pair * and the direction vector from the second to the third buoy pair * - * @return An Eigen::Array representing the predicted positions - * of the fourth buoy pair and the east buoy + * @return An Eigen::Array representing the predicted position + * of the east buoy */ - Eigen::Array predict_east_buoy( + Eigen::Array predict_east_buoy( const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, const Eigen::Vector2d &direction_vector_second_to_third_pair); @@ -113,23 +117,22 @@ class NavigationTaskNode : public NjordTaskBaseNode { * @brief Predict the position of the fifth buoy pair by using the fourth buoy * pair and the direction vector from the second to the third buoy pair * - * @return An Eigen::Array representing the predicted positions - * of the fifth buoy pair and the east buoy + * @return An Eigen::Array representing the predicted positions + * of the fifth buoy pair */ - Eigen::Array predict_fifth_buoy_pair( + Eigen::Array predict_fifth_buoy_pair( const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, - const geometry_msgs::msg::Point &buoy_11, const Eigen::Vector2d &direction_vector_second_to_third_pair); /** * @brief Predict the position of the sixth buoy pair by using the fifth buoy * pair and fourth buoy pair * - * @return An Eigen::Array representing the predicted positions - * of the fifth and sixth buoy pairs + * @return An Eigen::Array representing the predicted position + * of the sixth buoy pairs */ - Eigen::Array + Eigen::Array predict_sixth_buoy_pair(const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, const geometry_msgs::msg::Point &buoy_12, diff --git a/mission/njord_tasks/navigation_task/package.xml b/mission/njord_tasks/navigation_task/package.xml index a41c7cd..1c06d25 100644 --- a/mission/njord_tasks/navigation_task/package.xml +++ b/mission/njord_tasks/navigation_task/package.xml @@ -21,6 +21,7 @@ tf2 tf2_ros tf2_geometry_msgs + pcl_conversions diff --git a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml index 42e4e89..fdc8a50 100644 --- a/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml +++ b/mission/njord_tasks/navigation_task/params/navigation_task_params.yaml @@ -1,17 +1,17 @@ navigation_task_node: ros__parameters: - map_origin_lat: 63.4411585867919 - map_origin_lon: 10.419400373255625 + map_origin_lat: -33.72242824301795 + map_origin_lon: 150.6740063854522 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_lat: -33.722553423946906 + gps_start_lon: 150.67394210456843 + gps_end_lat: -33.72253671663162 + gps_end_lon: 150.67413245758723 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 + gps_end_x: 1.1791211357704299 + gps_end_y: 17.485397720327114 + gps_frame_coords_set: true map_origin_topic: "/map/origin" odom_topic: "/seapath/odom/ned" landmark_topic: "landmarks_out" diff --git a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp index 781a224..895fc85 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -14,8 +14,34 @@ void NavigationTaskNode::main_task() { navigation_ready(); RCLCPP_INFO(this->get_logger(), "Navigation task started"); + auto odom_msg = get_odom(); + odom_start_point_ = odom_msg->pose.pose.position; // First pair of buoys Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair(); + sensor_msgs::msg::PointCloud2 buoy_vis_msg; + pcl::PointCloud buoy_vis; + + pcl::PointXYZRGB buoy_red_0; + buoy_red_0.x = predicted_first_buoy_pair(0, 0); + buoy_red_0.y = predicted_first_buoy_pair(1, 0); + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + pcl::PointXYZRGB buoy_green_1; + buoy_green_1.x = predicted_first_buoy_pair(0, 1); + buoy_green_1.y = predicted_first_buoy_pair(1, 1); + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + double distance_to_first_buoy_pair = this->get_parameter("distance_to_first_buoy_pair").as_double(); if (distance_to_first_buoy_pair > 6.0) { @@ -31,6 +57,7 @@ void NavigationTaskNode::main_task() { tf2::doTransform(waypoint_to_approach_first_pair_base_link, waypoint_to_approach_first_pair_odom, transform); send_waypoint(waypoint_to_approach_first_pair_odom); + set_desired_heading(odom_start_point_, waypoint_to_approach_first_pair_odom); reach_waypoint(1.0); } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); @@ -41,6 +68,29 @@ void NavigationTaskNode::main_task() { if (buoy_landmarks_0_to_1.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_0 = pcl::PointXYZRGB(); + buoy_green_1 = pcl::PointXYZRGB(); + buoy_red_0.x = buoy_landmarks_0_to_1[0].pose_odom_frame.position.x; + buoy_red_0.y = buoy_landmarks_0_to_1[0].pose_odom_frame.position.y; + buoy_red_0.z = 0.0; + buoy_red_0.r = 255; + buoy_red_0.g = 0; + buoy_red_0.b = 0; + buoy_vis.push_back(buoy_red_0); + buoy_green_1.x = buoy_landmarks_0_to_1[1].pose_odom_frame.position.x; + buoy_green_1.y = buoy_landmarks_0_to_1[1].pose_odom_frame.position.y; + buoy_green_1.z = 0.0; + buoy_green_1.r = 0; + buoy_green_1.g = 255; + buoy_green_1.b = 0; + buoy_vis.push_back(buoy_green_1); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_first_pair; waypoint_first_pair.x = (buoy_landmarks_0_to_1[0].pose_odom_frame.position.x + @@ -52,74 +102,202 @@ void NavigationTaskNode::main_task() { 2; waypoint_first_pair.z = 0.0; send_waypoint(waypoint_first_pair); + set_desired_heading(odom_start_point_, waypoint_first_pair); reach_waypoint(1.0); // Second pair of buoys - Eigen::Array predicted_first_and_second_pair = - predict_first_and_second_buoy_pair( + Eigen::Array predicted_second_pair = + predict_second_buoy_pair( buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position); - std::vector buoy_landmarks_0_to_3 = - get_buoy_landmarks(predicted_first_and_second_pair); - if (buoy_landmarks_0_to_3.size() != 4) { + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_red_2; + pcl::PointXYZRGB buoy_green_3; + buoy_red_2.x = predicted_second_pair(0, 0); + buoy_red_2.y = predicted_second_pair(1, 0); + buoy_red_2.z = 0.0; + buoy_red_2.r = 255; + buoy_red_2.g = 0; + buoy_red_2.b = 0; + buoy_vis.push_back(buoy_red_2); + buoy_green_3.x = predicted_second_pair(0, 1); + buoy_green_3.y = predicted_second_pair(1, 1); + buoy_green_3.z = 0.0; + buoy_green_3.r = 0; + buoy_green_3.g = 255; + buoy_green_3.b = 0; + buoy_vis.push_back(buoy_green_3); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_2_to_3 = + get_buoy_landmarks(predicted_second_pair); + if (buoy_landmarks_2_to_3.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find four buoys"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_2 = pcl::PointXYZRGB(); + buoy_green_3 = pcl::PointXYZRGB(); + buoy_red_2.x = buoy_landmarks_2_to_3[0].pose_odom_frame.position.x; + buoy_red_2.y = buoy_landmarks_2_to_3[0].pose_odom_frame.position.y; + buoy_red_2.z = 0.0; + buoy_red_2.r = 255; + buoy_red_2.g = 0; + buoy_red_2.b = 0; + buoy_vis.push_back(buoy_red_2); + buoy_green_3.x = buoy_landmarks_2_to_3[1].pose_odom_frame.position.x; + buoy_green_3.y = buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; + buoy_green_3.z = 0.0; + buoy_green_3.r = 0; + buoy_green_3.g = 255; + buoy_green_3.b = 0; + buoy_vis.push_back(buoy_green_3); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_second_pair; waypoint_second_pair.x = - (buoy_landmarks_0_to_3[2].pose_odom_frame.position.x + - buoy_landmarks_0_to_3[3].pose_odom_frame.position.x) / + (buoy_landmarks_2_to_3[0].pose_odom_frame.position.x + + buoy_landmarks_2_to_3[1].pose_odom_frame.position.x) / 2; waypoint_second_pair.y = - (buoy_landmarks_0_to_3[2].pose_odom_frame.position.y + - buoy_landmarks_0_to_3[3].pose_odom_frame.position.y) / + (buoy_landmarks_2_to_3[0].pose_odom_frame.position.y + + buoy_landmarks_2_to_3[1].pose_odom_frame.position.y) / 2; waypoint_second_pair.z = 0.0; send_waypoint(waypoint_second_pair); + set_desired_heading(waypoint_first_pair, waypoint_second_pair); reach_waypoint(1.0); // West buoy - Eigen::Array predicted_west_buoy = - predict_west_buoy(buoy_landmarks_0_to_3[0].pose_odom_frame.position, - buoy_landmarks_0_to_3[1].pose_odom_frame.position, - buoy_landmarks_0_to_3[2].pose_odom_frame.position, - buoy_landmarks_0_to_3[3].pose_odom_frame.position); - std::vector buoy_landmarks_2_to_4 = + Eigen::Array predicted_west_buoy = + predict_west_buoy(buoy_landmarks_0_to_1[0].pose_odom_frame.position, + buoy_landmarks_0_to_1[1].pose_odom_frame.position, + buoy_landmarks_2_to_3[0].pose_odom_frame.position, + buoy_landmarks_2_to_3[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_west; + buoy_west.x = predicted_west_buoy(0, 0); + buoy_west.y = predicted_west_buoy(1, 0); + buoy_west.z = 0.0; + buoy_west.r = 255; + buoy_west.g = 255; + buoy_west.b = 0; + buoy_vis.push_back(buoy_west); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_4 = get_buoy_landmarks(predicted_west_buoy); - if (buoy_landmarks_2_to_4.size() != 3) { + if (buoy_landmarks_4.size() != 1) { RCLCPP_ERROR(this->get_logger(), "Could not find west buoy"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_west = pcl::PointXYZRGB(); + buoy_west.x = buoy_landmarks_4[0].pose_odom_frame.position.x; + buoy_west.y = buoy_landmarks_4[0].pose_odom_frame.position.y; + buoy_west.z = 0.0; + buoy_west.r = 255; + buoy_west.g = 255; + buoy_west.b = 0; + buoy_vis.push_back(buoy_west); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + Eigen::Vector2d direction_vector_upwards; direction_vector_upwards - << buoy_landmarks_2_to_4[2].pose_odom_frame.position.x - - buoy_landmarks_0_to_3[3].pose_odom_frame.position.x, - buoy_landmarks_2_to_4[2].pose_odom_frame.position.y - - buoy_landmarks_0_to_3[3].pose_odom_frame.position.y; + << buoy_landmarks_2_to_3[0].pose_odom_frame.position.x - + buoy_landmarks_2_to_3[1].pose_odom_frame.position.x, + buoy_landmarks_2_to_3[0].pose_odom_frame.position.y - + buoy_landmarks_2_to_3[1].pose_odom_frame.position.y; direction_vector_upwards.normalize(); // Set waypoint two meters above west buoy geometry_msgs::msg::Point waypoint_west_buoy; - waypoint_west_buoy.x = buoy_landmarks_2_to_4[4].pose_odom_frame.position.x + - direction_vector_upwards(0) * 2; - waypoint_west_buoy.y = buoy_landmarks_2_to_4[4].pose_odom_frame.position.y + - direction_vector_upwards(1) * 2; + waypoint_west_buoy.x = buoy_landmarks_4[0].pose_odom_frame.position.x + + direction_vector_upwards(0) * 3; + waypoint_west_buoy.y = buoy_landmarks_4[0].pose_odom_frame.position.y + + direction_vector_upwards(1) * 3; waypoint_west_buoy.z = 0.0; send_waypoint(waypoint_west_buoy); + set_desired_heading(waypoint_second_pair, waypoint_west_buoy); reach_waypoint(1.0); // Third pair of buoys Eigen::Array predicted_third_buoy_pair = predict_third_buoy_pair( - buoy_landmarks_0_to_3[0].pose_odom_frame.position, - buoy_landmarks_0_to_3[1].pose_odom_frame.position, - buoy_landmarks_0_to_3[2].pose_odom_frame.position, - buoy_landmarks_0_to_3[3].pose_odom_frame.position); + buoy_landmarks_0_to_1[0].pose_odom_frame.position, + buoy_landmarks_0_to_1[1].pose_odom_frame.position, + buoy_landmarks_2_to_3[0].pose_odom_frame.position, + buoy_landmarks_2_to_3[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_red_5; + pcl::PointXYZRGB buoy_green_6; + buoy_red_5.x = predicted_third_buoy_pair(0, 0); + buoy_red_5.y = predicted_third_buoy_pair(1, 0); + buoy_red_5.z = 0.0; + buoy_red_5.r = 255; + buoy_red_5.g = 0; + buoy_red_5.b = 0; + buoy_vis.push_back(buoy_red_5); + buoy_green_6.x = predicted_third_buoy_pair(0, 1); + buoy_green_6.y = predicted_third_buoy_pair(1, 1); + buoy_green_6.z = 0.0; + buoy_green_6.r = 0; + buoy_green_6.g = 255; + buoy_green_6.b = 0; + buoy_vis.push_back(buoy_green_6); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); std::vector buoy_landmarks_5_to_6 = get_buoy_landmarks(predicted_third_buoy_pair); if (buoy_landmarks_5_to_6.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find third buoy pair"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_5 = pcl::PointXYZRGB(); + buoy_green_6 = pcl::PointXYZRGB(); + buoy_red_5.x = buoy_landmarks_5_to_6[0].pose_odom_frame.position.x; + buoy_red_5.y = buoy_landmarks_5_to_6[0].pose_odom_frame.position.y; + buoy_red_5.z = 0.0; + buoy_red_5.r = 255; + buoy_red_5.g = 0; + buoy_red_5.b = 0; + buoy_vis.push_back(buoy_red_5); + buoy_green_6.x = buoy_landmarks_5_to_6[1].pose_odom_frame.position.x; + buoy_green_6.y = buoy_landmarks_5_to_6[1].pose_odom_frame.position.y; + buoy_green_6.z = 0.0; + buoy_green_6.r = 0; + buoy_green_6.g = 255; + buoy_green_6.b = 0; + buoy_vis.push_back(buoy_green_6); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_third_pair; waypoint_third_pair.x = (buoy_landmarks_5_to_6[0].pose_odom_frame.position.x + @@ -132,6 +310,7 @@ void NavigationTaskNode::main_task() { waypoint_third_pair.z = 0.0; send_waypoint(waypoint_third_pair); + set_desired_heading(waypoint_west_buoy, waypoint_third_pair); Eigen::Vector2d direction_vector_second_to_third_pair; direction_vector_second_to_third_pair << waypoint_third_pair.x - waypoint_second_pair.x, @@ -144,57 +323,163 @@ void NavigationTaskNode::main_task() { waypoint_third_pair.y + direction_vector_second_to_third_pair(1) * 2; waypoint_through_third_pair.z = 0.0; send_waypoint(waypoint_through_third_pair); + set_desired_heading(waypoint_third_pair, waypoint_through_third_pair); reach_waypoint(1.0); // North buoy - Eigen::Array predicted_north_buoy = + Eigen::Array predicted_north_buoy = predict_north_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, buoy_landmarks_5_to_6[1].pose_odom_frame.position, direction_vector_second_to_third_pair); - std::vector buoy_landmarks_5_to_7 = + std::vector buoy_landmarks_7 = get_buoy_landmarks(predicted_north_buoy); - if (buoy_landmarks_5_to_7.size() != 3) { + if (buoy_landmarks_7.size() != 1) { RCLCPP_ERROR(this->get_logger(), "Could not find north buoy"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_north; + buoy_north.x = buoy_landmarks_7[0].pose_odom_frame.position.x; + buoy_north.y = buoy_landmarks_7[0].pose_odom_frame.position.y; + buoy_north.z = 0.0; + buoy_north.r = 255; + buoy_north.g = 255; + buoy_north.b = 0; + buoy_vis.push_back(buoy_north); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_north_buoy; - waypoint_north_buoy.x = buoy_landmarks_5_to_7[2].pose_odom_frame.position.x + - direction_vector_second_to_third_pair(0) * 2; - waypoint_north_buoy.y = buoy_landmarks_5_to_7[2].pose_odom_frame.position.y + - direction_vector_second_to_third_pair(1) * 2; + waypoint_north_buoy.x = buoy_landmarks_7[0].pose_odom_frame.position.x + + direction_vector_second_to_third_pair(0) * 2.5; + waypoint_north_buoy.y = buoy_landmarks_7[0].pose_odom_frame.position.y + + direction_vector_second_to_third_pair(1) * 2.5; waypoint_north_buoy.z = 0.0; send_waypoint(waypoint_north_buoy); + set_desired_heading(waypoint_through_third_pair, waypoint_north_buoy); reach_waypoint(1.0); // South buoy - Eigen::Array predicted_south_buoy = - predict_south_buoy(buoy_landmarks_5_to_7[0].pose_odom_frame.position, - buoy_landmarks_5_to_7[1].pose_odom_frame.position, - buoy_landmarks_5_to_7[2].pose_odom_frame.position, + Eigen::Array predicted_south_buoy = + predict_south_buoy(buoy_landmarks_5_to_6[0].pose_odom_frame.position, + buoy_landmarks_5_to_6[1].pose_odom_frame.position, direction_vector_second_to_third_pair); - std::vector buoy_landmarks_7_to_8 = + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_south; + buoy_south.x = predicted_south_buoy(0, 0); + buoy_south.y = predicted_south_buoy(1, 0); + buoy_south.z = 0.0; + buoy_south.r = 255; + buoy_south.g = 255; + buoy_south.b = 0; + buoy_vis.push_back(buoy_south); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_8 = get_buoy_landmarks(predicted_south_buoy); - if (buoy_landmarks_7_to_8.size() != 2) { + if (buoy_landmarks_8.size() != 1) { RCLCPP_ERROR(this->get_logger(), "Could not find south buoy"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_south = pcl::PointXYZRGB(); + buoy_south.x = buoy_landmarks_8[0].pose_odom_frame.position.x; + buoy_south.y = buoy_landmarks_8[0].pose_odom_frame.position.y; + buoy_south.z = 0.0; + buoy_south.r = 255; + buoy_south.g = 255; + buoy_south.b = 0; + buoy_vis.push_back(buoy_south); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + Eigen::Vector2d direction_vector_downwards; + direction_vector_downwards + << buoy_landmarks_5_to_6[1].pose_odom_frame.position.x - + buoy_landmarks_5_to_6[10].pose_odom_frame.position.x, + buoy_landmarks_5_to_6[1].pose_odom_frame.position.y - + buoy_landmarks_5_to_6[0].pose_odom_frame.position.y; + direction_vector_downwards.normalize(); + geometry_msgs::msg::Point waypoint_south_buoy; - waypoint_south_buoy.x = buoy_landmarks_7_to_8[1].pose_odom_frame.position.x - - direction_vector_second_to_third_pair(0) * 2; - waypoint_south_buoy.y = buoy_landmarks_7_to_8[1].pose_odom_frame.position.y - - direction_vector_second_to_third_pair(1) * 2; + waypoint_south_buoy.x = buoy_landmarks_8[0].pose_odom_frame.position.x - + direction_vector_second_to_third_pair(0) * 3 + + direction_vector_downwards(0) * 1; + waypoint_south_buoy.y = buoy_landmarks_8[0].pose_odom_frame.position.y - + direction_vector_second_to_third_pair(1) * 3 + + direction_vector_downwards(1) * 1; waypoint_south_buoy.z = 0.0; send_waypoint(waypoint_south_buoy); - reach_waypoint(1.0); + set_desired_heading(waypoint_north_buoy, waypoint_south_buoy); + reach_waypoint(0.2); // Fourth pair of buoys Eigen::Array predicted_fourth_buoy_pair = predict_fourth_buoy_pair( - buoy_landmarks_5_to_7[0].pose_odom_frame.position, - buoy_landmarks_5_to_7[1].pose_odom_frame.position); + buoy_landmarks_5_to_6[0].pose_odom_frame.position, + buoy_landmarks_5_to_6[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_red_9; + pcl::PointXYZRGB buoy_green_10; + buoy_red_9.x = predicted_fourth_buoy_pair(0, 0); + buoy_red_9.y = predicted_fourth_buoy_pair(1, 0); + buoy_red_9.z = 0.0; + buoy_red_9.r = 255; + buoy_red_9.g = 0; + buoy_red_9.b = 0; + buoy_vis.push_back(buoy_red_9); + buoy_green_10.x = predicted_fourth_buoy_pair(0, 1); + buoy_green_10.y = predicted_fourth_buoy_pair(1, 1); + buoy_green_10.z = 0.0; + buoy_green_10.r = 0; + buoy_green_10.g = 255; + buoy_green_10.b = 0; + buoy_vis.push_back(buoy_green_10); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + std::vector buoy_landmarks_9_to_10 = get_buoy_landmarks(predicted_fourth_buoy_pair); if (buoy_landmarks_9_to_10.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find fourth buoy pair"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_9 = pcl::PointXYZRGB(); + buoy_green_10 = pcl::PointXYZRGB(); + buoy_red_9.x = buoy_landmarks_9_to_10[0].pose_odom_frame.position.x; + buoy_red_9.y = buoy_landmarks_9_to_10[0].pose_odom_frame.position.y; + buoy_red_9.z = 0.0; + buoy_red_9.r = 255; + buoy_red_9.g = 0; + buoy_red_9.b = 0; + buoy_vis.push_back(buoy_red_9); + buoy_green_10.x = buoy_landmarks_9_to_10[1].pose_odom_frame.position.x; + buoy_green_10.y = buoy_landmarks_9_to_10[1].pose_odom_frame.position.y; + buoy_green_10.z = 0.0; + buoy_green_10.r = 0; + buoy_green_10.g = 255; + buoy_green_10.b = 0; + buoy_vis.push_back(buoy_green_10); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_fourth_pair; waypoint_fourth_pair.x = (buoy_landmarks_9_to_10[0].pose_odom_frame.position.x + @@ -206,82 +491,212 @@ void NavigationTaskNode::main_task() { 2; waypoint_fourth_pair.z = 0.0; send_waypoint(waypoint_fourth_pair); + set_desired_heading(waypoint_south_buoy, waypoint_fourth_pair); reach_waypoint(1.0); // East buoy - Eigen::Array predicted_east_buoy = + Eigen::Array predicted_east_buoy = predict_east_buoy(buoy_landmarks_9_to_10[0].pose_odom_frame.position, buoy_landmarks_9_to_10[1].pose_odom_frame.position, direction_vector_second_to_third_pair); - std::vector buoy_landmarks_9_to_11 = + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_east; + buoy_east.x = predicted_east_buoy(0, 0); + buoy_east.y = predicted_east_buoy(1, 0); + buoy_east.z = 0.0; + buoy_east.r = 255; + buoy_east.g = 255; + buoy_east.b = 0; + buoy_vis.push_back(buoy_east); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_11 = get_buoy_landmarks(predicted_east_buoy); - if (buoy_landmarks_9_to_11.size() != 3) { + if (buoy_landmarks_11.size() != 1) { RCLCPP_ERROR(this->get_logger(), "Could not find east buoy"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_east = pcl::PointXYZRGB(); + buoy_east.x = buoy_landmarks_11[0].pose_odom_frame.position.x; + buoy_east.y = buoy_landmarks_11[0].pose_odom_frame.position.y; + buoy_east.z = 0.0; + buoy_east.r = 255; + buoy_east.g = 255; + buoy_east.b = 0; + buoy_vis.push_back(buoy_east); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + Eigen::Vector2d direction_vector_9_to_10; direction_vector_9_to_10 - << buoy_landmarks_9_to_11[1].pose_odom_frame.position.x - - buoy_landmarks_9_to_11[0].pose_odom_frame.position.x, - buoy_landmarks_9_to_11[1].pose_odom_frame.position.y - - buoy_landmarks_9_to_11[0].pose_odom_frame.position.y; + << buoy_landmarks_9_to_10[1].pose_odom_frame.position.x - + buoy_landmarks_9_to_10[0].pose_odom_frame.position.x, + buoy_landmarks_9_to_10[1].pose_odom_frame.position.y - + buoy_landmarks_9_to_10[0].pose_odom_frame.position.y; direction_vector_9_to_10.normalize(); geometry_msgs::msg::Point waypoint_east_buoy; - waypoint_east_buoy.x = buoy_landmarks_9_to_11[2].pose_odom_frame.position.x + - direction_vector_9_to_10(0) * 2; - waypoint_east_buoy.y = buoy_landmarks_9_to_11[2].pose_odom_frame.position.y + - direction_vector_9_to_10(1) * 2; + waypoint_east_buoy.x = buoy_landmarks_11[0].pose_odom_frame.position.x + + direction_vector_9_to_10(0) * 3; + waypoint_east_buoy.y = buoy_landmarks_11[0].pose_odom_frame.position.y + + direction_vector_9_to_10(1) * 3; waypoint_east_buoy.z = 0.0; send_waypoint(waypoint_east_buoy); + set_desired_heading(waypoint_fourth_pair, waypoint_east_buoy); reach_waypoint(1.0); // Fifth pair of buoys - Eigen::Array predicted_fifth_buoy_pair = + Eigen::Array predicted_fifth_buoy_pair = predict_fifth_buoy_pair( - buoy_landmarks_9_to_11[0].pose_odom_frame.position, - buoy_landmarks_9_to_11[1].pose_odom_frame.position, - buoy_landmarks_9_to_11[2].pose_odom_frame.position, + buoy_landmarks_9_to_10[0].pose_odom_frame.position, + buoy_landmarks_9_to_10[1].pose_odom_frame.position, direction_vector_second_to_third_pair); - std::vector buoy_landmarks_11_to_13 = + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_red_12; + pcl::PointXYZRGB buoy_green_13; + buoy_red_12.x = predicted_fifth_buoy_pair(0, 0); + buoy_red_12.y = predicted_fifth_buoy_pair(1, 0); + buoy_red_12.z = 0.0; + buoy_red_12.r = 255; + buoy_red_12.g = 0; + buoy_red_12.b = 0; + buoy_vis.push_back(buoy_red_12); + buoy_green_13.x = predicted_fifth_buoy_pair(0, 1); + buoy_green_13.y = predicted_fifth_buoy_pair(1, 1); + buoy_green_13.z = 0.0; + buoy_green_13.r = 0; + buoy_green_13.g = 255; + buoy_green_13.b = 0; + buoy_vis.push_back(buoy_green_13); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_12_to_13 = get_buoy_landmarks(predicted_fifth_buoy_pair); - if (buoy_landmarks_11_to_13.size() != 3) { + if (buoy_landmarks_12_to_13.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find fifth buoy pair"); } + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_12 = pcl::PointXYZRGB(); + buoy_green_13 = pcl::PointXYZRGB(); + buoy_red_12.x = buoy_landmarks_12_to_13[0].pose_odom_frame.position.x; + buoy_red_12.y = buoy_landmarks_12_to_13[0].pose_odom_frame.position.y; + buoy_red_12.z = 0.0; + buoy_red_12.r = 255; + buoy_red_12.g = 0; + buoy_red_12.b = 0; + buoy_vis.push_back(buoy_red_12); + buoy_green_13.x = buoy_landmarks_12_to_13[1].pose_odom_frame.position.x; + buoy_green_13.y = buoy_landmarks_12_to_13[1].pose_odom_frame.position.y; + buoy_green_13.z = 0.0; + buoy_green_13.r = 0; + buoy_green_13.g = 255; + buoy_green_13.b = 0; + buoy_vis.push_back(buoy_green_13); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_fifth_pair; waypoint_fifth_pair.x = - (buoy_landmarks_11_to_13[1].pose_odom_frame.position.x + - buoy_landmarks_11_to_13[2].pose_odom_frame.position.x) / + (buoy_landmarks_12_to_13[0].pose_odom_frame.position.x + + buoy_landmarks_12_to_13[1].pose_odom_frame.position.x) / 2; waypoint_fifth_pair.y = - (buoy_landmarks_11_to_13[1].pose_odom_frame.position.y + - buoy_landmarks_11_to_13[2].pose_odom_frame.position.y) / + (buoy_landmarks_12_to_13[0].pose_odom_frame.position.y + + buoy_landmarks_12_to_13[1].pose_odom_frame.position.y) / 2; waypoint_fifth_pair.z = 0.0; send_waypoint(waypoint_fifth_pair); + set_desired_heading(waypoint_east_buoy, waypoint_fifth_pair); reach_waypoint(1.0); // Sixth pair of buoys - Eigen::Array predicted_sixth_buoy_pair = + Eigen::Array predicted_sixth_buoy_pair = predict_sixth_buoy_pair( - buoy_landmarks_9_to_11[0].pose_odom_frame.position, - buoy_landmarks_9_to_11[1].pose_odom_frame.position, - buoy_landmarks_11_to_13[1].pose_odom_frame.position, - buoy_landmarks_11_to_13[2].pose_odom_frame.position); - std::vector buoy_landmarks_12_to_15 = + buoy_landmarks_9_to_10[0].pose_odom_frame.position, + buoy_landmarks_9_to_10[1].pose_odom_frame.position, + buoy_landmarks_12_to_13[0].pose_odom_frame.position, + buoy_landmarks_12_to_13[1].pose_odom_frame.position); + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + pcl::PointXYZRGB buoy_red_14; + pcl::PointXYZRGB buoy_green_15; + buoy_red_14.x = predicted_sixth_buoy_pair(0, 0); + buoy_red_14.y = predicted_sixth_buoy_pair(1, 0); + buoy_red_14.z = 0.0; + buoy_red_14.r = 255; + buoy_red_14.g = 0; + buoy_red_14.b = 0; + buoy_vis.push_back(buoy_red_14); + buoy_green_15.x = predicted_sixth_buoy_pair(0, 1); + buoy_green_15.y = predicted_sixth_buoy_pair(1, 1); + buoy_green_15.z = 0.0; + buoy_green_15.r = 0; + buoy_green_15.g = 255; + buoy_green_15.b = 0; + buoy_vis.push_back(buoy_green_15); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + + std::vector buoy_landmarks_14_to_15 = get_buoy_landmarks(predicted_sixth_buoy_pair); - if (buoy_landmarks_12_to_15.size() != 4) { + if (buoy_landmarks_14_to_15.size() != 2) { RCLCPP_ERROR(this->get_logger(), "Could not find sixth buoy pair"); } + + buoy_vis = pcl::PointCloud(); + buoy_vis_msg = sensor_msgs::msg::PointCloud2(); + buoy_red_14 = pcl::PointXYZRGB(); + buoy_green_15 = pcl::PointXYZRGB(); + buoy_red_14.x = buoy_landmarks_14_to_15[0].pose_odom_frame.position.x; + buoy_red_14.y = buoy_landmarks_14_to_15[0].pose_odom_frame.position.y; + buoy_red_14.z = 0.0; + buoy_red_14.r = 255; + buoy_red_14.g = 0; + buoy_red_14.b = 0; + buoy_vis.push_back(buoy_red_14); + buoy_green_15.x = buoy_landmarks_14_to_15[1].pose_odom_frame.position.x; + buoy_green_15.y = buoy_landmarks_14_to_15[1].pose_odom_frame.position.y; + buoy_green_15.z = 0.0; + buoy_green_15.r = 0; + buoy_green_15.g = 255; + buoy_green_15.b = 0; + buoy_vis.push_back(buoy_green_15); + pcl::toROSMsg(buoy_vis, buoy_vis_msg); + buoy_vis_msg.header.frame_id = "odom"; + buoy_vis_msg.header.stamp = this->now(); + buoy_visualization_pub_->publish(buoy_vis_msg); + geometry_msgs::msg::Point waypoint_sixth_pair; waypoint_sixth_pair.x = - (buoy_landmarks_12_to_15[2].pose_odom_frame.position.x + - buoy_landmarks_12_to_15[3].pose_odom_frame.position.x) / + (buoy_landmarks_14_to_15[0].pose_odom_frame.position.x + + buoy_landmarks_14_to_15[1].pose_odom_frame.position.x) / 2; waypoint_sixth_pair.y = - (buoy_landmarks_12_to_15[2].pose_odom_frame.position.y + - buoy_landmarks_12_to_15[3].pose_odom_frame.position.y) / + (buoy_landmarks_14_to_15[0].pose_odom_frame.position.y + + buoy_landmarks_14_to_15[1].pose_odom_frame.position.y) / 2; waypoint_sixth_pair.z = 0.0; send_waypoint(waypoint_sixth_pair); + set_desired_heading(waypoint_fifth_pair, waypoint_sixth_pair); reach_waypoint(1.0); // Gps end goal @@ -290,6 +705,7 @@ void NavigationTaskNode::main_task() { gps_end_goal.y = this->get_parameter("gps_end_y").as_double(); gps_end_goal.z = 0.0; send_waypoint(gps_end_goal); + set_desired_heading(waypoint_sixth_pair, gps_end_goal); } Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { @@ -334,31 +750,27 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array -NavigationTaskNode::predict_first_and_second_buoy_pair( +Eigen::Array +NavigationTaskNode::predict_second_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; direction_vector << previous_waypoint_odom_frame_.x - - this->get_parameter("gps_start_x").as_double(), + odom_start_point_.x, previous_waypoint_odom_frame_.y - - this->get_parameter("gps_start_y").as_double(); + odom_start_point_.y; direction_vector.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x; - predicted_positions(1, 0) = buoy_0.y; - predicted_positions(0, 1) = buoy_1.x; - predicted_positions(1, 1) = buoy_1.y; - predicted_positions(0, 2) = buoy_0.x + direction_vector(0) * 5; - predicted_positions(1, 2) = buoy_0.y + direction_vector(1) * 5; - predicted_positions(0, 3) = buoy_1.x + direction_vector(0) * 5; - predicted_positions(1, 3) = buoy_1.y + direction_vector(1) * 5; + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * 5; + predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * 5; + predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * 5; + predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * 5; return predicted_positions; } -Eigen::Array +Eigen::Array NavigationTaskNode::predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1, const geometry_msgs::msg::Point &buoy_2, @@ -374,15 +786,11 @@ NavigationTaskNode::predict_west_buoy(const geometry_msgs::msg::Point &buoy_0, (buoy_0.y + buoy_2.y) / 2 - (buoy_1.y + buoy_3.y) / 2; direction_vector_up.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_2.x; - predicted_positions(1, 0) = buoy_2.y; - predicted_positions(0, 1) = buoy_3.x; - predicted_positions(1, 1) = buoy_3.y; - predicted_positions(0, 2) = buoy_3.x + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_3.x + direction_vector_right(0) * 12.5 * 1 / 2 + direction_vector_up(0) * 5 * 2 / 5; - predicted_positions(1, 2) = buoy_3.y + + predicted_positions(1, 0) = buoy_3.y + direction_vector_right(1) * 12.5 * 1 / 2 + direction_vector_up(1) * 5 * 2 / 5; @@ -409,7 +817,7 @@ Eigen::Array NavigationTaskNode::predict_third_buoy_pair( return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_north_buoy( +Eigen::Array NavigationTaskNode::predict_north_buoy( const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, const Eigen::Vector2d &direction_vector_second_to_third_pair) { @@ -417,34 +825,27 @@ Eigen::Array NavigationTaskNode::predict_north_buoy( direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; direction_5_to_6.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_5.x; - predicted_positions(1, 0) = buoy_5.y; - predicted_positions(0, 1) = buoy_6.x; - predicted_positions(1, 1) = buoy_6.y; - predicted_positions(0, 2) = buoy_6.x + direction_5_to_6(0) * 12.5 * 0.5 + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5 * 0.5 + direction_vector_second_to_third_pair(0) * 2; - predicted_positions(1, 2) = buoy_6.y + direction_5_to_6(1) * 12.5 * 0.5 + + predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5 * 0.5 + direction_vector_second_to_third_pair(1) * 2; return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_south_buoy( +Eigen::Array NavigationTaskNode::predict_south_buoy( const geometry_msgs::msg::Point &buoy_5, const geometry_msgs::msg::Point &buoy_6, - const geometry_msgs::msg::Point &buoy_7, const Eigen::Vector2d &direction_vector_second_to_third_pair) { Eigen::Vector2d direction_5_to_6; direction_5_to_6 << buoy_6.x - buoy_5.x, buoy_6.y - buoy_5.y; direction_5_to_6.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_7.x; - predicted_positions(1, 0) = buoy_7.y; - predicted_positions(0, 1) = buoy_6.x + direction_5_to_6(0) * 12.5 + + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_6.x + direction_5_to_6(0) * 12.5 + direction_vector_second_to_third_pair(0) * 8; - predicted_positions(1, 1) = buoy_6.y + direction_5_to_6(1) * 12.5 + + predicted_positions(1, 0) = buoy_6.y + direction_5_to_6(1) * 12.5 + direction_vector_second_to_third_pair(1) * 8; return predicted_positions; @@ -468,7 +869,7 @@ Eigen::Array NavigationTaskNode::predict_fourth_buoy_pair( return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_east_buoy( +Eigen::Array NavigationTaskNode::predict_east_buoy( const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, const Eigen::Vector2d &direction_vector_second_to_third_pair) { @@ -476,41 +877,34 @@ Eigen::Array NavigationTaskNode::predict_east_buoy( direction_9_to_10 << buoy_10.x - buoy_9.x, buoy_10.y - buoy_9.y; direction_9_to_10.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_9.x; - predicted_positions(1, 0) = buoy_9.y; - predicted_positions(0, 1) = buoy_10.x; - predicted_positions(1, 1) = buoy_10.y; - predicted_positions(0, 2) = buoy_9.x + direction_9_to_10(0) * 2.5 - + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_9.x + direction_9_to_10(0) * 2.5 - direction_vector_second_to_third_pair(0) * 7.5; - predicted_positions(1, 2) = buoy_9.y + direction_9_to_10(1) * 2.5 - + predicted_positions(1, 0) = buoy_9.y + direction_9_to_10(1) * 2.5 - direction_vector_second_to_third_pair(1) * 7.5; return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_fifth_buoy_pair( +Eigen::Array NavigationTaskNode::predict_fifth_buoy_pair( const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, - const geometry_msgs::msg::Point &buoy_11, const Eigen::Vector2d &direction_vector_second_to_third_pair) { - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_11.x; - predicted_positions(1, 0) = buoy_11.y; - predicted_positions(0, 1) = + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_9.x - direction_vector_second_to_third_pair(0) * 12.5; - predicted_positions(1, 1) = + predicted_positions(1, 0) = buoy_9.y - direction_vector_second_to_third_pair(1) * 12.5; - predicted_positions(0, 2) = + predicted_positions(0, 1) = buoy_10.x - direction_vector_second_to_third_pair(0) * 12.5; - predicted_positions(1, 2) = + predicted_positions(1, 1) = buoy_10.y - direction_vector_second_to_third_pair(1) * 12.5; return predicted_positions; } -Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair( +Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair( const geometry_msgs::msg::Point &buoy_9, const geometry_msgs::msg::Point &buoy_10, const geometry_msgs::msg::Point &buoy_12, @@ -521,15 +915,11 @@ Eigen::Array NavigationTaskNode::predict_sixth_buoy_pair( (buoy_13.y + buoy_12.y) / 2 - (buoy_10.y + buoy_9.y) / 2; direction_fourth_to_fifth_pair.normalize(); - Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_12.x; - predicted_positions(1, 0) = buoy_12.y; - predicted_positions(0, 1) = buoy_13.x; - predicted_positions(1, 1) = buoy_13.y; - predicted_positions(0, 2) = buoy_12.x + direction_fourth_to_fifth_pair(0) * 5; - predicted_positions(1, 2) = buoy_12.y + direction_fourth_to_fifth_pair(1) * 5; - predicted_positions(0, 3) = buoy_13.x + direction_fourth_to_fifth_pair(0) * 5; - predicted_positions(1, 3) = buoy_13.y + direction_fourth_to_fifth_pair(1) * 5; + Eigen::Array predicted_positions; + predicted_positions(0, 0) = buoy_12.x + direction_fourth_to_fifth_pair(0) * 5; + predicted_positions(1, 0) = buoy_12.y + direction_fourth_to_fifth_pair(1) * 5; + predicted_positions(0, 1) = buoy_13.x + direction_fourth_to_fifth_pair(0) * 5; + predicted_positions(1, 1) = buoy_13.y + direction_fourth_to_fifth_pair(1) * 5; return predicted_positions; } diff --git a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp index 8c80ddf..32bf3c3 100644 --- a/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp +++ b/mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp @@ -6,11 +6,13 @@ #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "vortex_msgs/msg/landmark_array.hpp" #include "vortex_msgs/srv/waypoint.hpp" +#include "vortex_msgs/srv/desired_velocity.hpp" #include #include #include @@ -120,15 +122,21 @@ class NjordTaskBaseNode : public rclcpp::Node { */ void reach_waypoint(const double distance_threshold); + void set_desired_heading(const geometry_msgs::msg::Point &prev_waypoint, const geometry_msgs::msg::Point &next_waypoint); + rclcpp::Publisher::SharedPtr gps_map_coord_visualization_pub_; rclcpp::Publisher::SharedPtr waypoint_visualization_pub_; + rclcpp::Publisher::SharedPtr + buoy_visualization_pub_; rclcpp::Subscription::SharedPtr map_origin_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr landmarks_sub_; rclcpp::Client::SharedPtr waypoint_client_; + rclcpp::Client::SharedPtr + heading_client_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; @@ -148,6 +156,7 @@ class NjordTaskBaseNode : public rclcpp::Node { bool navigation_ready_ = false; geometry_msgs::msg::Point previous_waypoint_odom_frame_; + geometry_msgs::msg::Point odom_start_point_; }; #endif // NJORD_TASK_BASE_ROS_HPP diff --git a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp index d8ed78a..06ab1bc 100644 --- a/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp +++ b/mission/njord_tasks/njord_task_base/src/njord_task_base_ros.cpp @@ -38,6 +38,10 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, gps_map_coord_visualization_pub_ = this->create_publisher( "/gps_map_coord_visualization", qos_sensor_data); + + buoy_visualization_pub_ = + this->create_publisher( + "/buoy_visualization", qos_sensor_data); tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -61,7 +65,10 @@ NjordTaskBaseNode::NjordTaskBaseNode(const std::string &node_name, "/waypoint_visualization", qos_sensor_data); waypoint_client_ = - this->create_client("/waypoint"); + this->create_client("waypoint_list"); + + heading_client_ = + this->create_client("yaw_reference"); } void NjordTaskBaseNode::get_map_odom_tf() { @@ -442,6 +449,11 @@ void NjordTaskBaseNode::send_waypoint( waypoint.x, waypoint.y); // Check if the service was successful + geometry_msgs::msg::PoseStamped waypoint_vis; + waypoint_vis.header.frame_id = "odom"; + waypoint_vis.pose.position.x = waypoint.x; + waypoint_vis.pose.position.y = waypoint.y; + waypoint_visualization_pub_->publish(waypoint_vis); auto status = result_future.wait_for(std::chrono::seconds(5)); while (status == std::future_status::timeout) { RCLCPP_INFO(this->get_logger(), "Waypoint service timed out"); @@ -451,11 +463,6 @@ void NjordTaskBaseNode::send_waypoint( RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); } - geometry_msgs::msg::PoseStamped waypoint_vis; - waypoint_vis.header.frame_id = "odom"; - waypoint_vis.pose.position.x = waypoint.x; - waypoint_vis.pose.position.y = waypoint.y; - waypoint_visualization_pub_->publish(waypoint_vis); previous_waypoint_odom_frame_ = waypoint; } @@ -478,4 +485,24 @@ void NjordTaskBaseNode::reach_waypoint(const double distance_threshold) { } RCLCPP_INFO(this->get_logger(), "Reached waypoint"); +} + +void NjordTaskBaseNode::set_desired_heading( + const geometry_msgs::msg::Point &prev_waypoint, + const geometry_msgs::msg::Point &next_waypoint) { + auto request = std::make_shared(); + double dx = next_waypoint.x - prev_waypoint.x; + double dy = next_waypoint.y - prev_waypoint.y; + double desired_heading = std::atan2(dy, dx); + request->u_desired = desired_heading; + auto result_future = heading_client_->async_send_request(request); + RCLCPP_INFO(this->get_logger(), "Desired heading sent: %f", desired_heading); + auto status = result_future.wait_for(std::chrono::seconds(5)); + while (status == std::future_status::timeout) { + RCLCPP_INFO(this->get_logger(), "Desired heading service timed out"); + status = result_future.wait_for(std::chrono::seconds(5)); + } + if (!result_future.get()->success) { + RCLCPP_INFO(this->get_logger(), "Desired heading service failed"); + } } \ No newline at end of file