add orientation filter, fix remaining TODOs

Signed-off-by: Michael Ferguson <[email protected]>

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

@@ -151,6 +151,14 @@ class GracefulController : public nav2_core::Controller
     const std::vector<geometry_msgs::msg::PoseStamped> & poses,
     std::vector<double> & distances);
 
+  /**
+   * @brief Control law requires proper orientations, not all planners provide them
+   * @param path Path to add orientations into
+   * @returns Path with orientations.
+   */
+  std::vector<geometry_msgs::msg::PoseStamped> addOrientations(
+    const std::vector<geometry_msgs::msg::PoseStamped> & path);
+
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::string plugin_name_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -51,6 +51,7 @@ struct Parameters
   bool prefer_final_rotation;
   double rotation_scaling_factor;
   bool allow_backward;
+  bool add_orientations;
 };
 
 /**

nav2_graceful_controller/src/graceful_controller.cpp

@@ -132,9 +132,16 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
   control_law_->setSlowdownRadius(params_->slowdown_radius);
   control_law_->setSpeedLimit(params_->v_linear_min, params_->v_linear_max, params_->v_angular_max);
 
-  // Transform path to robot base frame and publish it
+  // Transform path to robot base frame
   auto transformed_plan = path_handler_->transformGlobalPlan(
     pose, params_->max_robot_pose_search_dist);
+
+  // Add proper orientations to plan, if desired
+  if (params_->add_orientations) {
+    transformed_plan.poses = addOrientations(transformed_plan.poses);
+  }
+
+  // Publish plan for visualization
   transformed_plan_pub_->publish(transformed_plan);
 
   // Transform local frame to global frame to use in collision checking
@@ -148,7 +155,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
       logger_, "Could not transform %s to %s: %s",
       costmap_ros_->getBaseFrameID().c_str(), costmap_ros_->getGlobalFrameID().c_str(),
       ex.what());
-    return cmd_vel;  // TODO(fergs): this seems bad?
+    throw nav2_core::NoValidControl("Could not transform");
   }
 
   // Compute distance to goal as the path's integrated distance to account for path curvatures
@@ -422,6 +429,33 @@ void GracefulController::computeDistanceAlongPath(
   }
 }
 
+std::vector<geometry_msgs::msg::PoseStamped> GracefulController::addOrientations(
+  const std::vector<geometry_msgs::msg::PoseStamped> & path)
+{
+  std::vector<geometry_msgs::msg::PoseStamped> oriented_path;
+  oriented_path.resize(path.size());
+  if (path.empty()) {
+    // This really shouldn't happen
+    return oriented_path;
+  }
+
+  // The last pose will already be oriented since it is our goal
+  oriented_path.back() = path.back();
+
+  // For each pose, point at the next one
+  for (size_t i = 0; i < oriented_path.size() - 1; ++i) {
+    // XY will be unchanged
+    oriented_path[i] = path[i];
+    // Get relative yaw angle
+    double dx = path[i + 1].pose.position.x - path[i].pose.position.x;
+    double dy = path[i + 1].pose.position.y - path[i].pose.position.y;
+    double yaw = std::atan2(dy, dx);
+    oriented_path[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
+  }
+
+  return oriented_path;
+}
+
 }  // namespace nav2_graceful_controller
 
 // Register this controller as a nav2_core plugin

nav2_graceful_controller/src/parameter_handler.cpp

@@ -43,7 +43,7 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".max_lookahead", rclcpp::ParameterValue(1.0));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_robot_pose_search_dist",
-    rclcpp::ParameterValue(costmap_size_x / 2.0));  // TODO(fergs): this is crazy
+    rclcpp::ParameterValue(costmap_size_x / 2.0));
   declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(3.0));
   declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(2.0));
   declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.2));
@@ -66,6 +66,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".rotation_scaling_factor", rclcpp::ParameterValue(0.5));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".add_orientations", rclcpp::ParameterValue(false));
 
   node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
   node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead);
@@ -96,6 +98,7 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(plugin_name_ + ".prefer_final_rotation", params_.prefer_final_rotation);
   node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor);
   node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward);
+  node->get_parameter(plugin_name_ + ".add_orientations", params_.add_orientations);
 
   if ((params_.initial_rotation_tolerance > 0.0) && params_.allow_backward) {
     RCLCPP_WARN(
@@ -176,6 +179,8 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
           continue;
         }
         params_.allow_backward = parameter.as_bool();
+      } else if (name == plugin_name_ + ".add_orientations") {
+        params_.add_orientations = parameter.as_bool();
       }
     }
   }

nav2_graceful_controller/test/test_graceful_controller.cpp

@@ -286,7 +286,8 @@ TEST(GracefulControllerTest, dynamicParameters) {
       rclcpp::Parameter("test.prefer_final_rotation", false),
       rclcpp::Parameter("test.rotation_scaling_factor", 13.0),
       rclcpp::Parameter("test.v_angular_min_in_place", 14.0),
-      rclcpp::Parameter("test.allow_backward", true)});
+      rclcpp::Parameter("test.allow_backward", true),
+      rclcpp::Parameter("test.add_orientations", true)});
 
   // Spin
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
@@ -308,6 +309,7 @@ TEST(GracefulControllerTest, dynamicParameters) {
   EXPECT_EQ(node->get_parameter("test.rotation_scaling_factor").as_double(), 13.0);
   EXPECT_EQ(node->get_parameter("test.v_angular_min_in_place").as_double(), 14.0);
   EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), true);
+  EXPECT_EQ(node->get_parameter("test.add_orientations").as_bool(), true);
 
   // Set initial rotation to true
   results = params->set_parameters_atomically(