Skip to content

Commit 2f741fd

Browse files
idesign0idesign0
authored
Fix(macOS): Use Mach Precedence Policy for Soft Real-Time Thread Priority and std::chrono::duration Casting Error (#5679)
* Fix(macOS): Use Mach Precedence Policy for Soft Real-Time Thread Priority Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com> * precommit checks Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com> * Fix: Use explicit cast to microseconds for sleep in LoopRate Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com> * Use of THREAD_TIME_CONSTRAINT_POLICY in setSoftRealTimePriority() Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com> * Pre-Commit Changes Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com> * Pre-commits: ament_uncrustify fixes Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com> --------- Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com>
1 parent 4d29041 commit 2f741fd

File tree

179 files changed

+1014
-691
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

179 files changed

+1014
-691
lines changed

‎nav2_amcl/src/amcl_node.cpp‎

Lines changed: 18 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1010,32 +1010,32 @@ rcl_interfaces::msg::SetParametersResult AmclNode::validateParameterUpdatesCallb
10101010
(param_name != "laser_min_range" || param_name != "laser_max_range"))
10111011
{
10121012
RCLCPP_WARN(
1013-
get_logger(), "The value of parameter '%s' is incorrectly set to %f, "
1014-
"it should be >=0. Ignoring parameter update.",
1015-
param_name.c_str(), parameter.as_double());
1013+
get_logger(), "The value of parameter '%s' is incorrectly set to %f, "
1014+
"it should be >=0. Ignoring parameter update.",
1015+
param_name.c_str(), parameter.as_double());
10161016
result.successful = false;
10171017
}
10181018
} else if (param_type == ParameterType::PARAMETER_INTEGER) {
10191019
if (parameter.as_int() <= 0.0 && param_name == "resample_interval") {
10201020
RCLCPP_WARN(
1021-
get_logger(), "The value of resample_interval is incorrectly set, "
1022-
"it should be >0. Ignoring parameter update.");
1021+
get_logger(), "The value of resample_interval is incorrectly set, "
1022+
"it should be >0. Ignoring parameter update.");
10231023
result.successful = false;
10241024
} else if (parameter.as_int() < 0.0) {
10251025
RCLCPP_WARN(
1026-
get_logger(), "The value of parameter '%s' is incorrectly set to %ld, "
1027-
"it should be >=0. Ignoring parameter update.",
1028-
param_name.c_str(), parameter.as_int());
1026+
get_logger(), "The value of parameter '%s' is incorrectly set to %ld, "
1027+
"it should be >=0. Ignoring parameter update.",
1028+
param_name.c_str(), parameter.as_int());
10291029
result.successful = false;
10301030
} else if (param_name == "max_particles" && parameter.as_int() < min_particles_) {
10311031
RCLCPP_WARN(
1032-
get_logger(), "The value of max_particles is incorrectly set, "
1033-
"it should be larger than min_particles. Ignoring parameter update.");
1032+
get_logger(), "The value of max_particles is incorrectly set, "
1033+
"it should be larger than min_particles. Ignoring parameter update.");
10341034
result.successful = false;
10351035
} else if (param_name == "min_particles" && parameter.as_int() > max_particles_) {
10361036
RCLCPP_WARN(
1037-
get_logger(), "The value of min_particles is incorrectly set, "
1038-
"it should be smaller than max particles. Ignoring parameter update.");
1037+
get_logger(), "The value of min_particles is incorrectly set, "
1038+
"it should be smaller than max particles. Ignoring parameter update.");
10391039
result.successful = false;
10401040
}
10411041
}
@@ -1399,17 +1399,20 @@ AmclNode::initServices()
13991399
{
14001400
global_loc_srv_ = create_service<std_srvs::srv::Empty>(
14011401
"reinitialize_global_localization",
1402-
std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1,
1402+
std::bind(
1403+
&AmclNode::globalLocalizationCallback, this, std::placeholders::_1,
14031404
std::placeholders::_2, std::placeholders::_3));
14041405

14051406
initial_guess_srv_ = create_service<nav2_msgs::srv::SetInitialPose>(
14061407
"set_initial_pose",
1407-
std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2,
1408+
std::bind(
1409+
&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2,
14081410
std::placeholders::_3));
14091411

14101412
nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
14111413
"request_nomotion_update",
1412-
std::bind(&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2,
1414+
std::bind(
1415+
&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2,
14131416
std::placeholders::_3));
14141417
}
14151418

