Skip to content

Commit

Permalink
[RotationShimController] Rotate to goal heading (#4332)
Browse files Browse the repository at this point in the history
When arriving in the goal xy tolerance, the rotation shim controller
takes back the control to command the robot to rotate in the goal
heading orientation.

The initial goal of the rotationShimController was to rotate the robot
at the beginning of a navigation towards the paths orientation because
some controllers are not good at performing in place rotations. For the
same reason, the rotationShimController should be able to rotate the
robot towards the goal heading.

Signed-off-by: Antoine Gennart <gennart.antoine@gmail.com>
  • Loading branch information
gennartan authored and SteveMacenski committed Aug 23, 2024
1 parent 06ec958 commit 2fe76a2
Show file tree
Hide file tree
Showing 5 changed files with 191 additions and 1 deletion.
4 changes: 4 additions & 0 deletions nav2_rotation_shim_controller/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@ This is useful for situations when working with plugins that are either too spec

As such, this controller will check the rough heading difference with respect to the robot and a newly received path. If within a threshold, it will pass the request onto the controller to execute. If it is outside of the threshold, this controller will rotate the robot towards that path heading. Once it is within the tolerance, it will then pass off control-execution from this rotation shim controller onto the primary controller plugin. At this point, the robot is still going to be rotating, allowing the current plugin to take control for a smooth hand off into path tracking. It is recommended to be more generous than strict in the angular threshold to allow for a smoother transition, but should be tuned for a specific application's desired behaviors.

When the `rotate_to_goal_heading` parameter is set to true, this controller is also able to take back control of the robot when reaching the XY goal tolerance of the goal checker. In this case, the robot will rotate towards the goal heading until the goal checker validate the goal and ends the current navigation task.

The Rotation Shim Controller is suitable for:
- Robots that can rotate in place, such as differential and omnidirectional robots.
- Preference to rotate in place rather than 'spiral out' when starting to track a new path that is at a significantly different heading than the robot's current heading.
Expand All @@ -35,6 +37,7 @@ See its [Configuration Guide Page](https://navigation.ros.org/configuration/pack
| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading |
| `max_angular_accel` | Maximum angular acceleration for rotation to heading |
| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. |
| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading |

Example fully-described XML with default parameter values:

Expand Down Expand Up @@ -67,6 +70,7 @@ controller_server:
rotate_to_heading_angular_vel: 1.8
max_angular_accel: 3.2
simulate_ahead_time: 1.0
rotate_to_goal_heading: false
# DWB parameters
...
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,13 @@ class RotationShimController : public nav2_core::Controller
*/
geometry_msgs::msg::PoseStamped getSampledPathPt();

/**
* @brief Find the goal point in path
* May throw exception if the path is empty
* @return pt location of the output point
*/
geometry_msgs::msg::PoseStamped getSampledPathGoal();

/**
* @brief Uses TF to find the location of the sampled path point in base frame
* @param pt location of the sampled path point
Expand Down Expand Up @@ -168,6 +175,7 @@ class RotationShimController : public nav2_core::Controller
double forward_sampling_distance_, angular_dist_threshold_;
double rotate_to_heading_angular_vel_, max_angular_accel_;
double control_duration_, simulate_ahead_time_;
bool rotate_to_goal_heading_;

// Dynamic parameters handler
std::mutex mutex_;
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
// Copyright (c) 2023 Open Navigation LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_
#define NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_

#include "nav2_core/goal_checker.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rclcpp/rclcpp.hpp"

namespace nav2_rotation_shim_controller::utils
{

/**
* @brief get the current pose of the robot
* @param goal_checker goal checker to get tolerances
* @param robot robot pose
* @param goal goal pose
* @return bool Whether the robot is in the distance tolerance ignoring rotation and speed
*/
inline bool withinPositionGoalTolerance(
nav2_core::GoalChecker * goal_checker,
const geometry_msgs::msg::Pose & robot,
const geometry_msgs::msg::Pose & goal)
{
if (goal_checker) {
geometry_msgs::msg::Pose pose_tolerance;
geometry_msgs::msg::Twist velocity_tolerance;
goal_checker->getTolerances(pose_tolerance, velocity_tolerance);

const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x;

auto dx = robot.position.x - goal.position.x;
auto dy = robot.position.y - goal.position.y;

auto dist_sq = dx * dx + dy * dy;

if (dist_sq < pose_tolerance_sq) {
return true;
}
}

return false;
}

} // namespace nav2_rotation_shim_controller::utils

#endif // NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <utility>

#include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp"
#include "nav2_rotation_shim_controller/tools/utils.hpp"

using rcl_interfaces::msg::ParameterType;

Expand Down Expand Up @@ -61,6 +62,8 @@ void RotationShimController::configure(
node, plugin_name_ + ".simulate_ahead_time", rclcpp::ParameterValue(1.0));
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING);
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false));

node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_);
Expand All @@ -74,6 +77,8 @@ void RotationShimController::configure(
node->get_parameter("controller_frequency", control_frequency);
control_duration_ = 1.0 / control_frequency;

node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_);

