Fix(macOS): Use Mach Precedence Policy for Soft Real-Time Thread Priority and std::chrono::duration Casting Error

nav2_amcl/src/amcl_node.cpp

@@ -1008,32 +1008,32 @@ rcl_interfaces::msg::SetParametersResult AmclNode::validateParameterUpdatesCallb
         (param_name != "laser_min_range" || param_name != "laser_max_range"))
       {
         RCLCPP_WARN(
-        get_logger(), "The value of parameter '%s' is incorrectly set to %f, "
-        "it should be >=0. Ignoring parameter update.",
-        param_name.c_str(), parameter.as_double());
+          get_logger(), "The value of parameter '%s' is incorrectly set to %f, "
+          "it should be >=0. Ignoring parameter update.",
+          param_name.c_str(), parameter.as_double());
         result.successful = false;
       }
     } else if (param_type == ParameterType::PARAMETER_INTEGER) {
       if (parameter.as_int() <= 0.0 && param_name == "resample_interval") {
         RCLCPP_WARN(
-        get_logger(), "The value of resample_interval is incorrectly set, "
-        "it should be >0. Ignoring parameter update.");
+          get_logger(), "The value of resample_interval is incorrectly set, "
+          "it should be >0. Ignoring parameter update.");
         result.successful = false;
       } else if (parameter.as_int() < 0.0) {
         RCLCPP_WARN(
-        get_logger(), "The value of parameter '%s' is incorrectly set to %ld, "
-        "it should be >=0. Ignoring parameter update.",
-        param_name.c_str(), parameter.as_int());
+          get_logger(), "The value of parameter '%s' is incorrectly set to %ld, "
+          "it should be >=0. Ignoring parameter update.",
+          param_name.c_str(), parameter.as_int());
         result.successful = false;
       } else if (param_name == "max_particles" && parameter.as_int() < min_particles_) {
         RCLCPP_WARN(
-        get_logger(), "The value of max_particles is incorrectly set, "
-        "it should be larger than min_particles. Ignoring parameter update.");
+          get_logger(), "The value of max_particles is incorrectly set, "
+          "it should be larger than min_particles. Ignoring parameter update.");
         result.successful = false;
       } else if (param_name == "min_particles" && parameter.as_int() > max_particles_) {
         RCLCPP_WARN(
-        get_logger(), "The value of min_particles is incorrectly set, "
-        "it should be smaller than max particles. Ignoring parameter update.");
+          get_logger(), "The value of min_particles is incorrectly set, "
+          "it should be smaller than max particles. Ignoring parameter update.");
         result.successful = false;
       }
     }
