From 377a693d9ee0fccec315aa612cb96f593e6e2531 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 10 Aug 2026 11:21:33 +0200 Subject: [PATCH] Use of get_safe MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_mpc_controller/MPCController.cpp | 12 ++++++------ .../src/easynav_mppi_controller/MPPIController.cpp | 6 +++--- .../RegulatedPurePursuitController.cpp | 10 +++++----- .../tests/regulated_pp_controller_tests.cpp | 6 +++--- .../easynav_serest_controller/SerestController.cpp | 14 +++++++------- .../easynav_simple_controller/SimpleController.cpp | 6 +++--- .../src/easynav_vff_controller/VffController.cpp | 4 ++-- .../easynav_bonxai_maps_manager/CMakeLists.txt | 3 +++ .../src/easynav_costmap_planner/CostmapPlanner.cpp | 2 +- .../src/easynav_navmap_planner/AStarPlanner.cpp | 2 +- .../src/easynav_simple_planner/SimplePlanner.cpp | 2 +- 11 files changed, 35 insertions(+), 32 deletions(-) diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index ac825d4c..e2a32068 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -132,7 +132,7 @@ MPCController::update_rt(NavState & nav_state) { // If navigation is IDLE, force zero velocity if (nav_state.has("navigation_state")) { - const auto nav_state_val = nav_state.get("navigation_state"); + const auto nav_state_val = nav_state.get_safe("navigation_state"); if (nav_state_val == easynav::GoalManager::State::IDLE) { cmd_vel_.header.stamp = get_node()->now(); cmd_vel_.twist.linear.x = 0.0; @@ -151,7 +151,7 @@ MPCController::update_rt(NavState & nav_state) return; } - nav_msgs::msg::Path path = nav_state.get("path"); + nav_msgs::msg::Path path = nav_state.get_safe("path"); if (path.poses.empty()) { // If the path is empty, stop the robot cmd_vel_.header.frame_id = path.header.frame_id; @@ -165,7 +165,7 @@ MPCController::update_rt(NavState & nav_state) // Build a local path that: // 1) keeps only the segment that brings the robot closer to the goal, and // 2) prepends a short straight segment from the robot pose to that segment. - const auto & robot_pose_msg = nav_state.get("robot_pose"); + const auto & robot_pose_msg = nav_state.get_safe("robot_pose"); const auto & robot_p = robot_pose_msg.pose.pose.position; // Goal is the last point of the planner path @@ -226,7 +226,7 @@ MPCController::update_rt(NavState & nav_state) cloud_out.header.stamp = get_node()->now(); detection_pub_->publish(cloud_out); - const auto pose = nav_state.get("robot_pose").pose.pose; + const auto pose = nav_state.get_safe("robot_pose").pose.pose; double roll_, pitch_, yaw_; tf2::Quaternion q( pose.orientation.x, @@ -296,10 +296,10 @@ MPCController::update_rt(NavState & nav_state) double yaw_tol = fallback_goal_yaw_tol_; if (nav_state.has("goal_tolerance.position")) { - pos_tol = nav_state.get("goal_tolerance.position"); + pos_tol = nav_state.get_safe("goal_tolerance.position"); } if (nav_state.has("goal_tolerance.yaw")) { - yaw_tol = nav_state.get("goal_tolerance.yaw"); + yaw_tol = nav_state.get_safe("goal_tolerance.yaw"); } const double dx_g = goal_pose.position.x - pose.position.x; diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp index f80ca7ff..e6f193ff 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp @@ -138,7 +138,7 @@ MPPIController::update_rt(NavState & nav_state) { // If navigation is IDLE, force zero velocity if (nav_state.has("navigation_state")) { - const auto nav_state_val = nav_state.get("navigation_state"); + const auto nav_state_val = nav_state.get_safe("navigation_state"); if (nav_state_val == easynav::GoalManager::State::IDLE) { twist_stamped_.header.stamp = get_node()->now(); twist_stamped_.twist.linear.x = 0.0; @@ -161,7 +161,7 @@ MPPIController::update_rt(NavState & nav_state) return; } - const auto & path = nav_state.get("path"); + const auto & path = nav_state.get_safe("path"); if (path.poses.empty()) { // If the path is empty, stop the robot and clear markers @@ -181,7 +181,7 @@ MPPIController::update_rt(NavState & nav_state) return; } - const auto & pose = nav_state.get("robot_pose").pose.pose; + const auto & pose = nav_state.get_safe("robot_pose").pose.pose; const auto & perceptions = nav_state.get_no_group(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); const auto & filtered = PointPerceptionsOpsView(perceptions) diff --git a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp index 7d78d1a5..5e0a08d7 100644 --- a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp +++ b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp @@ -403,7 +403,7 @@ void RegulatedPurePursuitController::update_rt(NavState & nav_state) { if (nav_state.has("navigation_state")) { - const auto goal_state = nav_state.get("navigation_state"); + const auto goal_state = nav_state.get_safe("navigation_state"); if (goal_state == easynav::GoalManager::State::IDLE) { std_msgs::msg::Header header; header.stamp = get_node()->now(); @@ -414,7 +414,7 @@ RegulatedPurePursuitController::update_rt(NavState & nav_state) if (!nav_state.has("path") || !nav_state.has("robot_pose")) {return;} - const auto & path = nav_state.get("path"); + const auto & path = nav_state.get_safe("path"); std_msgs::msg::Header header; header.frame_id = path.header.frame_id; @@ -425,7 +425,7 @@ RegulatedPurePursuitController::update_rt(NavState & nav_state) return; } - const auto & robot_pose = nav_state.get("robot_pose").pose.pose; + const auto & robot_pose = nav_state.get_safe("robot_pose").pose.pose; const double robot_yaw = tf2::getYaw(robot_pose.orientation); const auto & goal_pose = path.poses.back().pose; @@ -433,10 +433,10 @@ RegulatedPurePursuitController::update_rt(NavState & nav_state) double xy_tol = xy_goal_tolerance_; double yaw_tol = yaw_goal_tolerance_; if (nav_state.has("goal_tolerance.position")) { - xy_tol = nav_state.get("goal_tolerance.position"); + xy_tol = nav_state.get_safe("goal_tolerance.position"); } if (nav_state.has("goal_tolerance.yaw")) { - yaw_tol = nav_state.get("goal_tolerance.yaw"); + yaw_tol = nav_state.get_safe("goal_tolerance.yaw"); } const double dist_to_goal = std::hypot( diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp index 467b1465..3f21f185 100644 --- a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp +++ b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp @@ -100,9 +100,9 @@ TEST(DynamicWindowPurePursuit, ComputeDynamicWindowClampsToAccelLimits) const auto window = easynav::dynamic_window_pure_pursuit::computeDynamicWindow( current_speed, /*max_linear_vel=*/1.0, /*min_linear_vel=*/-1.0, - /*max_angular_vel=*/1.0, /*min_angular_vel=*/-1.0, - /*max_linear_accel=*/2.0, /*max_linear_decel=*/2.0, - /*max_angular_accel=*/2.0, /*max_angular_decel=*/2.0, /*dt=*/0.1); + /*max_angular_vel=*/ 1.0, /*min_angular_vel=*/-1.0, + /*max_linear_accel=*/ 2.0, /*max_linear_decel=*/2.0, + /*max_angular_accel=*/ 2.0, /*max_angular_decel=*/2.0, /*dt=*/0.1); EXPECT_NEAR(window.max_linear_vel, 0.2, 1e-9); EXPECT_NEAR(window.min_linear_vel, -0.2, 1e-9); diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index a617994e..c6fa3fdf 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -288,7 +288,7 @@ SerestController::closest_obstacle_distance( // 1) Prefer direct measurement if it exists if (nav_state.has("closest_obstacle_distance")) { try { - return nav_state.get("closest_obstacle_distance"); + return nav_state.get_safe("closest_obstacle_distance"); } catch (...) { // fall through to estimation } @@ -380,8 +380,8 @@ SerestController::fetch_required_inputs( return false; } - path = nav_state.get("path"); - odom = nav_state.get("robot_pose"); + path = nav_state.get_safe("path"); + odom = nav_state.get_safe("robot_pose"); if (rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) { last_input_ts_ = rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()); @@ -637,10 +637,10 @@ SerestController::update_rt(NavState & nav_state) double goal_pos_tol = goal_pos_tol_; double goal_yaw_tol = goal_yaw_tol_deg_ * (M_PI / 180.0); if (nav_state.has("goal_tolerance.position")) { - goal_pos_tol = nav_state.get("goal_tolerance.position"); + goal_pos_tol = nav_state.get_safe("goal_tolerance.position"); } if (nav_state.has("goal_tolerance.yaw")) { - goal_yaw_tol = nav_state.get("goal_tolerance.yaw"); + goal_yaw_tol = nav_state.get_safe("goal_tolerance.yaw"); } // Propagate the resolved tolerances to the members consumed by compute_goal_zone() // and maybe_final_align_and_publish(), so a GoalManager override actually takes effect. @@ -725,7 +725,7 @@ SerestController::update_rt(NavState & nav_state) d_closest, v_safe, v_curv, /*alpha*/1.0, allow_reverse_, dist_to_end, dist_xy_goal, gamma_slow, - /*in_final_align*/0, /*arrived*/0); + /*in_final_align*/ 0, /*arrived*/0); return; } } @@ -834,7 +834,7 @@ SerestController::update_rt(NavState & nav_state) d_closest, v_safe, v_curv, alpha, allow_reverse_, dist_to_end, dist_xy_goal, gamma_slow, - /*in_final_align=*/0, /*arrived=*/0); + /*in_final_align=*/ 0, /*arrived=*/0); } } // namespace easynav diff --git a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp index 6e80af64..94f7e397 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -93,7 +93,7 @@ SimpleController::update_rt(NavState & nav_state) if (!nav_state.has("path")) {return;} if (!nav_state.has("robot_pose")) {return;} - const auto & path = nav_state.get("path"); + const auto & path = nav_state.get_safe("path"); if (path.poses.empty()) { twist_stamped_.header.frame_id = path.header.frame_id; @@ -108,12 +108,12 @@ SimpleController::update_rt(NavState & nav_state) } // If we're very close to the final path pose, stop the robot. - const auto & pose = nav_state.get("robot_pose").pose.pose; + const auto & pose = nav_state.get_safe("robot_pose").pose.pose; const auto & goal_pose = path.poses.back().pose; const auto clock_type = get_node()->get_clock()->get_clock_type(); rclcpp::Time latest_stamp( - nav_state.get("robot_pose").header.stamp, + nav_state.get_safe("robot_pose").header.stamp, clock_type); if (rclcpp::Time(path.poses.back().header.stamp, latest_stamp.get_clock_type()) > latest_stamp) diff --git a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp index 1fe63e46..67cdac8a 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -206,7 +206,7 @@ void VffController::update_rt(NavState & nav_state) if (!nav_state.has("goals")) {return;} if (!nav_state.has("robot_pose")) {return;} - const auto & all_goals = nav_state.get("goals"); + const auto & all_goals = nav_state.get_safe("goals"); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); if (all_goals.goals.empty()) { @@ -218,7 +218,7 @@ void VffController::update_rt(NavState & nav_state) return; } - const auto & robot_pose = nav_state.get("robot_pose"); + const auto & robot_pose = nav_state.get_safe("robot_pose"); // Current position double current_x_ = robot_pose.pose.pose.position.x; diff --git a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt index 3c59e822..942a72d1 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt @@ -94,6 +94,9 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) + # Vendored third-party library: excluded from uncrustify instead of + # reformatting upstream code (same rationale as copyright/cpplint above). + set(ament_cmake_uncrustify_ADDITIONAL_EXCLUDE include/bonxai/*) ament_lint_auto_find_test_dependencies() endif() diff --git a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp index 702f5be8..842b0864 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -151,7 +151,7 @@ void CostmapPlanner::update(NavState & nav_state) } const auto & map = nav_state.get("map"); - const auto & robot_pose = nav_state.get("robot_pose"); + const auto & robot_pose = nav_state.get_safe("robot_pose"); const auto & goal = goals.goals.front().pose; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp index 3f65c574..845e1b54 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -89,7 +89,7 @@ void AStarPlanner::update(NavState & nav_state) const auto & navmap = nav_state.get<::navmap::NavMap>("map.navmap"); - const auto & robot_pose = nav_state.get("robot_pose"); + const auto & robot_pose = nav_state.get_safe("robot_pose"); const auto & goal = goals.goals.front().pose; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp index aa8f707d..30093942 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -126,7 +126,7 @@ SimplePlanner::update(NavState & nav_state) return; } - const auto & robot_pose = nav_state.get("robot_pose"); + const auto & robot_pose = nav_state.get_safe("robot_pose"); const auto & goal = goals.goals.front().pose; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();