Add utils and minor fixes

Signed-off-by: Alberto Tudela <[email protected]>

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}

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

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.

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;

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

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_

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;

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<rclcpp::Parameter> 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") {

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;
 }
 

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