@@ -1397,17 +1397,20 @@ AmclNode::initServices()
 {
   global_loc_srv_ = create_service<std_srvs::srv::Empty>(
     "reinitialize_global_localization",
-    std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1,
+    std::bind(
+      &AmclNode::globalLocalizationCallback, this, std::placeholders::_1,
       std::placeholders::_2, std::placeholders::_3));
 
   initial_guess_srv_ = create_service<nav2_msgs::srv::SetInitialPose>(
     "set_initial_pose",
-    std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2,
+    std::bind(
+      &AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 
   nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
     "request_nomotion_update",
-    std::bind(&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2,
+    std::bind(
+      &AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 }
 

nav2_amcl/src/map/map_cspace.cpp

@@ -132,7 +132,8 @@ void enqueue(
 
   map->cells[map_index].occ_dist = distance * map->scale;
 
-  Q.emplace(CellData{map, static_cast<unsigned int>(i), static_cast<unsigned int>(j),
+  Q.emplace(
+    CellData{map, static_cast<unsigned int>(i), static_cast<unsigned int>(j),
       static_cast<unsigned int>(src_i), static_cast<unsigned int>(src_j),
       map->cells[map_index].occ_dist});
 
@@ -179,27 +180,27 @@ void map_update_cspace(map_t * map, double max_occ_dist)
     CellData current_cell = Q.top();
     if (current_cell.i_ > 0) {
       enqueue(
-          map, current_cell.i_ - 1, current_cell.j_,
-          current_cell.src_i_, current_cell.src_j_,
-          Q, cdm, marked);
+        map, current_cell.i_ - 1, current_cell.j_,
+        current_cell.src_i_, current_cell.src_j_,
+        Q, cdm, marked);
     }
     if (current_cell.j_ > 0) {
       enqueue(
-          map, current_cell.i_, current_cell.j_ - 1,
-          current_cell.src_i_, current_cell.src_j_,
-          Q, cdm, marked);
+        map, current_cell.i_, current_cell.j_ - 1,
+        current_cell.src_i_, current_cell.src_j_,
+        Q, cdm, marked);
     }
     if (static_cast<int>(current_cell.i_) < map->size_x - 1) {
       enqueue(
-          map, current_cell.i_ + 1, current_cell.j_,
-          current_cell.src_i_, current_cell.src_j_,
-          Q, cdm, marked);
+        map, current_cell.i_ + 1, current_cell.j_,
+        current_cell.src_i_, current_cell.src_j_,
+        Q, cdm, marked);
     }
     if (static_cast<int>(current_cell.j_) < map->size_y - 1) {
       enqueue(
-          map, current_cell.i_, current_cell.j_ + 1,
-          current_cell.src_i_, current_cell.src_j_,
-          Q, cdm, marked);
+        map, current_cell.i_, current_cell.j_ + 1,
+        current_cell.src_i_, current_cell.src_j_,
+        Q, cdm, marked);
     }
 
     Q.pop();

nav2_amcl/test/test_dynamic_parameters.cpp

@@ -140,7 +140,7 @@ TEST(WPTest, test_dynamic_parameters)
   EXPECT_EQ(amcl->get_parameter("min_particles").as_int(), 100);
 }
 
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
 

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -93,12 +93,12 @@ class BehaviorTreeEngine
    */
   std::string extractBehaviorTreeID(const std::string & file_or_id);
 
-    /**
-   * @brief Function to create a BT from a BehaviorTree ID
-   * @param tree_id BehaviorTree ID
-   * @param blackboard Blackboard for BT
-   * @return BT::Tree Created behavior tree
-    */
+  /**
+ * @brief Function to create a BT from a BehaviorTree ID
+ * @param tree_id BehaviorTree ID
+ * @param blackboard Blackboard for BT
+ * @return BT::Tree Created behavior tree
+  */
   BT::Tree createTree(
     const std::string & tree_id,
     BT::Blackboard::Ptr blackboard);

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -77,9 +77,10 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
   };
 
   if (node->has_parameter("error_code_names")) {
-    throw std::runtime_error("parameter 'error_code_names' has been replaced by "
-      " 'error_code_name_prefixes' and MUST be removed.\n"
-      " Please review migration guide and update your configuration.");
+    throw std::runtime_error(
+            "parameter 'error_code_names' has been replaced by "
+            " 'error_code_name_prefixes' and MUST be removed.\n"
+            " Please review migration guide and update your configuration.");
   }
 
   // Declare and get error code name prefixes parameter
@@ -100,8 +101,9 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
         << "Make sure these match your BT and there are not other sources of error codes you"
         << "reported to your application");
   } else {
-    RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:"
-      << error_code_name_prefixes_str);
+    RCLCPP_INFO_STREAM(
+      logger_, "Error_code parameters were set to:"
+        << error_code_name_prefixes_str);
   }
 }
 
@@ -123,7 +125,8 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
   // Use suffix '_rclcpp_node' to keep parameter file consistency #1773
 
   auto new_arguments = node->get_node_options().arguments();
-  nav2::replaceOrAddArgument(new_arguments, "-r", "__node", std::string("__node:=") +
+  nav2::replaceOrAddArgument(
+    new_arguments, "-r", "__node", std::string("__node:=") +
     std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node");
   auto options = node->get_node_options();
   options = options.arguments(new_arguments);
@@ -271,7 +274,8 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
             continue;
           }
           if (registered_ids.count(id)) {
-            RCLCPP_WARN(logger_, "Skipping conflicting BT file %s (duplicate ID %s)",
+            RCLCPP_WARN(
+              logger_, "Skipping conflicting BT file %s (duplicate ID %s)",
               entry.path().c_str(), id.c_str());
             continue;
           }
@@ -311,7 +315,8 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
       register_all_bt_files();
     }
   } catch (const std::exception & e) {
-    setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+    setInternalError(
+      ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
       "Exception registering behavior trees: " + std::string(e.what()));
     return false;
   }
