Skip to content

Use of get_safe #77

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
fmrico wants to merge 1 commit into
base: rolling
Choose a base branch
from
Open

Use of get_safe #77

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 6 additions & 6 deletions controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<easynav::GoalManager::State>("navigation_state");
const auto nav_state_val = nav_state.get_safe<easynav::GoalManager::State>("navigation_state");
if (nav_state_val == easynav::GoalManager::State::IDLE) {
cmd_vel_.header.stamp = get_node()->now();
cmd_vel_.twist.linear.x = 0.0;
Expand All @@ -151,7 +151,7 @@ MPCController::update_rt(NavState & nav_state)
return;
}

nav_msgs::msg::Path path = nav_state.get<nav_msgs::msg::Path>("path");
nav_msgs::msg::Path path = nav_state.get_safe<nav_msgs::msg::Path>("path");
if (path.poses.empty()) {
// If the path is empty, stop the robot
cmd_vel_.header.frame_id = path.header.frame_id;
Expand All @@ -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<nav_msgs::msg::Odometry>("robot_pose");
const auto & robot_pose_msg = nav_state.get_safe<nav_msgs::msg::Odometry>("robot_pose");
const auto & robot_p = robot_pose_msg.pose.pose.position;

// Goal is the last point of the planner path
Expand Down Expand Up @@ -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<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
const auto pose = nav_state.get_safe<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
double roll_, pitch_, yaw_;
tf2::Quaternion q(
pose.orientation.x,
Expand Down Expand Up @@ -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<double>("goal_tolerance.position");
pos_tol = nav_state.get_safe<double>("goal_tolerance.position");
}
if (nav_state.has("goal_tolerance.yaw")) {
yaw_tol = nav_state.get<double>("goal_tolerance.yaw");
yaw_tol = nav_state.get_safe<double>("goal_tolerance.yaw");
}

const double dx_g = goal_pose.position.x - pose.position.x;
Expand Down
6 changes: 3 additions & 3 deletions controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<easynav::GoalManager::State>("navigation_state");
const auto nav_state_val = nav_state.get_safe<easynav::GoalManager::State>("navigation_state");
if (nav_state_val == easynav::GoalManager::State::IDLE) {
twist_stamped_.header.stamp = get_node()->now();
twist_stamped_.twist.linear.x = 0.0;
Expand All @@ -161,7 +161,7 @@ MPPIController::update_rt(NavState & nav_state)
return;
}

const auto & path = nav_state.get<nav_msgs::msg::Path>("path");
const auto & path = nav_state.get_safe<nav_msgs::msg::Path>("path");

if (path.poses.empty()) {
// If the path is empty, stop the robot and clear markers
Expand All @@ -181,7 +181,7 @@ MPPIController::update_rt(NavState & nav_state)
return;
}

const auto & pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
const auto & pose = nav_state.get_safe<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
const auto & perceptions = nav_state.get_no_group<PointPerception>();
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
const auto & filtered = PointPerceptionsOpsView(perceptions)
Expand Down
10 changes: 5 additions & 5 deletions ...ated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -403,7 +403,7 @@ void
RegulatedPurePursuitController::update_rt(NavState & nav_state)
{
if (nav_state.has("navigation_state")) {
const auto goal_state = nav_state.get<easynav::GoalManager::State>("navigation_state");
const auto goal_state = nav_state.get_safe<easynav::GoalManager::State>("navigation_state");
if (goal_state == easynav::GoalManager::State::IDLE) {
std_msgs::msg::Header header;
header.stamp = get_node()->now();
Expand All @@ -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<nav_msgs::msg::Path>("path");
const auto & path = nav_state.get_safe<nav_msgs::msg::Path>("path");

std_msgs::msg::Header header;
header.frame_id = path.header.frame_id;
Expand All @@ -425,18 +425,18 @@ RegulatedPurePursuitController::update_rt(NavState & nav_state)
return;
}

const auto & robot_pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
const auto & robot_pose = nav_state.get_safe<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
const double robot_yaw = tf2::getYaw(robot_pose.orientation);

const auto & goal_pose = path.poses.back().pose;

double xy_tol = xy_goal_tolerance_;
double yaw_tol = yaw_goal_tolerance_;
if (nav_state.has("goal_tolerance.position")) {
xy_tol = nav_state.get<double>("goal_tolerance.position");
xy_tol = nav_state.get_safe<double>("goal_tolerance.position");
}
if (nav_state.has("goal_tolerance.yaw")) {
yaw_tol = nav_state.get<double>("goal_tolerance.yaw");
yaw_tol = nav_state.get_safe<double>("goal_tolerance.yaw");
}

const double dist_to_goal = std::hypot(
Expand Down
6 changes: 3 additions & 3 deletions controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
14 changes: 7 additions & 7 deletions controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>("closest_obstacle_distance");
return nav_state.get_safe<double>("closest_obstacle_distance");
} catch (...) {
// fall through to estimation
}
Expand Down Expand Up @@ -380,8 +380,8 @@ SerestController::fetch_required_inputs(
return false;
}

path = nav_state.get<nav_msgs::msg::Path>("path");
odom = nav_state.get<nav_msgs::msg::Odometry>("robot_pose");
path = nav_state.get_safe<nav_msgs::msg::Path>("path");
odom = nav_state.get_safe<nav_msgs::msg::Odometry>("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());
Expand Down Expand Up @@ -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<double>("goal_tolerance.position");
goal_pos_tol = nav_state.get_safe<double>("goal_tolerance.position");
}
if (nav_state.has("goal_tolerance.yaw")) {
goal_yaw_tol = nav_state.get<double>("goal_tolerance.yaw");
goal_yaw_tol = nav_state.get_safe<double>("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.
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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
Expand Down
6 changes: 3 additions & 3 deletions controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<nav_msgs::msg::Path>("path");
const auto & path = nav_state.get_safe<nav_msgs::msg::Path>("path");

if (path.poses.empty()) {
twist_stamped_.header.frame_id = path.header.frame_id;
Expand All @@ -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<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
const auto & pose = nav_state.get_safe<nav_msgs::msg::Odometry>("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(
Comment on lines +111 to 115
nav_state.get<nav_msgs::msg::Odometry>("robot_pose").header.stamp,
nav_state.get_safe<nav_msgs::msg::Odometry>("robot_pose").header.stamp,
clock_type);
if (rclcpp::Time(path.poses.back().header.stamp,
latest_stamp.get_clock_type()) > latest_stamp)
Expand Down
4 changes: 2 additions & 2 deletions controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<nav_msgs::msg::Goals>("goals");
const auto & all_goals = nav_state.get_safe<nav_msgs::msg::Goals>("goals");
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();

if (all_goals.goals.empty()) {
Expand All @@ -218,7 +218,7 @@ void VffController::update_rt(NavState & nav_state)
return;
}

const auto & robot_pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose");
const auto & robot_pose = nav_state.get_safe<nav_msgs::msg::Odometry>("robot_pose");

// Current position
double current_x_ = robot_pose.pose.pose.position.x;
Expand Down
3 changes: 3 additions & 0 deletions maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()

Expand Down
2 changes: 1 addition & 1 deletion planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,7 @@ void CostmapPlanner::update(NavState & nav_state)
}

const auto & map = nav_state.get<Costmap2D>("map");
const auto & robot_pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose");
const auto & robot_pose = nav_state.get_safe<nav_msgs::msg::Odometry>("robot_pose");
const auto & goal = goals.goals.front().pose;
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();

Expand Down
2 changes: 1 addition & 1 deletion planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<nav_msgs::msg::Odometry>("robot_pose");
const auto & robot_pose = nav_state.get_safe<nav_msgs::msg::Odometry>("robot_pose");
const auto & goal = goals.goals.front().pose;
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();

Expand Down
2 changes: 1 addition & 1 deletion planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,7 @@ SimplePlanner::update(NavState & nav_state)
return;
}

const auto & robot_pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose");
const auto & robot_pose = nav_state.get_safe<nav_msgs::msg::Odometry>("robot_pose");
const auto & goal = goals.goals.front().pose;
const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();

Expand Down
Loading