diff --git a/nav2_graceful_motion_controller/CMakeLists.txt b/nav2_graceful_motion_controller/CMakeLists.txt index bfef3f0874b..e87afcac46f 100644 --- a/nav2_graceful_motion_controller/CMakeLists.txt +++ b/nav2_graceful_motion_controller/CMakeLists.txt @@ -43,6 +43,7 @@ add_library(${library_name} SHARED src/parameter_handler.cpp src/path_handler.cpp src/smooth_control_law.cpp + src/utils.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp index f8a2c4001fa..8f894059cc7 100644 --- a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp @@ -32,15 +32,15 @@ namespace nav2_graceful_motion_controller */ struct EgocentricPolarCoordinates { - double r; // Distance between the robot pose and the target pose. - double phi; // Orientation of target with respect to the line of sight - // from the robot to the target. - double delta; // Steering angle of the robot with respect to the line of sight. + float r; // Distance between the robot pose and the target pose. + float phi; // Orientation of target with respect to the line of sight + // from the robot to the target. + float delta; // Steering angle of the robot with respect to the line of sight. EgocentricPolarCoordinates( - const double & r_in = 0.0, - const double & phi_in = 0.0, - const double & delta_in = 0.0) + const float & r_in = 0.0, + const float & phi_in = 0.0, + const float & delta_in = 0.0) : r(r_in), phi(phi_in), delta(delta_in) {} /** @@ -60,7 +60,7 @@ struct EgocentricPolarCoordinates r = sqrt(dX * dX + dY * dY); phi = angles::normalize_angle(tf2::getYaw(target.orientation) + line_of_sight); - delta = tf2::getYaw(current.orientation) + line_of_sight; + delta = angles::normalize_angle(tf2::getYaw(current.orientation) + line_of_sight); } }; diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp index 5dd4d6916f0..dc03808eff6 100644 --- a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp @@ -30,8 +30,7 @@ #include "nav2_graceful_motion_controller/path_handler.hpp" #include "nav2_graceful_motion_controller/parameter_handler.hpp" #include "nav2_graceful_motion_controller/smooth_control_law.hpp" - -#include "visualization_msgs/msg/marker.hpp" +#include "nav2_graceful_motion_controller/utils.hpp" namespace nav2_graceful_motion_controller { @@ -118,26 +117,6 @@ class GracefulMotionController : public nav2_core::Controller const double & motion_target_dist, const nav_msgs::msg::Path & path); - /** - * @brief Create a PointStamped message of the motion target for - * debugging / visualization porpuses. - * - * @param motion_target Motion target in PoseStamped format - * @return geometry_msgs::msg::PointStamped Motion target in PointStamped format - */ - geometry_msgs::msg::PointStamped createMotionTargetMsg( - const geometry_msgs::msg::PoseStamped & motion_target); - - /** - * @brief Create a flat circle marker of radius slowdown_radius around the motion target for - * debugging / visualization porpuses. - * - * @param motion_target Motion target - * @return visualization_msgs::msg::Marker Slowdown marker - */ - visualization_msgs::msg::Marker createSlowdownMsg( - const geometry_msgs::msg::PoseStamped & motion_target); - /** * @brief Simulate trajectory calculating in every step the new velocity command based on * a new curvature value and checking for collisions. diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp index bc232249105..ce905d7a820 100644 --- a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp @@ -41,6 +41,7 @@ struct Parameters double lambda; double v_linear_min; double v_linear_max; + double v_linear_max_initial; double v_angular_max; double slowdown_radius; bool initial_rotation; diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp index 0b661ddd623..de57d2165f0 100644 --- a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp @@ -88,6 +88,17 @@ class SmoothControlLaw const geometry_msgs::msg::Pose & current = geometry_msgs::msg::Pose()); protected: + /** + * @brief Calculate the path curvature using a Lyapunov-based feedback control law from + * "A smooth control law for graceful motion" (Jong Jin Park and Benjamin Kuipers). + * + * @param r Distance between the robot and the target. + * @param phi Orientation of target with respect to the line of sight from the robot to the target. + * @param delta Steering angle of the robot. + * @return The curvature + */ + double calculateCurvature(double r, double phi, double delta); + /** * @brief Ratio of the rate of change in phi to the rate of change in r. Controls the convergence * of the slow subsystem. @@ -134,17 +145,6 @@ class SmoothControlLaw * @brief Maximum angular velocity. */ double v_angular_max_; - - /** - * @brief Calculate the path curvature using a Lyapunov-based feedback control law from - * "A smooth control law for graceful motion" (Jong Jin Park and Benjamin Kuipers). - * - * @param r Distance between the robot and the target. - * @param phi Orientation of target with respect to the line of sight from the robot to the target. - * @param delta Steering angle of the robot. - * @return The curvature - */ - double calculateCurvature(double r, double phi, double delta); }; } // namespace nav2_graceful_motion_controller diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/utils.hpp b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/utils.hpp new file mode 100644 index 00000000000..ba0868ac207 --- /dev/null +++ b/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/utils.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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_GRACEFUL_MOTION_CONTROLLER__UTILS_HPP_ +#define NAV2_GRACEFUL_MOTION_CONTROLLER__UTILS_HPP_ + +#include "geometry_msgs/msg/point_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "visualization_msgs/msg/marker.hpp" + +namespace nav2_graceful_motion_controller +{ +/** + * @brief Create a PointStamped message of the motion target for + * debugging / visualization porpuses. + * + * @param motion_target Motion target in PoseStamped format + * @return geometry_msgs::msg::PointStamped Motion target in PointStamped format + */ +geometry_msgs::msg::PointStamped createMotionTargetMsg( + const geometry_msgs::msg::PoseStamped & motion_target); + +/** + * @brief Create a flat circle marker of radius slowdown_radius around the motion target for + * debugging / visualization porpuses. + * + * @param motion_target Motion target + * @param slowdown_radius Radius of the slowdown circle + * @return visualization_msgs::msg::Marker Slowdown marker + */ +visualization_msgs::msg::Marker createSlowdownMarker( + const geometry_msgs::msg::PoseStamped & motion_target, const double & slowdown_radius); + +} // namespace nav2_graceful_motion_controller + +#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__UTILS_HPP_ diff --git a/nav2_graceful_motion_controller/src/graceful_motion_controller.cpp b/nav2_graceful_motion_controller/src/graceful_motion_controller.cpp index 6b403d4d637..7c3f89ecde8 100644 --- a/nav2_graceful_motion_controller/src/graceful_motion_controller.cpp +++ b/nav2_graceful_motion_controller/src/graceful_motion_controller.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "nav2_core/controller_exceptions.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_graceful_motion_controller/graceful_motion_controller.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -123,11 +124,13 @@ geometry_msgs::msg::TwistStamped GracefulMotionController::computeVelocityComman // Get the particular point on the path at the motion target distance and publish it auto motion_target = getMotionTarget(params_->motion_target_dist, transformed_plan); - auto motion_target_point = createMotionTargetMsg(motion_target); + auto motion_target_point = nav2_graceful_motion_controller::createMotionTargetMsg(motion_target); motion_target_pub_->publish(motion_target_point); // Publish marker for slowdown radius around motion target for debugging / visualization - auto slowdown_marker = createSlowdownMsg(motion_target); + auto slowdown_marker = nav2_graceful_motion_controller::createSlowdownMarker( + motion_target, + params_->slowdown_radius); slowdown_pub_->publish(slowdown_marker); // Compute distance to goal @@ -144,8 +147,7 @@ geometry_msgs::msg::TwistStamped GracefulMotionController::computeVelocityComman double dx = goal_pose.pose.position.x - stl_pose.pose.position.x; double dy = goal_pose.pose.position.y - stl_pose.pose.position.y; double yaw = std::atan2(dy, dx); - motion_target.pose.orientation.z = sin(yaw / 2.0); - motion_target.pose.orientation.w = cos(yaw / 2.0); + motion_target.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); } // Compute velocity command: @@ -204,7 +206,7 @@ void GracefulMotionController::setSpeedLimit( if (percentage) { // Speed limit is expressed in % from maximum speed of robot params_->v_linear_max = std::max( - params_->v_linear_max * speed_limit / 100.0, + params_->v_linear_max_initial * speed_limit / 100.0, params_->v_linear_min); } else { // Speed limit is expressed in m/s @@ -222,7 +224,7 @@ geometry_msgs::msg::PoseStamped GracefulMotionController::getMotionTarget( // Find the first pose which is at a distance greater than the motion target distance auto goal_pose_it = std::find_if( transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { - return hypot(ps.pose.position.x, ps.pose.position.y) >= motion_target_dist; + return std::hypot(ps.pose.position.x, ps.pose.position.y) >= motion_target_dist; }); // If the pose is not far enough, take the last pose @@ -233,37 +235,6 @@ geometry_msgs::msg::PoseStamped GracefulMotionController::getMotionTarget( return *goal_pose_it; } -geometry_msgs::msg::PointStamped GracefulMotionController::createMotionTargetMsg( - const geometry_msgs::msg::PoseStamped & motion_target) -{ - geometry_msgs::msg::PointStamped motion_target_point; - motion_target_point.header = motion_target.header; - motion_target_point.point = motion_target.pose.position; - motion_target_point.point.z = 0.01; - return motion_target_point; -} - -visualization_msgs::msg::Marker GracefulMotionController::createSlowdownMsg( - const geometry_msgs::msg::PoseStamped & motion_target) -{ - visualization_msgs::msg::Marker slowdown_marker; - slowdown_marker.header = motion_target.header; - slowdown_marker.ns = "slowdown"; - slowdown_marker.id = 0; - slowdown_marker.type = visualization_msgs::msg::Marker::SPHERE; - slowdown_marker.action = visualization_msgs::msg::Marker::ADD; - slowdown_marker.pose = motion_target.pose; - slowdown_marker.pose.position.z = 0.01; - slowdown_marker.scale.x = params_->slowdown_radius * 2.0; - slowdown_marker.scale.y = params_->slowdown_radius * 2.0; - slowdown_marker.scale.z = 0.02; - slowdown_marker.color.a = 0.2; - slowdown_marker.color.r = 0.0; - slowdown_marker.color.g = 1.0; - slowdown_marker.color.b = 0.0; - return slowdown_marker; -} - bool GracefulMotionController::simulateTrajectory( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::PoseStamped & motion_target, nav_msgs::msg::Path & trajectory) @@ -310,9 +281,7 @@ bool GracefulMotionController::simulateTrajectory( } // Check if we reach the goal - double error_x = motion_target.pose.position.x - next_pose.pose.position.x; - double error_y = motion_target.pose.position.y - next_pose.pose.position.y; - distance = std::hypot(error_x, error_y); + distance = nav2_util::geometry_utils::euclidean_distance(motion_target.pose, next_pose.pose); }while(distance > resolution_ && trajectory.poses.size() < max_iter); return true; diff --git a/nav2_graceful_motion_controller/src/parameter_handler.cpp b/nav2_graceful_motion_controller/src/parameter_handler.cpp index 20549ed972c..99cfdee03b9 100644 --- a/nav2_graceful_motion_controller/src/parameter_handler.cpp +++ b/nav2_graceful_motion_controller/src/parameter_handler.cpp @@ -77,6 +77,7 @@ ParameterHandler::ParameterHandler( node->get_parameter(plugin_name_ + ".lambda", params_.lambda); node->get_parameter(plugin_name_ + ".v_linear_min", params_.v_linear_min); node->get_parameter(plugin_name_ + ".v_linear_max", params_.v_linear_max); + params_.v_linear_max_initial = params_.v_linear_max; node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max); node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius); node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation); @@ -115,6 +116,7 @@ ParameterHandler::dynamicParametersCallback(std::vector param params_.v_linear_min = parameter.as_double(); } else if (name == plugin_name_ + ".v_linear_max") { params_.v_linear_max = parameter.as_double(); + params_.v_linear_max_initial = params_.v_linear_max; } else if (name == plugin_name_ + ".v_angular_max") { params_.v_angular_max = parameter.as_double(); } else if (name == plugin_name_ + ".slowdown_radius") { diff --git a/nav2_graceful_motion_controller/src/smooth_control_law.cpp b/nav2_graceful_motion_controller/src/smooth_control_law.cpp index daf7d623ed1..4658cb9c597 100644 --- a/nav2_graceful_motion_controller/src/smooth_control_law.cpp +++ b/nav2_graceful_motion_controller/src/smooth_control_law.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_graceful_motion_controller/ego_polar_coords.hpp" #include "nav2_graceful_motion_controller/smooth_control_law.hpp" @@ -80,8 +81,7 @@ geometry_msgs::msg::Pose SmoothControlLaw::calculateNextPose( next.position.x = current.position.x + vel.linear.x * dt * cos(yaw); next.position.y = current.position.y + vel.linear.x * dt * sin(yaw); yaw += vel.angular.z * dt; - next.orientation.z = sin(yaw / 2.0); - next.orientation.w = cos(yaw / 2.0); + next.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); return next; } diff --git a/nav2_graceful_motion_controller/src/utils.cpp b/nav2_graceful_motion_controller/src/utils.cpp new file mode 100644 index 00000000000..488b6790223 --- /dev/null +++ b/nav2_graceful_motion_controller/src/utils.cpp @@ -0,0 +1,51 @@ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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. + +#include "nav2_graceful_motion_controller/utils.hpp" + +namespace nav2_graceful_motion_controller +{ + +geometry_msgs::msg::PointStamped createMotionTargetMsg( + const geometry_msgs::msg::PoseStamped & motion_target) +{ + geometry_msgs::msg::PointStamped motion_target_point; + motion_target_point.header = motion_target.header; + motion_target_point.point = motion_target.pose.position; + motion_target_point.point.z = 0.01; + return motion_target_point; +} + +visualization_msgs::msg::Marker createSlowdownMarker( + const geometry_msgs::msg::PoseStamped & motion_target, const double & slowdown_radius) +{ + visualization_msgs::msg::Marker slowdown_marker; + slowdown_marker.header = motion_target.header; + slowdown_marker.ns = "slowdown"; + slowdown_marker.id = 0; + slowdown_marker.type = visualization_msgs::msg::Marker::SPHERE; + slowdown_marker.action = visualization_msgs::msg::Marker::ADD; + slowdown_marker.pose = motion_target.pose; + slowdown_marker.pose.position.z = 0.01; + slowdown_marker.scale.x = slowdown_radius * 2.0; + slowdown_marker.scale.y = slowdown_radius * 2.0; + slowdown_marker.scale.z = 0.02; + slowdown_marker.color.a = 0.2; + slowdown_marker.color.r = 0.0; + slowdown_marker.color.g = 1.0; + slowdown_marker.color.b = 0.0; + return slowdown_marker; +} + +} // namespace nav2_graceful_motion_controller diff --git a/nav2_graceful_motion_controller/test/test_egopolar.cpp b/nav2_graceful_motion_controller/test/test_egopolar.cpp index f13447a8309..0560e2d04b2 100644 --- a/nav2_graceful_motion_controller/test/test_egopolar.cpp +++ b/nav2_graceful_motion_controller/test/test_egopolar.cpp @@ -21,22 +21,22 @@ TEST(EgocentricPolarCoordinatesTest, constructorDefault) { nav2_graceful_motion_controller::EgocentricPolarCoordinates coords; - EXPECT_DOUBLE_EQ(0.0, coords.r); - EXPECT_DOUBLE_EQ(0.0, coords.phi); - EXPECT_DOUBLE_EQ(0.0, coords.delta); + EXPECT_FLOAT_EQ(0.0, coords.r); + EXPECT_FLOAT_EQ(0.0, coords.phi); + EXPECT_FLOAT_EQ(0.0, coords.delta); } TEST(EgocentricPolarCoordinatesTest, constructorWithValues) { - double r_value = 5.0; - double phi_value = 1.2; - double delta_value = -0.5; + float r_value = 5.0; + float phi_value = 1.2; + float delta_value = -0.5; nav2_graceful_motion_controller::EgocentricPolarCoordinates coords(r_value, phi_value, delta_value); - EXPECT_DOUBLE_EQ(r_value, coords.r); - EXPECT_DOUBLE_EQ(phi_value, coords.phi); - EXPECT_DOUBLE_EQ(delta_value, coords.delta); + EXPECT_FLOAT_EQ(r_value, coords.r); + EXPECT_FLOAT_EQ(phi_value, coords.phi); + EXPECT_FLOAT_EQ(delta_value, coords.delta); } TEST(EgocentricPolarCoordinatesTest, constructorFromPoses) { @@ -54,9 +54,9 @@ TEST(EgocentricPolarCoordinatesTest, constructorFromPoses) { // Perform assertions based on expected values. - EXPECT_DOUBLE_EQ(3.6055512428283691, coords.r); - EXPECT_DOUBLE_EQ(-0.18279374837875384, coords.phi); - EXPECT_DOUBLE_EQ(-1.1827937483787536, coords.delta); + EXPECT_FLOAT_EQ(3.6055512428283691, coords.r); + EXPECT_FLOAT_EQ(-0.18279374837875384, coords.phi); + EXPECT_FLOAT_EQ(-1.1827937483787536, coords.delta); } int main(int argc, char ** argv) diff --git a/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp b/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp index dce125c59b1..4674b4cb9e3 100644 --- a/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp +++ b/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp @@ -59,15 +59,15 @@ class GMControllerFixture : public nav2_graceful_motion_controller::GracefulMoti geometry_msgs::msg::PointStamped createMotionTargetMsg( const geometry_msgs::msg::PoseStamped & motion_target) { - return nav2_graceful_motion_controller::GracefulMotionController::createMotionTargetMsg( - motion_target); + return nav2_graceful_motion_controller::createMotionTargetMsg(motion_target); } - visualization_msgs::msg::Marker createSlowdownMsg( + visualization_msgs::msg::Marker createSlowdownMarker( const geometry_msgs::msg::PoseStamped & motion_target) { - return nav2_graceful_motion_controller::GracefulMotionController::createSlowdownMsg( - motion_target); + return nav2_graceful_motion_controller::createSlowdownMarker( + motion_target, + params_->slowdown_radius); } geometry_msgs::msg::Twist rotateToTarget(const double & angle_to_target) @@ -76,6 +76,14 @@ class GMControllerFixture : public nav2_graceful_motion_controller::GracefulMoti angle_to_target); } + bool simulateTrajectory( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::PoseStamped & motion_target, nav_msgs::msg::Path & trajectory) + { + return nav2_graceful_motion_controller::GracefulMotionController::simulateTrajectory( + robot_pose, motion_target, trajectory); + } + double getSpeedLinearMax() {return params_->v_linear_max;} nav_msgs::msg::Path transformGlobalPlan( @@ -379,7 +387,7 @@ TEST(GracefulMotionControllerTest, createSlowdownMsg) { rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); // Create slowdown message - auto slowdown_msg = controller->createSlowdownMsg(motion_target); + auto slowdown_msg = controller->createSlowdownMarker(motion_target); // Check results EXPECT_EQ(slowdown_msg.header.frame_id, "map");