Migrate to Kilted (#94)

* update msg for Kilted

* [opennav_coverage_bt] migrate BT xml files

* migrate coverage_navigator

* rm error_msg from ComputeCoveragePath

* use newer rostooling docker images, run test for Kilted

* run linter on Kilted

* use base image

Author: adivardi

.github/workflows/lint.yml

@@ -7,7 +7,7 @@ jobs:
     name: ament_${{ matrix.linter }}
     runs-on: ubuntu-latest
     container:
-      image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-ros-base-latest
+      image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-kilted-ros-base:master
     strategy:
       fail-fast: false
       matrix:
@@ -17,7 +17,7 @@ jobs:
     - uses: ros-tooling/[email protected]
       with:
         linter: ${{ matrix.linter }}
-        distribution: jazzy
+        distribution: kilted
         package-name: |
             opennav_coverage
             opennav_coverage_bt

.github/workflows/test.yml

@@ -12,18 +12,18 @@ jobs:
       ROS_DISTRO: ${{ matrix.ros_distro }}
       RMW_IMPLEMENTATION: rmw_cyclonedds_cpp
     container:
-      image: rostooling/setup-ros-docker:ubuntu-noble-latest
+      image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-kilted-ros-base:master
     strategy:
       fail-fast: false
       matrix:
-          ros_distro: [jazzy]
+          ros_distro: [kilted]
     steps:
     - uses: actions/checkout@v2
     - name: Install Cyclone DDS
       run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y
     - name: Build and run tests
       id: action-ros-ci
-      uses: ros-tooling/action-ros-ci@v0.3
+      uses: ros-tooling/action-ros-ci@v0.4
       with:
         import-token: ${{ secrets.GITHUB_TOKEN }}
         target-ros2-distro: ${{ matrix.ros_distro }}

opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage.xml

@@ -19,7 +19,7 @@
          'polygons="{field_polygon}" polygons_frame_id="{polygon_frame_id}"' if set polygon via NavigateCompleteCoverage
          or file_field="{field_filepath}" if setting polygon file via NavigateCompleteCoverage -->
         <ComputeCoveragePath nav_path="{path}" polygons="{field_polygon}" polygons_frame_id="{polygon_frame_id}" error_code_id="{compute_coverage_error_code}"/>
-        <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
+        <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
       </Sequence>
     </RateController>
   </BehaviorTree>

opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage_nav_to_start.xml

@@ -24,11 +24,11 @@
 
         <!-- Go to start of path before navigating -->
         <GetPoseFromPath path="{path}" pose="{start_pose}" index="0" />
-        <ComputePathToPose goal="{start_pose}" path="{path_to_start}" planner_id="GridBased" error_code_id="{compute_path_error_code}"/>
-        <FollowPath path="{path_to_start}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
+        <ComputePathToPose goal="{start_pose}" path="{path_to_start}" planner_id="GridBased" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
+        <FollowPath path="{path_to_start}" controller_id="FollowPath" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
 
         <!-- Follow computed path -->
-        <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
+        <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
       </Sequence>
     </RateController>
   </BehaviorTree>

opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml

@@ -19,7 +19,7 @@
          'polygons="{field_polygon}" polygons_frame_id="{polygon_frame_id}"' if set polygon via NavigateCompleteCoverage
          or file_field="{field_filepath}" if setting polygon file via NavigateCompleteCoverage -->
         <ComputeCoveragePath nav_path="{path}" file_field="{field_filepath}" error_code_id="{compute_coverage_error_code}"/>
-        <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
+        <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
       </Sequence>
     </RateController>
   </BehaviorTree>

opennav_coverage_msgs/action/NavigateCompleteCoverage.action

@@ -14,8 +14,13 @@ string behavior_tree
 # Error codes
 # Note: The expected priority order of the errors should match the message order
 uint16 NONE=0
+uint16 UNKNOWN=800
+uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=801
+uint16 TF_ERROR=802
 
 uint16 error_code
+string error_msg
+
 ---
 #feedback definition
 

opennav_coverage_navigator/include/opennav_coverage_navigator/coverage_navigator.hpp

@@ -111,8 +111,9 @@ class CoverageNavigator
   /**
    * @brief Goal pose initialization on the blackboard
    * @param goal Action template's goal message to process
+   * @return bool if goal was initialized successfully to be processed
    */
-  void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal);
+  bool initializeGoalPose(ActionT::Goal::ConstSharedPtr goal);
 
   rclcpp::Time start_time_;
   std::string path_blackboard_id_, field_blackboard_id_, polygon_blackboard_id_;

opennav_coverage_navigator/src/coverage_navigator.cpp

@@ -51,6 +51,20 @@ CoverageNavigator::configure(
 
   // Odometry smoother object for getting current speed
   odom_smoother_ = odom_smoother;
+
+  // Groot monitoring
+  if (!node->has_parameter(getName() + ".enable_groot_monitoring")) {
+    node->declare_parameter(getName() + ".enable_groot_monitoring", false);
+  }
+
+  if (!node->has_parameter(getName() + ".groot_server_port")) {
+    node->declare_parameter(getName() + ".groot_server_port", 1667);
+  }
+
+  bt_action_server_->setGrootMonitoring(
+      node->get_parameter(getName() + ".enable_groot_monitoring").as_bool(),
+      node->get_parameter(getName() + ".groot_server_port").as_int());
+
   return true;
 }
 
@@ -90,11 +104,12 @@ CoverageNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
     RCLCPP_ERROR(
       logger_, "BT file not found: %s. Navigation canceled.",
       bt_xml_filename.c_str());
+    bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+      std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled.");
     return false;
   }
 
-  initializeGoalPose(goal);
-  return true;
+  return initializeGoalPose(goal);
 }
 
 void
@@ -185,7 +200,13 @@ CoverageNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
     // if pending goal requests the same BT as the current goal, accept the pending goal
     // if pending goal has an empty behavior_tree field, it requests the default BT file
     // accept the pending goal if the current goal is running the default BT file
-    initializeGoalPose(bt_action_server_->acceptPendingGoal());
+    if (!initializeGoalPose(bt_action_server_->acceptPendingGoal())) {
+      RCLCPP_WARN(
+        logger_,
+        "Preemption request was rejected since the goal could not be "
+        "initialized. For now, continuing to track the last goal until completion.");
+      bt_action_server_->terminatePendingGoal();
+    }
   } else {
     RCLCPP_WARN(
       logger_,
@@ -198,14 +219,19 @@ CoverageNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
   }
 }
 
-void
+bool
 CoverageNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  nav2_util::getCurrentPose(
-    current_pose, *feedback_utils_.tf,
-    feedback_utils_.global_frame, feedback_utils_.robot_frame,
-    feedback_utils_.transform_tolerance);
+  if (!nav2_util::getCurrentPose(
+      current_pose, *feedback_utils_.tf,
+      feedback_utils_.global_frame, feedback_utils_.robot_frame,
+      feedback_utils_.transform_tolerance))
+  {
+    bt_action_server_->setInternalError(ActionT::Result::TF_ERROR,
+      "Initial robot pose is not available.");
+    return false;
+  }
 
   if (goal->field_filepath.size() == 0) {
     RCLCPP_INFO(
@@ -227,6 +253,8 @@ CoverageNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
   blackboard->set<std::vector<geometry_msgs::msg::Polygon>>(
     polygon_blackboard_id_, goal->polygons);
   blackboard->set<std::string>(polygon_frame_blackboard_id_, goal->frame_id);
+
+  return true;
 }
 
 }  // namespace opennav_coverage_navigator