Concurrency setup change

mission/njord_tasks/collision_avoidance_task/include/collision_avoidance_task/collision_avoidance_task_ros.hpp

@@ -18,7 +18,6 @@ class CollisionAvoidanceTaskNode : public NjordTaskBaseNode {
   void main_task();
 
 private:
-  geometry_msgs::msg::Point previous_waypoint_odom_frame_;
 };
 
 } // namespace collision_avoidance_task

mission/njord_tasks/collision_avoidance_task/params/collision_avoidance_task_params.yaml

@@ -16,5 +16,5 @@ collision_avoidance_task_node:
 
     map_origin_topic: "/map/origin"
     odom_topic: "/seapath/odom/ned"
-    landmark_pose_topic: "landmarks_out"
+    landmark_topic: "landmarks_out"
 

mission/njord_tasks/collision_avoidance_task/src/collision_avoidance_task_ros.cpp

@@ -18,14 +18,14 @@ void CollisionAvoidanceTaskNode::main_task() {
 
   while (true) {
     rclcpp::sleep_for(std::chrono::milliseconds(100));
-    std::unique_lock<std::mutex> setup_lock(setup_mutex_);
+    std::unique_lock<std::mutex> setup_lock(navigation_mutex_);
     if (!(this->get_parameter("map_origin_set").as_bool())) {
       RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms");
       setup_lock.unlock();
       continue;
     }
     if (!(this->get_parameter("gps_frame_coords_set").as_bool())) {
-      setup_map_odom_tf_and_subs();
+      get_map_odom_tf();
       set_gps_odom_points();
       setup_lock.unlock();
       break;

mission/njord_tasks/docking_task/include/docking_task/docking_task_ros.hpp

@@ -93,8 +93,6 @@ class DockingTaskNode : public NjordTaskBaseNode {
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr grid_sub_;
 
   nav_msgs::msg::OccupancyGrid::SharedPtr grid_msg_;
-
-  geometry_msgs::msg::Point previous_waypoint_odom_frame_;
 };
 
 } // namespace docking_task

mission/njord_tasks/docking_task/params/docking_task_params.yaml

@@ -28,5 +28,5 @@ docking_task_node:
     map_origin_topic: "/map/origin"
     grid_topic: "grid"
     odom_topic: "/seapath/odom/ned"
-    landmark_pose_topic: "landmarks_out"
+    landmark_topic: "landmarks_out"
 

mission/njord_tasks/docking_task/src/docking_task_ros.cpp

@@ -29,15 +29,15 @@ void DockingTaskNode::main_task() {
 
   while (true) {
     rclcpp::sleep_for(std::chrono::milliseconds(100));
-    std::unique_lock<std::mutex> setup_lock(setup_mutex_);
+    std::unique_lock<std::mutex> setup_lock(navigation_mutex_);
     if (!(this->get_parameter("map_origin_set").as_bool())) {
       RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms");
       setup_lock.unlock();
       continue;
     }
     if (!(this->get_parameter("gps_frame_coords_set").as_bool())) {
       set_gps_odom_points();
-      setup_map_odom_tf_and_subs();
+      get_map_odom_tf();
       setup_lock.unlock();
       break;
     }

mission/njord_tasks/maneuvering_task/include/maneuvering_task/maneuvering_task_ros.hpp

@@ -17,8 +17,10 @@ class ManeuveringTaskNode : public NjordTaskBaseNode {
    */
   void main_task();
 
+  Eigen::Array<double, 2, 2> predict_first_buoy_pair();
+
+
 private:
-  geometry_msgs::msg::Point previous_waypoint_odom_frame_;
 };
 
 } // namespace maneuvering_task

mission/njord_tasks/maneuvering_task/params/maneuvering_task_params.yaml

@@ -16,5 +16,5 @@ maneuvering_task_node:
 
     map_origin_topic: "/map/origin"
     odom_topic: "/seapath/odom/ned"
-    landmark_pose_topic: "landmarks_out"
+    landmark_topic: "landmarks_out"
 

mission/njord_tasks/maneuvering_task/src/maneuvering_task_ros.cpp

@@ -17,20 +17,58 @@ void ManeuveringTaskNode::main_task() {
 
   while (true) {
     rclcpp::sleep_for(std::chrono::milliseconds(100));
-    std::unique_lock<std::mutex> setup_lock(setup_mutex_);
+    std::unique_lock<std::mutex> setup_lock(navigation_mutex_);
     if (!(this->get_parameter("map_origin_set").as_bool())) {
       RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms");
       setup_lock.unlock();
       continue;
     }
     if (!(this->get_parameter("gps_frame_coords_set").as_bool())) {
       set_gps_odom_points();
-      setup_map_odom_tf_and_subs();
+      get_map_odom_tf();
       setup_lock.unlock();
       break;
     }
     setup_lock.unlock();
   }
 }
 
+Eigen::Array<double, 2, 2> ManeuveringTaskNode::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;
+  buoy_0_base_link_frame.header.frame_id = "base_link";
+  buoy_1_base_link_frame.header.frame_id = "base_link";
+
+  double distance_to_first_buoy_pair =
+      this->get_parameter("distance_to_first_buoy_pair").as_double();
+
+  buoy_0_base_link_frame.pose.position.x = distance_to_first_buoy_pair;
+  buoy_0_base_link_frame.pose.position.y = -2.5;
+  buoy_0_base_link_frame.pose.position.z = 0.0;
+  buoy_1_base_link_frame.pose.position.x = distance_to_first_buoy_pair;
+  buoy_1_base_link_frame.pose.position.y = 2.5;
+  buoy_1_base_link_frame.pose.position.z = 0.0;
+
+  geometry_msgs::msg::PoseStamped buoy_0_odom_frame;
+  geometry_msgs::msg::PoseStamped buoy_1_odom_frame;
+
+  try {
+    auto transform = tf_buffer_->lookupTransform(
+        "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0));
+    tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform);
+    tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform);
+  } catch (tf2::TransformException &ex) {
+    RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
+  }
+
+  Eigen::Array<double, 2, 2> predicted_positions;
+  predicted_positions(0, 0) = buoy_0_odom_frame.pose.position.x;
+  predicted_positions(1, 0) = buoy_0_odom_frame.pose.position.y;
+  predicted_positions(0, 1) = buoy_1_odom_frame.pose.position.x;
+  predicted_positions(1, 1) = buoy_1_odom_frame.pose.position.y;
+
+  return predicted_positions;
+}
+
 } // namespace maneuvering_task

