diff --git a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp index 16e1199..c6309ca 100644 --- a/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp +++ b/mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp @@ -1,14 +1,14 @@ #ifndef COLLISION_AVOIDANCE_TASK_ROS_HPP #define COLLISION_AVOIDANCE_TASK_ROS_HPP +#include +#include #include #include #include #include #include -#include // Required for tf2::doTransform -#include -#include +#include // Required for tf2::doTransform namespace collision_avoidance_task { @@ -30,7 +30,7 @@ class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { /** * @brief Predict the positions of the first two buoys - * + * * @return Eigen::Array predicted_positions */ Eigen::Array predict_first_buoy_pair(); @@ -39,33 +39,33 @@ class CollisionAvoidanceTaskNode : public NjordTaskBaseNode { predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1); - void track_vessel(const Eigen::Vector2d& direction_vector_downwards, - const Eigen::Vector2d& direction_vector_forwards, - const geometry_msgs::msg::Point& waypoint_second_buoy_pair); + void track_vessel(const Eigen::Vector2d &direction_vector_downwards, + const Eigen::Vector2d &direction_vector_forwards, + const geometry_msgs::msg::Point &waypoint_second_buoy_pair); nav_msgs::msg::Odometry get_vessel_odom(); - LandmarkOdomID filter_landmarks( - const vortex_msgs::msg::LandmarkArray &landmarks, - const Eigen::Vector2d& direction_vector_downwards, - const Eigen::Vector2d& direction_vector_forwards, - const geometry_msgs::msg::Point& waypoint_second_buoy_pair); + LandmarkOdomID + filter_landmarks(const vortex_msgs::msg::LandmarkArray &landmarks, + const Eigen::Vector2d &direction_vector_downwards, + const Eigen::Vector2d &direction_vector_forwards, + const geometry_msgs::msg::Point &waypoint_second_buoy_pair); void vessel_collision_heading(); - double calculate_angle(const geometry_msgs::msg::Vector3& twist1, const geometry_msgs::msg::Vector3& twist2); + double calculate_angle(const geometry_msgs::msg::Vector3 &twist1, + const geometry_msgs::msg::Vector3 &twist2); - bool has_vessel_passed_freya(const Eigen::Vector2d& direction_vector_downwards); + bool + has_vessel_passed_freya(const Eigen::Vector2d &direction_vector_downwards); 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_1); Eigen::Array predict_fourth_buoy_pair(const geometry_msgs::msg::Point &buoy_red, - const geometry_msgs::msg::Point &buoy_green); - - + const geometry_msgs::msg::Point &buoy_green); private: mutable std::mutex vessel_odom_mutex_; diff --git a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp index 9d3f7ca..9c79855 100644 --- a/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp +++ b/mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp @@ -9,7 +9,7 @@ CollisionAvoidanceTaskNode::CollisionAvoidanceTaskNode( declare_parameter("distance_to_first_buoy_pair", 2.0); declare_parameter("distance_between_buoy_pairs", 5.0); declare_parameter("vessel_assignment_confidence", 5); - + std::thread(&CollisionAvoidanceTaskNode::main_task, this).detach(); } @@ -17,7 +17,8 @@ void CollisionAvoidanceTaskNode::main_task() { navigation_ready(); RCLCPP_INFO(this->get_logger(), "Collision avoidance task started"); odom_start_point_ = get_odom()->pose.pose.position; - Eigen::Array predicted_first_buoy_pair = predict_first_buoy_pair(); + Eigen::Array predicted_first_buoy_pair = + predict_first_buoy_pair(); sensor_msgs::msg::PointCloud2 buoy_vis_msg; pcl::PointCloud buoy_vis; @@ -42,7 +43,7 @@ void CollisionAvoidanceTaskNode::main_task() { 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 + // First first buoy pair is far away, should be closer before trying to // measure it. double gps_end_x = this->get_parameter("gps_end_x").as_double(); double gps_end_y = this->get_parameter("gps_end_y").as_double(); @@ -56,10 +57,10 @@ void CollisionAvoidanceTaskNode::main_task() { if (distance > 5.0) { auto odom = get_odom(); geometry_msgs::msg::Point waypoint_toward_start; - waypoint_toward_start.x = - odom->pose.pose.position.x + direction_vector_to_end(0) * (distance - 3); - waypoint_toward_start.y = - odom->pose.pose.position.y + direction_vector_to_end(1) * (distance - 3); + waypoint_toward_start.x = odom->pose.pose.position.x + + direction_vector_to_end(0) * (distance - 3); + waypoint_toward_start.y = odom->pose.pose.position.y + + direction_vector_to_end(1) * (distance - 3); waypoint_toward_start.z = 0.0; send_waypoint(waypoint_toward_start); set_desired_heading(odom_start_point_, waypoint_toward_start); @@ -144,7 +145,7 @@ void CollisionAvoidanceTaskNode::main_task() { RCLCPP_ERROR(this->get_logger(), "Could not find two buoys"); } - buoy_vis = pcl::PointCloud(); + buoy_vis = pcl::PointCloud(); buoy_vis_msg = sensor_msgs::msg::PointCloud2(); buoy_red_0 = pcl::PointXYZRGB(); buoy_green_1 = pcl::PointXYZRGB(); @@ -180,75 +181,106 @@ void CollisionAvoidanceTaskNode::main_task() { send_waypoint(waypoint_second_pair); set_desired_heading(odom_start_point_, waypoint_second_pair); reach_waypoint(3.0); - + Eigen::Vector2d direction_vector_forwards; Eigen::Vector2d direction_vector_downwards; direction_vector_forwards << waypoint_second_pair.x - waypoint_first_pair.x, waypoint_second_pair.y - waypoint_first_pair.y; direction_vector_forwards.normalize(); - direction_vector_downwards << (measured_first_buoy_pair[1].pose_odom_frame.position.x + measured_second_buoy_pair[1].pose_odom_frame.position.x)/2 - - (measured_first_buoy_pair[0].pose_odom_frame.position.x + measured_second_buoy_pair[0].pose_odom_frame.position.x)/2, - (measured_first_buoy_pair[1].pose_odom_frame.position.y + measured_second_buoy_pair[1].pose_odom_frame.position.y)/2 - - (measured_first_buoy_pair[0].pose_odom_frame.position.y + measured_second_buoy_pair[0].pose_odom_frame.position.y)/2; + direction_vector_downwards + << (measured_first_buoy_pair[1].pose_odom_frame.position.x + + measured_second_buoy_pair[1].pose_odom_frame.position.x) / + 2 - + (measured_first_buoy_pair[0].pose_odom_frame.position.x + + measured_second_buoy_pair[0].pose_odom_frame.position.x) / + 2, + (measured_first_buoy_pair[1].pose_odom_frame.position.y + + measured_second_buoy_pair[1].pose_odom_frame.position.y) / + 2 - + (measured_first_buoy_pair[0].pose_odom_frame.position.y + + measured_second_buoy_pair[0].pose_odom_frame.position.y) / + 2; direction_vector_downwards.normalize(); geometry_msgs::msg::Point waypoint_midpoint; - waypoint_midpoint.x = waypoint_second_pair.x + direction_vector_forwards(0) * 10; - waypoint_midpoint.y = waypoint_second_pair.y + direction_vector_forwards(1) * 10; + waypoint_midpoint.x = + waypoint_second_pair.x + direction_vector_forwards(0) * 10; + waypoint_midpoint.y = + waypoint_second_pair.y + direction_vector_forwards(1) * 10; waypoint_midpoint.z = 0.0; send_waypoint(waypoint_midpoint); set_desired_heading(waypoint_second_pair, waypoint_midpoint); - std::thread(std::bind(&CollisionAvoidanceTaskNode::track_vessel, this, direction_vector_forwards, direction_vector_downwards, waypoint_second_pair)).detach(); + std::thread(std::bind(&CollisionAvoidanceTaskNode::track_vessel, this, + direction_vector_forwards, direction_vector_downwards, + waypoint_second_pair)) + .detach(); RCLCPP_INFO(this->get_logger(), "Tracking vessel"); reach_waypoint(9.0); - // Run until the angle between the twist vectors are between 60 and 120 degrees + // Run until the angle between the twist vectors are between 60 and 120 + // degrees vessel_collision_heading(); auto vessel_odom = get_vessel_odom(); auto freya_odom = get_odom(); Eigen::Vector2d direction_vector_freya_to_vessel; - direction_vector_freya_to_vessel << vessel_odom.pose.pose.position.x - freya_odom->pose.pose.position.x, + direction_vector_freya_to_vessel + << vessel_odom.pose.pose.position.x - freya_odom->pose.pose.position.x, vessel_odom.pose.pose.position.y - freya_odom->pose.pose.position.y; // Project the vector do find "height" difference of vessels - double downwards_diff = direction_vector_freya_to_vessel.dot(direction_vector_downwards); + double downwards_diff = + direction_vector_freya_to_vessel.dot(direction_vector_downwards); geometry_msgs::msg::Point first_avoidance_waypoint; if (std::abs(downwards_diff) > 5) { - first_avoidance_waypoint.x = vessel_odom.pose.pose.position.x - direction_vector_forwards(0) * 3; - first_avoidance_waypoint.y = vessel_odom.pose.pose.position.y - direction_vector_forwards(1) * 3; + first_avoidance_waypoint.x = + vessel_odom.pose.pose.position.x - direction_vector_forwards(0) * 3; + first_avoidance_waypoint.y = + vessel_odom.pose.pose.position.y - direction_vector_forwards(1) * 3; first_avoidance_waypoint.z = 0.0; send_waypoint(first_avoidance_waypoint); - set_desired_heading(freya_odom->pose.pose.position, first_avoidance_waypoint); + set_desired_heading(freya_odom->pose.pose.position, + first_avoidance_waypoint); } else { - first_avoidance_waypoint.x = vessel_odom.pose.pose.position.x - direction_vector_forwards(0) * 3 + direction_vector_downwards(0) * 2; - first_avoidance_waypoint.y = vessel_odom.pose.pose.position.y - direction_vector_forwards(1) * 3 + direction_vector_downwards(1) * 2; + first_avoidance_waypoint.x = vessel_odom.pose.pose.position.x - + direction_vector_forwards(0) * 3 + + direction_vector_downwards(0) * 2; + first_avoidance_waypoint.y = vessel_odom.pose.pose.position.y - + direction_vector_forwards(1) * 3 + + direction_vector_downwards(1) * 2; first_avoidance_waypoint.z = 0.0; send_waypoint(first_avoidance_waypoint); - set_desired_heading(freya_odom->pose.pose.position, first_avoidance_waypoint); + set_desired_heading(freya_odom->pose.pose.position, + first_avoidance_waypoint); } - while(!has_vessel_passed_freya(direction_vector_downwards)){ + while (!has_vessel_passed_freya(direction_vector_downwards)) { RCLCPP_INFO(this->get_logger(), "Vessel has not passed Freya yet"); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } freya_odom = get_odom(); geometry_msgs::msg::Point second_avoidance_waypoint; - second_avoidance_waypoint.x = freya_odom->pose.pose.position.x + direction_vector_forwards(0) * 5; - second_avoidance_waypoint.y = freya_odom->pose.pose.position.y + direction_vector_forwards(1) * 5; + second_avoidance_waypoint.x = + freya_odom->pose.pose.position.x + direction_vector_forwards(0) * 5; + second_avoidance_waypoint.y = + freya_odom->pose.pose.position.y + direction_vector_forwards(1) * 5; second_avoidance_waypoint.z = 0.0; // reach_waypoint(1.0); geometry_msgs::msg::Point back_on_track_waypoint; - back_on_track_waypoint.x = waypoint_second_pair.x + direction_vector_forwards(0) * 15 + direction_vector_downwards(0) * 0; - back_on_track_waypoint.y = waypoint_second_pair.y + direction_vector_forwards(1) * 15 + direction_vector_downwards(1) * 0; + back_on_track_waypoint.x = waypoint_second_pair.x + + direction_vector_forwards(0) * 15 + + direction_vector_downwards(0) * 0; + back_on_track_waypoint.y = waypoint_second_pair.y + + direction_vector_forwards(1) * 15 + + direction_vector_downwards(1) * 0; back_on_track_waypoint.z = 0.0; - auto request = std::make_shared(); + auto request = std::make_shared(); request->waypoint.push_back(second_avoidance_waypoint); request->waypoint.push_back(back_on_track_waypoint); auto result_future = waypoint_client_->async_send_request(request); - + // Check if the service was successful auto odom = get_odom(); double dx = back_on_track_waypoint.x - odom->pose.pose.position.x; @@ -281,7 +313,7 @@ void CollisionAvoidanceTaskNode::main_task() { predict_third_buoy_pair( measured_first_buoy_pair[0].pose_odom_frame.position, measured_first_buoy_pair[1].pose_odom_frame.position); - + buoy_vis = pcl::PointCloud(); buoy_vis_msg = sensor_msgs::msg::PointCloud2(); buoy_red_0 = pcl::PointXYZRGB(); @@ -342,7 +374,7 @@ void CollisionAvoidanceTaskNode::main_task() { waypoint_third_pair.y = (measured_third_buoy_pair[0].pose_odom_frame.position.y + measured_third_buoy_pair[1].pose_odom_frame.position.y) / - 2; + 2; waypoint_third_pair.z = 0.0; send_waypoint(waypoint_third_pair); set_desired_heading(back_on_track_waypoint, waypoint_third_pair); @@ -425,10 +457,10 @@ void CollisionAvoidanceTaskNode::main_task() { waypoint_end.z = 0.0; send_waypoint(waypoint_end); set_desired_heading(waypoint_fourth_pair, waypoint_end); - } -Eigen::Array CollisionAvoidanceTaskNode::predict_first_buoy_pair() { +Eigen::Array +CollisionAvoidanceTaskNode::predict_first_buoy_pair() { // Predict the positions of the first two buoys geometry_msgs::msg::PoseStamped buoy_0_base_link_frame; geometry_msgs::msg::PoseStamped buoy_1_base_link_frame; @@ -482,59 +514,67 @@ Eigen::Array CollisionAvoidanceTaskNode::predict_second_buoy_pair( this->get_parameter("distance_between_buoy_pairs").as_double(); Eigen::Array predicted_positions; - predicted_positions(0, 0) = buoy_0.x + direction_vector(0) * distance_between_buoy_pairs; - predicted_positions(1, 0) = buoy_0.y + direction_vector(1) * distance_between_buoy_pairs; - predicted_positions(0, 1) = buoy_1.x + direction_vector(0) * distance_between_buoy_pairs; - predicted_positions(1, 1) = buoy_1.y + direction_vector(1) * distance_between_buoy_pairs; + predicted_positions(0, 0) = + buoy_0.x + direction_vector(0) * distance_between_buoy_pairs; + predicted_positions(1, 0) = + buoy_0.y + direction_vector(1) * distance_between_buoy_pairs; + predicted_positions(0, 1) = + buoy_1.x + direction_vector(0) * distance_between_buoy_pairs; + predicted_positions(1, 1) = + buoy_1.y + direction_vector(1) * distance_between_buoy_pairs; return predicted_positions; } -void CollisionAvoidanceTaskNode::track_vessel(const Eigen::Vector2d& direction_vector_downwards, - const Eigen::Vector2d& direction_vector_forwards, - const geometry_msgs::msg::Point& waypoint_second_buoy_pair) { +void CollisionAvoidanceTaskNode::track_vessel( + const Eigen::Vector2d &direction_vector_downwards, + const Eigen::Vector2d &direction_vector_forwards, + const geometry_msgs::msg::Point &waypoint_second_buoy_pair) { std::shared_ptr landmark_msg; int32_t prev_vessel_id = -1; - int vessel_assignment_confidence = this->get_parameter("vessel_assignment_confidence").as_int(); + int vessel_assignment_confidence = + this->get_parameter("vessel_assignment_confidence").as_int(); int current_confidence = 0; - while (true){ - landmark_msg = get_landmarks_odom_frame(); - LandmarkOdomID vessel = filter_landmarks(*landmark_msg, direction_vector_downwards, direction_vector_forwards, waypoint_second_buoy_pair); + while (true) { + landmark_msg = get_landmarks_odom_frame(); + LandmarkOdomID vessel = + filter_landmarks(*landmark_msg, direction_vector_downwards, + direction_vector_forwards, waypoint_second_buoy_pair); - if (vessel.id == -1){ - current_confidence = 0; - continue; - } - if (current_confidence == 0){ - prev_vessel_id = vessel.id; - current_confidence++; - continue; - } - if (vessel.id != prev_vessel_id){ - current_confidence = 0; - continue; - } - if (vessel.id == prev_vessel_id){ - if(current_confidence < vessel_assignment_confidence){ - current_confidence++; - continue; - } + if (vessel.id == -1) { + current_confidence = 0; + continue; + } + if (current_confidence == 0) { + prev_vessel_id = vessel.id; + current_confidence++; + continue; + } + if (vessel.id != prev_vessel_id) { + current_confidence = 0; + continue; + } + if (vessel.id == prev_vessel_id) { + if (current_confidence < vessel_assignment_confidence) { current_confidence++; - std::unique_lock lock(vessel_odom_mutex_); - vessel_odom_ = vessel.odom; - new_vessel_odom_msg_ = true; - lock.unlock(); - vessel_odom_cv_.notify_one(); continue; } + current_confidence++; + std::unique_lock lock(vessel_odom_mutex_); + vessel_odom_ = vessel.odom; + new_vessel_odom_msg_ = true; + lock.unlock(); + vessel_odom_cv_.notify_one(); + continue; } + } } LandmarkOdomID CollisionAvoidanceTaskNode::filter_landmarks( const vortex_msgs::msg::LandmarkArray &landmarks, - const Eigen::Vector2d& direction_vector_downwards, - const Eigen::Vector2d& direction_vector_forwards, - const geometry_msgs::msg::Point& waypoint_second_buoy_pair) { + const Eigen::Vector2d &direction_vector_downwards, + const Eigen::Vector2d &direction_vector_forwards, + const geometry_msgs::msg::Point &waypoint_second_buoy_pair) { double max_velocity = std::numeric_limits::min(); LandmarkOdomID filtered_landmark; filtered_landmark.id = -1; @@ -543,18 +583,21 @@ LandmarkOdomID CollisionAvoidanceTaskNode::filter_landmarks( landmark.odom.twist.twist.linear.y); geometry_msgs::msg::Point landmark_pose = landmark.odom.pose.pose.position; Eigen::Vector2d vector_waypoint_to_landmark; - vector_waypoint_to_landmark << landmark_pose.x - waypoint_second_buoy_pair.x, + vector_waypoint_to_landmark + << landmark_pose.x - waypoint_second_buoy_pair.x, landmark_pose.y - waypoint_second_buoy_pair.y; - double projection_down = vector_waypoint_to_landmark.dot(direction_vector_downwards); - double projection_forwards = vector_waypoint_to_landmark.dot(direction_vector_forwards); + double projection_down = + vector_waypoint_to_landmark.dot(direction_vector_downwards); + double projection_forwards = + vector_waypoint_to_landmark.dot(direction_vector_forwards); - if (projection_down < -1 || projection_down > 20){ + if (projection_down < -1 || projection_down > 20) { continue; } - if (projection_forwards < -1 || projection_forwards > 20){ + if (projection_forwards < -1 || projection_forwards > 20) { continue; } - if (velocity > max_velocity && velocity > 0.1){ + if (velocity > max_velocity && velocity > 0.1) { max_velocity = velocity; filtered_landmark.odom = landmark.odom; filtered_landmark.id = landmark.id; @@ -579,75 +622,82 @@ void CollisionAvoidanceTaskNode::vessel_collision_heading() { geometry_msgs::msg::TransformStamped transform; while (!transform_success) { try { - transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero); + transform = tf_buffer_->lookupTransform("odom", "base_link", + tf2::TimePointZero); transform_success = true; } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); } } geometry_msgs::msg::Vector3 freya_odom_twist; - tf2::doTransform(freya_odom->twist.twist.linear, freya_odom_twist, transform); + tf2::doTransform(freya_odom->twist.twist.linear, freya_odom_twist, + transform); auto vessel_odom = get_vessel_odom(); - double collision_angle = calculate_angle(freya_odom_twist, vessel_odom.twist.twist.linear); + double collision_angle = + calculate_angle(freya_odom_twist, vessel_odom.twist.twist.linear); if (collision_angle > 60 || collision_angle < 120) { - return; + return; } continue; } - } // Function to calculate the angle between two vectors -double CollisionAvoidanceTaskNode::calculate_angle(const geometry_msgs::msg::Vector3& twist1, const geometry_msgs::msg::Vector3& twist2) { - // Extract linear velocities - double Ax = twist1.x; - double Ay = twist1.y; - double Bx = twist2.x; - double By = twist2.y; +double CollisionAvoidanceTaskNode::calculate_angle( + const geometry_msgs::msg::Vector3 &twist1, + const geometry_msgs::msg::Vector3 &twist2) { + // Extract linear velocities + double Ax = twist1.x; + double Ay = twist1.y; + double Bx = twist2.x; + double By = twist2.y; - // Calculate dot product - double dot_product = Ax * Bx + Ay * By; + // Calculate dot product + double dot_product = Ax * Bx + Ay * By; - // Calculate magnitudes - double magnitude_A = std::sqrt(Ax * Ax + Ay * Ay); - double magnitude_B = std::sqrt(Bx * Bx + By * By); + // Calculate magnitudes + double magnitude_A = std::sqrt(Ax * Ax + Ay * Ay); + double magnitude_B = std::sqrt(Bx * Bx + By * By); - // Calculate angle in radians - double angle_radians = std::acos(dot_product / (magnitude_A * magnitude_B)); + // Calculate angle in radians + double angle_radians = std::acos(dot_product / (magnitude_A * magnitude_B)); - // Convert to degrees if needed - double angle_degrees = angle_radians * (180.0 / M_PI); + // Convert to degrees if needed + double angle_degrees = angle_radians * (180.0 / M_PI); - return angle_degrees; + return angle_degrees; } -bool CollisionAvoidanceTaskNode::has_vessel_passed_freya(const Eigen::Vector2d& direction_vector_downwards) { - // Calculate the relative vector from freya to the vessel - auto freya_pose = get_odom(); - auto vessel_pose = get_vessel_odom(); - Eigen::Vector2d direction_vector_freya_to_vessel; - direction_vector_freya_to_vessel << vessel_pose.pose.pose.position.x - freya_pose->pose.pose.position.x, - vessel_pose.pose.pose.position.y - freya_pose->pose.pose.position.y; - - // Project the relative vector onto the direction_vector_downwards - double projection = direction_vector_freya_to_vessel.dot(direction_vector_downwards); - - // Check the sign of the projection - if (projection < 0) { - // The vessel has passed freya - return true; - } else { - // The vessel has not passed freya yet - return false; - } +bool CollisionAvoidanceTaskNode::has_vessel_passed_freya( + const Eigen::Vector2d &direction_vector_downwards) { + // Calculate the relative vector from freya to the vessel + auto freya_pose = get_odom(); + auto vessel_pose = get_vessel_odom(); + Eigen::Vector2d direction_vector_freya_to_vessel; + direction_vector_freya_to_vessel + << vessel_pose.pose.pose.position.x - freya_pose->pose.pose.position.x, + vessel_pose.pose.pose.position.y - freya_pose->pose.pose.position.y; + + // Project the relative vector onto the direction_vector_downwards + double projection = + direction_vector_freya_to_vessel.dot(direction_vector_downwards); + + // Check the sign of the projection + if (projection < 0) { + // The vessel has passed freya + return true; + } else { + // The vessel has not passed freya yet + return false; + } } Eigen::Array CollisionAvoidanceTaskNode::predict_third_buoy_pair( const geometry_msgs::msg::Point &buoy_0, const geometry_msgs::msg::Point &buoy_1) { Eigen::Vector2d direction_vector; - direction_vector << this->get_parameter("gps_end_x").as_double() - odom_start_point_.x, + direction_vector << this->get_parameter("gps_end_x").as_double() - + odom_start_point_.x, this->get_parameter("gps_end_y").as_double() - odom_start_point_.y; direction_vector.normalize(); Eigen::Array predicted_positions; @@ -663,7 +713,8 @@ Eigen::Array CollisionAvoidanceTaskNode::predict_fourth_buoy_pair( const geometry_msgs::msg::Point &buoy_red, const geometry_msgs::msg::Point &buoy_green) { Eigen::Vector2d direction_vector; - direction_vector << this->get_parameter("gps_end_x").as_double() - odom_start_point_.x, + direction_vector << this->get_parameter("gps_end_x").as_double() - + odom_start_point_.x, this->get_parameter("gps_end_y").as_double() - odom_start_point_.y; direction_vector.normalize(); Eigen::Array predicted_positions; 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 53f844e..a8137e9 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -56,10 +56,10 @@ void ManeuveringTaskNode::main_task() { if (distance > 5.0) { auto odom = get_odom(); geometry_msgs::msg::Point waypoint_toward_start; - waypoint_toward_start.x = - odom->pose.pose.position.x + direction_vector_to_end(0) * (distance - 3); - waypoint_toward_start.y = - odom->pose.pose.position.y + direction_vector_to_end(1) * (distance - 3); + waypoint_toward_start.x = odom->pose.pose.position.x + + direction_vector_to_end(0) * (distance - 3); + waypoint_toward_start.y = odom->pose.pose.position.y + + direction_vector_to_end(1) * (distance - 3); waypoint_toward_start.z = 0.0; send_waypoint(waypoint_toward_start); reach_waypoint(2.0); 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 cdd12ad..8a547d6 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 @@ -118,14 +118,17 @@ void NjordTaskBaseNode::set_gps_odom_points() { gps_start_odom_frame.position.y = this->get_parameter("gps_start_y").as_double(); geometry_msgs::msg::Pose gps_end_odom_frame; - gps_end_odom_frame.position.x = this->get_parameter("gps_end_x").as_double(); - gps_end_odom_frame.position.y = this->get_parameter("gps_end_y").as_double(); + gps_end_odom_frame.position.x = + this->get_parameter("gps_end_x").as_double(); + gps_end_odom_frame.position.y = + this->get_parameter("gps_end_y").as_double(); gps_points_odom_frame.poses.push_back(gps_start_odom_frame); gps_points_odom_frame.poses.push_back(gps_end_odom_frame); gps_map_coord_visualization_pub_->publish(gps_points_odom_frame); - RCLCPP_INFO(this->get_logger(), "GPS odom frame coordinates set to: %f, %f, %f, %f", - gps_start_odom_frame.position.x, gps_start_odom_frame.position.y, - gps_end_odom_frame.position.x, gps_end_odom_frame.position.y); + RCLCPP_INFO( + this->get_logger(), "GPS odom frame coordinates set to: %f, %f, %f, %f", + gps_start_odom_frame.position.x, gps_start_odom_frame.position.y, + gps_end_odom_frame.position.x, gps_end_odom_frame.position.y); return; } auto [gps_start_x, gps_start_y] =