Skip to content
Permalink

Comparing changes

This is a direct comparison between two commits made in this repository or its related repositories. View the default comparison for this range or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: ros-navigation/navigation2
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: 3aabd4aba2a94707f4bcec065c8299d5596d8810
Choose a base ref
..
head repository: ros-navigation/navigation2
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: 4095c29031065f0df17e610cb295f275c9507439
Choose a head ref
Showing with 7 additions and 5 deletions.
  1. +7 −5 nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
12 changes: 7 additions & 5 deletions nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
View file
Original file line number Diff line number Diff line change
@@ -151,7 +151,10 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
try {
geometry_msgs::msg::PoseStamped sampled_pt_goal = getSampledPathGoal();

if (!nav2_util::transformPoseInTargetFrame(sampled_pt_goal, sampled_pt_goal, *tf_, pose.header.frame_id)) {
if (!nav2_util::transformPoseInTargetFrame(
sampled_pt_goal, sampled_pt_goal, *tf_,
pose.header.frame_id))
{
throw nav2_core::ControllerTFError("Failed to transform pose to base frame!");
}

@@ -165,9 +168,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands

double angular_distance_to_heading = goal_yaw - pose_yaw;
if (angular_distance_to_heading > M_PI) {
angular_distance_to_heading -= 2*M_PI;
angular_distance_to_heading -= 2 * M_PI;
} else if (angular_distance_to_heading < -M_PI) {
angular_distance_to_heading += 2*M_PI;
angular_distance_to_heading += 2 * M_PI;
}

return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
@@ -246,8 +249,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt()
geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal()
{
if (current_path_.poses.empty()) {
throw nav2_core::ControllerException(
"Path is empty - cannot find a goal point");
throw nav2_core::InvalidPath("Path is empty - cannot find a goal point");
}

auto goal = current_path_.poses.back();