mission/njord_tasks/navigation_task/params/navigation_task_params.yaml

@@ -14,7 +14,7 @@ navigation_task_node:
     gps_frame_coords_set: false
     map_origin_topic: "/map/origin"
     odom_topic: "/seapath/odom/ned"
-    landmark_pose_topic: "landmarks_out"
+    landmark_topic: "landmarks_out"
     assignment_confidence: 10  # Number of consequtive identical assignments from auction algorithm before we consider the assignment as correct
 
     # Task specific parameters

mission/njord_tasks/navigation_task/src/navigation_task_ros.cpp

@@ -11,28 +11,9 @@ NavigationTaskNode::NavigationTaskNode(const rclcpp::NodeOptions &options)
 }
 
 void NavigationTaskNode::main_task() {
-  // Sleep for 3 seconds to allow system to initialize and tracks to be aquired
-  RCLCPP_INFO(
-      this->get_logger(),
-      "Waiting 3 seconds for system to initialize before starting main task");
-  rclcpp::sleep_for(std::chrono::seconds(3));
-
-  while (true) {
-    rclcpp::sleep_for(std::chrono::milliseconds(100));
-    std::unique_lock<std::mutex> setup_lock(setup_mutex_);
-    if (!(this->get_parameter("map_origin_set").as_bool())) {
-      RCLCPP_INFO(this->get_logger(), "Map origin not set, sleeping for 100ms");
-      setup_lock.unlock();
-      continue;
-    }
-    if (!(this->get_parameter("gps_frame_coords_set").as_bool())) {
-      setup_map_odom_tf_and_subs();
-      set_gps_odom_points();
-      setup_lock.unlock();
-      break;
-    }
-    setup_lock.unlock();
-  }
+
+  navigation_ready();
+  RCLCPP_INFO(this->get_logger(), "Navigation task started");
   // First pair of buoys
   Eigen::Array22d predicted_first_buoy_pair = predict_first_buoy_pair();
   std::vector<LandmarkPoseID> buoy_landmarks_0_to_1 =
@@ -311,13 +292,17 @@ Eigen::Array<double, 2, 2> NavigationTaskNode::predict_first_buoy_pair() {
   geometry_msgs::msg::PoseStamped buoy_0_odom_frame;
   geometry_msgs::msg::PoseStamped buoy_1_odom_frame;
 
-  try {
-    auto transform = tf_buffer_->lookupTransform(
-        "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0));
-    tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform);
-    tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform);
-  } catch (tf2::TransformException &ex) {
-    RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
+  bool transform_success = false;
+  while (!transform_success){
+    try {
+      auto transform = tf_buffer_->lookupTransform(
+          "odom", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0));
+      tf2::doTransform(buoy_0_base_link_frame, buoy_0_base_link_frame, transform);
+      tf2::doTransform(buoy_1_base_link_frame, buoy_1_base_link_frame, transform);
+      transform_success = true;
+    } catch (tf2::TransformException &ex) {
+      RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
+    }
   }
 
   Eigen::Array<double, 2, 2> predicted_positions;

mission/njord_tasks/njord_task_base/include/njord_task_base/njord_task_base_ros.hpp

@@ -37,12 +37,21 @@ class NjordTaskBaseNode : public rclcpp::Node {
   void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
   void landmark_callback(const vortex_msgs::msg::LandmarkArray::SharedPtr msg);
 
+  /**
+   * @brief Runs until the navigation systeam required for task completion is ready.
+   * This includes setting the map origin, getting the map to odom tf, and
+   * and getting the odom coordinates of the start and end GPS points.
+   */
+  void navigation_ready();
+
   /**
    * @brief Spins until the map to odom tf is available.
    * Stores the tf in the member variable map_odom_tf_
    * Then initializes the odom and landmark subscribers
    */
-  void setup_map_odom_tf_and_subs();
+  void get_map_odom_tf();
+
+  void initialize_subscribers();
 
   /**
    * @brief Set the gps_start_odom_frame_ and gps_end_odom_frame_ member
@@ -125,7 +134,7 @@ class NjordTaskBaseNode : public rclcpp::Node {
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   geometry_msgs::msg::TransformStamped map_odom_tf_;
 
-  mutable std::mutex setup_mutex_;
+  mutable std::mutex navigation_mutex_;
   mutable std::mutex odom_mutex_;
   mutable std::mutex landmark_mutex_;
 
@@ -136,6 +145,7 @@ class NjordTaskBaseNode : public rclcpp::Node {
   std::shared_ptr<vortex_msgs::msg::LandmarkArray> landmarks_msg_;
   bool new_odom_msg_ = false;
   bool new_landmark_msg_ = false;
+  bool navigation_ready_ = false;
 
   geometry_msgs::msg::Point previous_waypoint_odom_frame_;
 };