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 0a15d3f..31b523b 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); - std::thread(&DockingTaskNode::main_task, this).detach(); } @@ -61,8 +60,8 @@ void DockingTaskNode::main_task() { waypoint_to_approach_first_pair_base_link.y = 0.0; waypoint_to_approach_first_pair_base_link.z = 0.0; try { - auto transform = tf_buffer_->lookupTransform( - "odom", "base_link", tf2::TimePointZero); + auto transform = + tf_buffer_->lookupTransform("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); @@ -122,7 +121,7 @@ void DockingTaskNode::main_task() { predict_second_buoy_pair( buoy_landmarks_0_to_1[0].pose_odom_frame.position, buoy_landmarks_0_to_1[1].pose_odom_frame.position); - + buoy_vis = pcl::PointCloud(); buoy_vis_msg = sensor_msgs::msg::PointCloud2(); buoy_red_0 = pcl::PointXYZRGB(); @@ -190,12 +189,11 @@ void DockingTaskNode::main_task() { reach_waypoint(1.0); // Third pair of buoys - 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_2_to_3[2].pose_odom_frame.position, - buoy_landmarks_2_to_3[3].pose_odom_frame.position); + 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_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(); @@ -262,7 +260,7 @@ void DockingTaskNode::main_task() { 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(); } @@ -314,10 +312,8 @@ 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 - - odom_start_point_.x, - previous_waypoint_odom_frame_.y - - odom_start_point_.y; + direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, + previous_waypoint_odom_frame_.y - 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 a1106fb..a131bd2 100644 --- a/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp +++ b/mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp @@ -22,32 +22,33 @@ void ManeuveringTaskNode::main_task() { 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); + 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_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 - odom_start_point_.x, gps_end_y - odom_start_point_.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 = @@ -67,29 +68,29 @@ 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); - + 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 + @@ -122,53 +123,53 @@ void ManeuveringTaskNode::main_task() { 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(); - 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); + 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); + 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 = @@ -186,8 +187,7 @@ void ManeuveringTaskNode::main_task() { // Setup variable to navigate formation Eigen::Vector2d direction_vector_forwards; - direction_vector_forwards - << (waypoint_second_pair.x - waypoint_first_pair.x), + direction_vector_forwards << (waypoint_second_pair.x - waypoint_first_pair.x), (waypoint_second_pair.y - waypoint_first_pair.y); direction_vector_forwards.normalize(); @@ -212,7 +212,7 @@ void ManeuveringTaskNode::main_task() { measured_buoy_2_to_3[0].pose_odom_frame.position; geometry_msgs::msg::Point green_buoy = measured_buoy_2_to_3[1].pose_odom_frame.position; - geometry_msgs::msg::Point waypoint_prev_pair = waypoint_second_pair; + 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 = @@ -285,8 +285,7 @@ void ManeuveringTaskNode::main_task() { reach_waypoint(1.0); red_buoy = measured_next_pair[0].pose_odom_frame.position; green_buoy = measured_next_pair[1].pose_odom_frame.position; - vector_to_next_pair - << waypoint_next_pair.x - waypoint_prev_pair.x, + vector_to_next_pair << waypoint_next_pair.x - waypoint_prev_pair.x, waypoint_next_pair.y - waypoint_prev_pair.y; waypoint_prev_pair = waypoint_next_pair; } @@ -320,54 +319,54 @@ void ManeuveringTaskNode::main_task() { 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); + 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); + 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 = @@ -440,10 +439,8 @@ 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 - - odom_start_point_.x, - previous_waypoint_odom_frame_.y - - odom_start_point_.y; + direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, + previous_waypoint_odom_frame_.y - odom_start_point_.y; direction_vector.normalize(); Eigen::Array predicted_positions; 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 0185283..e224c3d 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 @@ -7,7 +7,6 @@ #include #include - namespace navigation_task { class NavigationTaskNode : public NjordTaskBaseNode { @@ -38,7 +37,7 @@ class NavigationTaskNode : public NjordTaskBaseNode { */ Eigen::Array predict_second_buoy_pair(const geometry_msgs::msg::Point &buoy_0, - const geometry_msgs::msg::Point &buoy_1); + const geometry_msgs::msg::Point &buoy_1); /** * @brief Predict the positions of the west buoy by using the two first buoy @@ -118,7 +117,7 @@ class NavigationTaskNode : public NjordTaskBaseNode { * 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 + * of the fifth buoy pair */ Eigen::Array predict_fifth_buoy_pair( const geometry_msgs::msg::Point &buoy_9, 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 895fc85..9398eca 100644 --- a/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp +++ b/mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp @@ -41,7 +41,7 @@ void NavigationTaskNode::main_task() { 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) { @@ -57,7 +57,8 @@ 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); + 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()); @@ -87,7 +88,7 @@ void NavigationTaskNode::main_task() { 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.frame_id = "odom"; buoy_vis_msg.header.stamp = this->now(); buoy_visualization_pub_->publish(buoy_vis_msg); @@ -106,10 +107,9 @@ void NavigationTaskNode::main_task() { reach_waypoint(1.0); // Second pair of buoys - 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); + 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); buoy_vis = pcl::PointCloud(); buoy_vis_msg = sensor_msgs::msg::PointCloud2(); @@ -198,7 +198,7 @@ void NavigationTaskNode::main_task() { 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_4.size() != 1) { @@ -218,7 +218,7 @@ void NavigationTaskNode::main_task() { 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_3[0].pose_odom_frame.position.x - @@ -412,11 +412,11 @@ void NavigationTaskNode::main_task() { geometry_msgs::msg::Point waypoint_south_buoy; 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; + 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; + direction_vector_second_to_third_pair(1) * 3 + + direction_vector_downwards(1) * 1; waypoint_south_buoy.z = 0.0; send_waypoint(waypoint_south_buoy); set_desired_heading(waypoint_north_buoy, waypoint_south_buoy); @@ -632,7 +632,7 @@ void NavigationTaskNode::main_task() { 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; @@ -750,15 +750,12 @@ Eigen::Array NavigationTaskNode::predict_first_buoy_pair() { return predicted_positions; } -Eigen::Array -NavigationTaskNode::predict_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 - - odom_start_point_.x, - previous_waypoint_odom_frame_.y - - odom_start_point_.y; + direction_vector << previous_waypoint_odom_frame_.x - odom_start_point_.x, + previous_waypoint_odom_frame_.y - odom_start_point_.y; direction_vector.normalize(); Eigen::Array 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 32bf3c3..d0c5542 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 @@ -11,8 +11,8 @@ #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 "vortex_msgs/srv/waypoint.hpp" #include #include #include @@ -122,7 +122,8 @@ 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); + 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_; @@ -135,8 +136,7 @@ class NjordTaskBaseNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr landmarks_sub_; rclcpp::Client::SharedPtr waypoint_client_; - rclcpp::Client::SharedPtr - heading_client_; + rclcpp::Client::SharedPtr heading_client_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; 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 06ab1bc..3f592be 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,7 +38,7 @@ 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); @@ -463,7 +463,6 @@ void NjordTaskBaseNode::send_waypoint( RCLCPP_INFO(this->get_logger(), "Waypoint service failed"); } - previous_waypoint_odom_frame_ = waypoint; }