try {
primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
RCLCPP_INFO(
Expand Down Expand Up @@ -140,6 +145,41 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * goal_checker)
{
// Rotate to goal heading when in goal xy tolerance
if (rotate_to_goal_heading_) {
std::lock_guard<std::mutex> lock_reinit(mutex_);

try {
geometry_msgs::msg::PoseStamped sampled_pt_goal = getSampledPathGoal();

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!");
}

if (utils::withinPositionGoalTolerance(
goal_checker,
pose.pose,
sampled_pt_goal.pose))
{
double pose_yaw = tf2::getYaw(pose.pose.orientation);
double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation);

double angular_distance_to_heading = angles::shortest_angular_distance(pose_yaw, goal_yaw);

return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
}
} catch (const std::runtime_error & e) {
RCLCPP_INFO(
logger_,
"Rotation Shim Controller was unable to find a goal point,"
" a rotational collision was detected, or TF failed to transform"
" into base frame! what(): %s", e.what());
}
}

if (path_updated_) {
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
Expand Down Expand Up @@ -202,6 +242,17 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt()
"passing off to primary controller plugin.", forward_sampling_distance_));
}

geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal()
{
if (current_path_.poses.empty()) {
throw nav2_core::InvalidPath("Path is empty - cannot find a goal point");
}

auto goal = current_path_.poses.back();
goal.header.stamp = clock_->now();
return goal;
}

geometry_msgs::msg::Pose
RotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt)
{
Expand Down Expand Up @@ -305,6 +356,10 @@ RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter>
} else if (name == plugin_name_ + ".simulate_ahead_time") {
simulate_ahead_time_ = parameter.as_double();
}
} else if (type == ParameterType::PARAMETER_BOOL) {
if (name == plugin_name_ + ".rotate_to_goal_heading") {
rotate_to_goal_heading_ = parameter.as_bool();
}
}
}

Expand Down
65 changes: 64 additions & 1 deletion nav2_rotation_shim_controller/test/test_shim_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -309,6 +309,67 @@ TEST(RotationShimControllerTest, computeVelocityTests)
EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);
}

TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) {
auto ctrl = std::make_shared<RotationShimShim>();
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
std::string name = "PathFollower";
auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
auto listener = std::make_shared<tf2_ros::TransformListener>(*tf, node, true);
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
rclcpp_lifecycle::State state;
costmap->on_configure(state);
auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(node);

geometry_msgs::msg::TransformStamped transform;
transform.header.frame_id = "base_link";
transform.child_frame_id = "odom";
transform.transform.rotation.x = 0.0;
transform.transform.rotation.y = 0.0;
transform.transform.rotation.z = 0.0;
transform.transform.rotation.w = 1.0;
tf_broadcaster->sendTransform(transform);

// set a valid primary controller so we can do lifecycle
node->declare_parameter(
"PathFollower.primary_controller",
std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
node->declare_parameter(
"PathFollower.rotate_to_goal_heading",
true);

auto controller = std::make_shared<RotationShimShim>();
controller->configure(node, name, tf, costmap);
controller->activate();

// Test state update and path setting
nav_msgs::msg::Path path;
path.header.frame_id = "fake_frame";
path.poses.resize(4);

geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = "base_link";
geometry_msgs::msg::Twist velocity;
nav2_controller::SimpleGoalChecker checker;
checker.initialize(node, "checker", costmap);

path.header.frame_id = "base_link";
path.poses[0].pose.position.x = 0.0;
path.poses[0].pose.position.y = 0.0;
path.poses[1].pose.position.x = 0.05;
path.poses[1].pose.position.y = 0.05;
path.poses[2].pose.position.x = 0.10;
path.poses[2].pose.position.y = 0.10;
path.poses[3].pose.position.x = 0.20;
path.poses[3].pose.position.y = 0.20;
path.poses[3].header.frame_id = "base_link";

// this should make the goal checker to validated the fact that the robot is in range
// of the goal. The rotation shim controller should rotate toward the goal heading
// then it will throw an exception because the costmap is bogus
controller->setPlan(path);
EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);
}

TEST(RotationShimControllerTest, testDynamicParameter)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
Expand Down Expand Up @@ -338,7 +399,8 @@ TEST(RotationShimControllerTest, testDynamicParameter)
rclcpp::Parameter("test.rotate_to_heading_angular_vel", 7.0),
rclcpp::Parameter("test.max_angular_accel", 7.0),
rclcpp::Parameter("test.simulate_ahead_time", 7.0),
rclcpp::Parameter("test.primary_controller", std::string("HI"))});
rclcpp::Parameter("test.primary_controller", std::string("HI")),
rclcpp::Parameter("test.rotate_to_goal_heading", true)});

rclcpp::spin_until_future_complete(
node->get_node_base_interface(),
Expand All @@ -349,4 +411,5 @@ TEST(RotationShimControllerTest, testDynamicParameter)
EXPECT_EQ(node->get_parameter("test.rotate_to_heading_angular_vel").as_double(), 7.0);
EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0);
EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0);
EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true);
}

0 comments on commit 2fe76a2

Please sign in to comment.