‎nav2_amcl/src/map/map_cspace.cpp‎

Lines changed: 14 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -132,7 +132,8 @@ void enqueue(
132132

133133
map->cells[map_index].occ_dist = distance * map->scale;
134134

135-
Q.emplace(CellData{map, static_cast<unsigned int>(i), static_cast<unsigned int>(j),
135+
Q.emplace(
136+
CellData{map, static_cast<unsigned int>(i), static_cast<unsigned int>(j),
136137
static_cast<unsigned int>(src_i), static_cast<unsigned int>(src_j),
137138
map->cells[map_index].occ_dist});
138139

@@ -179,27 +180,27 @@ void map_update_cspace(map_t * map, double max_occ_dist)
179180
CellData current_cell = Q.top();
180181
if (current_cell.i_ > 0) {
181182
enqueue(
182-
map, current_cell.i_ - 1, current_cell.j_,
183-
current_cell.src_i_, current_cell.src_j_,
184-
Q, cdm, marked);
183+
map, current_cell.i_ - 1, current_cell.j_,
184+
current_cell.src_i_, current_cell.src_j_,
185+
Q, cdm, marked);
185186
}
186187
if (current_cell.j_ > 0) {
187188
enqueue(
188-
map, current_cell.i_, current_cell.j_ - 1,
189-
current_cell.src_i_, current_cell.src_j_,
190-
Q, cdm, marked);
189+
map, current_cell.i_, current_cell.j_ - 1,
190+
current_cell.src_i_, current_cell.src_j_,
191+
Q, cdm, marked);
191192
}
192193
if (static_cast<int>(current_cell.i_) < map->size_x - 1) {
193194
enqueue(
194-
map, current_cell.i_ + 1, current_cell.j_,
195-
current_cell.src_i_, current_cell.src_j_,
196-
Q, cdm, marked);
195+
map, current_cell.i_ + 1, current_cell.j_,
196+
current_cell.src_i_, current_cell.src_j_,
197+
Q, cdm, marked);
197198
}
198199
if (static_cast<int>(current_cell.j_) < map->size_y - 1) {
199200
enqueue(
200-
map, current_cell.i_, current_cell.j_ + 1,
201-
current_cell.src_i_, current_cell.src_j_,
202-
Q, cdm, marked);
201+
map, current_cell.i_, current_cell.j_ + 1,
202+
current_cell.src_i_, current_cell.src_j_,
203+
Q, cdm, marked);
203204
}
204205

205206
Q.pop();

‎nav2_amcl/test/test_dynamic_parameters.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -140,7 +140,7 @@ TEST(WPTest, test_dynamic_parameters)
140140
EXPECT_EQ(amcl->get_parameter("min_particles").as_int(), 100);
141141
}
142142

143-
int main(int argc, char **argv)
143+
int main(int argc, char ** argv)
144144
{
145145
::testing::InitGoogleTest(&argc, argv);
146146

‎nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -93,12 +93,12 @@ class BehaviorTreeEngine
9393
*/
9494
std::string extractBehaviorTreeID(const std::string & file_or_id);
9595

96-
/**
97-
* @brief Function to create a BT from a BehaviorTree ID
98-
* @param tree_id BehaviorTree ID
99-
* @param blackboard Blackboard for BT
100-
* @return BT::Tree Created behavior tree
101-
*/
96+
/**
97+
* @brief Function to create a BT from a BehaviorTree ID
98+
* @param tree_id BehaviorTree ID
99+
* @param blackboard Blackboard for BT
100+
* @return BT::Tree Created behavior tree
101+
*/
102102
BT::Tree createTree(
103103
const std::string & tree_id,
104104
BT::Blackboard::Ptr blackboard);

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp‎

Lines changed: 20 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -77,9 +77,10 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
7777
};
7878