@@ -327,10 +332,11 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
       blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
       blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
       blackboard->set<std::chrono::milliseconds>(
-          "wait_for_service_timeout", wait_for_service_timeout_);
+        "wait_for_service_timeout", wait_for_service_timeout_);
     }
   } catch (const std::exception & e) {
-    setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
+    setInternalError(
+      ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
       std::string("Exception when creating BT tree from ID: ") + e.what());
     return false;
   }
@@ -405,7 +411,8 @@ void BtActionServer<ActionT, NodeT>::executeCallback()
 
     case nav2_behavior_tree::BtStatus::FAILED:
       action_server_->terminate_current(result);
-      RCLCPP_ERROR(logger_, "Goal failed error_code:%d error_msg:'%s'", result->error_code,
+      RCLCPP_ERROR(
+        logger_, "Goal failed error_code:%d error_msg:'%s'", result->error_code,
         result->error_msg.c_str());
       break;
 
@@ -425,7 +432,8 @@ void BtActionServer<ActionT, NodeT>::setInternalError(
 {
   internal_error_code_ = error_code;
   internal_error_msg_ = error_msg;
-  RCLCPP_ERROR(logger_, "Setting internal error error_code:%d, error_msg:%s",
+  RCLCPP_ERROR(
+    logger_, "Setting internal error error_code:%d, error_msg:%s",
     internal_error_code_, internal_error_msg_.c_str());
 }
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

@@ -94,7 +94,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
         BT::InputPort<geometry_msgs::msg::PoseStamped>(
           "start",
           "Used as the planner start pose instead of the current robot pose, if use_start is"
-                   " not false (i.e. not provided or set to true)"),
+          " not false (i.e. not provided or set to true)"),
         BT::InputPort<bool>(
           "use_start", "For using or not using (i.e. ignoring) the provided start pose"),
         BT::InputPort<std::string>(

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp

@@ -103,7 +103,8 @@ class ComputeRouteAction : public BtActionNode<nav2_msgs::action::ComputeRoute>
           "use_poses", false, "Whether to use poses or IDs for start and goal"),
         BT::OutputPort<ActionResult::_route_type>(
           "route", "The route computed by ComputeRoute node"),
-        BT::OutputPort<builtin_interfaces::msg::Duration>("planning_time",
+        BT::OutputPort<builtin_interfaces::msg::Duration>(
+          "planning_time",
           "Time taken to compute route"),
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputeRoute node"),
         BT::OutputPort<ActionResult::_error_code_type>(

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

@@ -64,7 +64,8 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
 
     return providedBasicPorts(
       {
-        BT::InputPort<nav_msgs::msg::Goals>("input_goals",
+        BT::InputPort<nav_msgs::msg::Goals>(
+          "input_goals",
           "Original goals to remove from"),
         BT::InputPort<double>(
           "cost_threshold", 254.0,
@@ -73,11 +74,14 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
         BT::InputPort<bool>(
           "consider_unknown_as_obstacle", false,
           "Whether to consider unknown cost as obstacle"),
-        BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
+        BT::OutputPort<nav_msgs::msg::Goals>(
+          "output_goals",
           "Goals with in-collision goals removed"),
-        BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("input_waypoint_statuses",
+        BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
+          "input_waypoint_statuses",
           "Original waypoint_statuses to mark waypoint status from"),
-        BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
+        BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
+          "output_waypoint_statuses",
           "Waypoint_statuses with in-collision waypoints marked")
       });
   }

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

@@ -56,16 +56,20 @@ class RemovePassedGoals : public BT::ActionNodeBase
     BT::RegisterJsonDefinition<std::vector<nav2_msgs::msg::WaypointStatus>>();
 
     return {
-      BT::InputPort<nav_msgs::msg::Goals>("input_goals",
-          "Original goals to remove viapoints from"),
-      BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
-          "Goals with passed viapoints removed"),
+      BT::InputPort<nav_msgs::msg::Goals>(
+        "input_goals",
+        "Original goals to remove viapoints from"),
+      BT::OutputPort<nav_msgs::msg::Goals>(
+        "output_goals",
+        "Goals with passed viapoints removed"),
       BT::InputPort<double>("radius", 0.5, "radius to goal for it to be considered for removal"),
       BT::InputPort<std::string>("robot_base_frame", "Robot base frame"),
-      BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("input_waypoint_statuses",
-          "Original waypoint_statuses to mark waypoint status from"),
-      BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
-          "Waypoint_statuses with passed waypoints marked")
+      BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
+        "input_waypoint_statuses",
+        "Original waypoint_statuses to mark waypoint status from"),
+      BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
+        "output_waypoint_statuses",
+        "Waypoint_statuses with passed waypoints marked")
     };
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

