diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index c3812ec1d3c..6d4e8b86b33 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -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 @@ -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_; diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp new file mode 100644 index 00000000000..0a4ff4ac163 --- /dev/null +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp @@ -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_ diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index afd32381459..4d40b056a54 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -19,6 +19,7 @@ #include #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" +#include "nav2_rotation_shim_controller/tools/utils.hpp" using rcl_interfaces::msg::ParameterType; @@ -60,6 +61,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_); @@ -73,6 +76,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( @@ -139,6 +144,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 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 (nav2_rotation_shim_controller::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 lock(*(costmap->getMutex())); @@ -201,6 +241,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) { @@ -305,6 +356,10 @@ RotationShimController::dynamicParametersCallback(std::vector } 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(); + } } } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 1c172436e68..922b6672056 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -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(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(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(); + 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("ShimControllerTest"); @@ -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(), @@ -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); }