7979
if (node->has_parameter("error_code_names")) {
80-
throw std::runtime_error("parameter 'error_code_names' has been replaced by "
81-
" 'error_code_name_prefixes' and MUST be removed.\n"
82-
" Please review migration guide and update your configuration.");
80+
throw std::runtime_error(
81+
"parameter 'error_code_names' has been replaced by "
82+
" 'error_code_name_prefixes' and MUST be removed.\n"
83+
" Please review migration guide and update your configuration.");
8384
}
8485

8586
// Declare and get error code name prefixes parameter
@@ -100,8 +101,9 @@ BtActionServer<ActionT, NodeT>::BtActionServer(
100101
<< "Make sure these match your BT and there are not other sources of error codes you"
101102
<< "reported to your application");
102103
} else {
103-
RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:"
104-
<< error_code_name_prefixes_str);
104+
RCLCPP_INFO_STREAM(
105+
logger_, "Error_code parameters were set to:"
106+
<< error_code_name_prefixes_str);
105107
}
106108
}
107109

@@ -123,7 +125,8 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
123125
// Use suffix '_rclcpp_node' to keep parameter file consistency #1773
124126

125127
auto new_arguments = node->get_node_options().arguments();
126-
nav2::replaceOrAddArgument(new_arguments, "-r", "__node", std::string("__node:=") +
128+
nav2::replaceOrAddArgument(
129+
new_arguments, "-r", "__node", std::string("__node:=") +
127130
std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node");
128131
auto options = node->get_node_options();
129132
options = options.arguments(new_arguments);
@@ -271,7 +274,8 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
271274
continue;
272275
}
273276
if (registered_ids.count(id)) {
274-
RCLCPP_WARN(logger_, "Skipping conflicting BT file %s (duplicate ID %s)",
277+
RCLCPP_WARN(
278+
logger_, "Skipping conflicting BT file %s (duplicate ID %s)",
275279
entry.path().c_str(), id.c_str());
276280
continue;
277281
}
@@ -311,7 +315,8 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
311315
register_all_bt_files();
312316
}
313317
} catch (const std::exception & e) {
314-
setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
318+
setInternalError(
319+
ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
315320
"Exception registering behavior trees: " + std::string(e.what()));
316321
return false;
317322
}
@@ -327,10 +332,11 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
327332
blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
328333
blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
329334
blackboard->set<std::chrono::milliseconds>(
330-
"wait_for_service_timeout", wait_for_service_timeout_);
335+
"wait_for_service_timeout", wait_for_service_timeout_);
331336
}
332337
} catch (const std::exception & e) {
333-
setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
338+
setInternalError(
339+
ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
334340
std::string("Exception when creating BT tree from ID: ") + e.what());
335341
return false;
336342
}
@@ -405,7 +411,8 @@ void BtActionServer<ActionT, NodeT>::executeCallback()
405411