@@ -75,8 +75,8 @@ class IsPathValidCondition : public BT::ConditionNode
       BT::InputPort<std::chrono::milliseconds>("server_timeout"),
       BT::InputPort<unsigned int>("max_cost", 253, "Maximum cost of the path"),
       BT::InputPort<bool>(
-          "consider_unknown_as_obstacle", false,
-          "Whether to consider unknown cost as obstacle")
+        "consider_unknown_as_obstacle", false,
+        "Whether to consider unknown cost as obstacle")
     };
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp

@@ -74,12 +74,12 @@ class IsPoseOccupiedCondition : public BT::ConditionNode
       BT::InputPort<geometry_msgs::msg::PoseStamped>("pose", "Pose to check if occupied"),
       BT::InputPort<std::string>("service_name", "global_costmap/get_cost_global_costmap"),
       BT::InputPort<double>(
-          "cost_threshold", 254.0,
-          "Cost threshold for considering a pose occupied"),
+        "cost_threshold", 254.0,
+        "Cost threshold for considering a pose occupied"),
       BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
       BT::InputPort<bool>(
-          "consider_unknown_as_obstacle", false,
-          "Whether to consider unknown cost as obstacle"),
+        "consider_unknown_as_obstacle", false,
+        "Whether to consider unknown cost as obstacle"),
       BT::InputPort<std::chrono::milliseconds>("server_timeout"),
     };
   }

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp

@@ -71,10 +71,12 @@ class IsStoppedCondition : public BT::ConditionNode
     BT::RegisterJsonDefinition<std::chrono::milliseconds>();
 
     return {
-      BT::InputPort<double>("velocity_threshold", 0.01,
-          "Velocity threshold below which robot is considered stopped"),
-      BT::InputPort<std::chrono::milliseconds>("duration_stopped", 1000ms,
-          "Duration (ms) the velocity must remain below the threshold"),
+      BT::InputPort<double>(
+        "velocity_threshold", 0.01,
+        "Velocity threshold below which robot is considered stopped"),
+      BT::InputPort<std::chrono::milliseconds>(
+        "duration_stopped", 1000ms,
+        "Duration (ms) the velocity must remain below the threshold"),
     };
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp

@@ -64,10 +64,12 @@ class GoalUpdater : public BT::DecoratorNode
     return {
       BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Original Goal"),
       BT::InputPort<nav_msgs::msg::Goals>("input_goals", "Original Goals"),
-      BT::OutputPort<geometry_msgs::msg::PoseStamped>("output_goal",
-          "Received Goal by subscription"),
-      BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
-          "Received Goals by subscription")
+      BT::OutputPort<geometry_msgs::msg::PoseStamped>(
+        "output_goal",
+        "Received Goal by subscription"),
+      BT::OutputPort<nav_msgs::msg::Goals>(
+        "output_goals",
+        "Received Goals by subscription")
     };
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp

@@ -61,7 +61,7 @@ class LoopRate
     auto time_to_sleep = next_interval - now;
     std::chrono::nanoseconds time_to_sleep_ns(time_to_sleep.nanoseconds());
     // Sleep (can get interrupted by emitWakeUpSignal())
-    tree_->sleep(time_to_sleep_ns);
+    tree_->sleep(std::chrono::duration_cast<std::chrono::microseconds>(time_to_sleep_ns));
     return true;
   }
 

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -41,9 +41,9 @@ void ComputePathToPoseAction::on_tick()
       // in case we don't have a "start" pose
       goal_.use_start = false;
       RCLCPP_ERROR(
-          node_->get_logger(),
-          "use_start is set to true but no start pose was provided, falling back to default "
-          "behavior, i.e. using the current robot pose");
+        node_->get_logger(),
+        "use_start is set to true but no start pose was provided, falling back to default "
+        "behavior, i.e. using the current robot pose");
     }
   } else {
     // else if "use_start" is not provided, but "start" is, then use it in order to not change