406412
case nav2_behavior_tree::BtStatus::FAILED:
407413
action_server_->terminate_current(result);
408-
RCLCPP_ERROR(logger_, "Goal failed error_code:%d error_msg:'%s'", result->error_code,
414+
RCLCPP_ERROR(
415+
logger_, "Goal failed error_code:%d error_msg:'%s'", result->error_code,
409416
result->error_msg.c_str());
410417
break;
411418

@@ -425,7 +432,8 @@ void BtActionServer<ActionT, NodeT>::setInternalError(
425432
{
426433
internal_error_code_ = error_code;
427434
internal_error_msg_ = error_msg;
428-
RCLCPP_ERROR(logger_, "Setting internal error error_code:%d, error_msg:%s",
435+
RCLCPP_ERROR(
436+
logger_, "Setting internal error error_code:%d, error_msg:%s",
429437
internal_error_code_, internal_error_msg_.c_str());
430438
}
431439

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -94,7 +94,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
9494
BT::InputPort<geometry_msgs::msg::PoseStamped>(
9595
"start",
9696
"Used as the planner start pose instead of the current robot pose, if use_start is"
97-
" not false (i.e. not provided or set to true)"),
97+
" not false (i.e. not provided or set to true)"),
9898
BT::InputPort<bool>(
9999
"use_start", "For using or not using (i.e. ignoring) the provided start pose"),
100100
BT::InputPort<std::string>(

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

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -103,7 +103,8 @@ class ComputeRouteAction : public BtActionNode<nav2_msgs::action::ComputeRoute>
103103
"use_poses", false, "Whether to use poses or IDs for start and goal"),
104104
BT::OutputPort<ActionResult::_route_type>(
105105
"route", "The route computed by ComputeRoute node"),
106-
BT::OutputPort<builtin_interfaces::msg::Duration>("planning_time",
106+
BT::OutputPort<builtin_interfaces::msg::Duration>(
107+
"planning_time",
107108
"Time taken to compute route"),
108109
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputeRoute node"),
109110
BT::OutputPort<ActionResult::_error_code_type>(

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

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -64,7 +64,8 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
6464

6565
return providedBasicPorts(
6666
{
67-
BT::InputPort<nav_msgs::msg::Goals>("input_goals",
67+
BT::InputPort<nav_msgs::msg::Goals>(
68+
"input_goals",
6869
"Original goals to remove from"),
6970
BT::InputPort<double>(
7071
"cost_threshold", 254.0,
@@ -73,11 +74,14 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
7374
BT::InputPort<bool>(
7475
"consider_unknown_as_obstacle", false,
7576
"Whether to consider unknown cost as obstacle"),
76-
BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
77+
BT::OutputPort<nav_msgs::msg::Goals>(
78+
"output_goals",
7779
"Goals with in-collision goals removed"),
78-
BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("input_waypoint_statuses",
80+
BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
81+
"input_waypoint_statuses",
7982
"Original waypoint_statuses to mark waypoint status from"),
80-
BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
83+
BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
84+
"output_waypoint_statuses",
8185
"Waypoint_statuses with in-collision waypoints marked")
8286
});
8387
}

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

Lines changed: 12 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -56,16 +56,20 @@ class RemovePassedGoals : public BT::ActionNodeBase
5656
BT::RegisterJsonDefinition<std::vector<nav2_msgs::msg::WaypointStatus>>();
5757

5858
return {
59-
BT::InputPort<nav_msgs::msg::Goals>("input_goals",
60-
"Original goals to remove viapoints from"),
61-
BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
62-
"Goals with passed viapoints removed"),
59+
BT::InputPort<nav_msgs::msg::Goals>(
60+
"input_goals",
61+
"Original goals to remove viapoints from"),
62+
BT::OutputPort<nav_msgs::msg::Goals>(
63+
"output_goals",
64+
"Goals with passed viapoints removed"),
6365
BT::InputPort<double>("radius", 0.5, "radius to goal for it to be considered for removal"),
6466
BT::InputPort<std::string>("robot_base_frame", "Robot base frame"),
65-
BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("input_waypoint_statuses",
66-
"Original waypoint_statuses to mark waypoint status from"),
67-
BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
68-
"Waypoint_statuses with passed waypoints marked")
67+
BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
68+
"input_waypoint_statuses",
69+
"Original waypoint_statuses to mark waypoint status from"),
70+
BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>(
71+
"output_waypoint_statuses",
72+
"Waypoint_statuses with passed waypoints marked")
6973
};
7074
}
7175

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -75,8 +75,8 @@ class IsPathValidCondition : public BT::ConditionNode
7575
BT::InputPort<std::chrono::milliseconds>("server_timeout"),
7676
BT::InputPort<unsigned int>("max_cost", 253, "Maximum cost of the path"),
7777
BT::InputPort<bool>(
78-
"consider_unknown_as_obstacle", false,
79-
"Whether to consider unknown cost as obstacle")
78+
"consider_unknown_as_obstacle", false,
79+
"Whether to consider unknown cost as obstacle")
8080
};
8181
}
8282

0 commit comments

Comments
 (0)