Implemented angular cost

frenet_trajectory_planner/include/frenet_trajectory_planner/conversion_adapters/circle_adapter.hpp

@@ -86,6 +86,7 @@ CartesianState CircleAdapter::convertFrenet2Cartesian(const FrenetState & frenet
   // cartesian_state[6] = std::atan2(t_frenet_[1], t_frenet_[0]) + std::atan2(
   //   frenet_state[4],
   //   frenet_state[1]); // implement in a correct way (yaw angle also should be added to frenet state)
+  cartesian_state[6] = std::atan2(cartesian_state[4], cartesian_state[1]);
 
   return cartesian_state;
 

nav2_frenet_ilqr_controller/CMakeLists.txt

@@ -55,7 +55,8 @@ target_link_libraries(${library_name} PRIVATE
 add_library(nav2_frenet_ilqr_controller_costs SHARED
   src/costs/lateral_distance_cost.cpp
   src/costs/longtitutal_velocity_cost.cpp
-  src/costs/obstacle_cost.cpp)
+  src/costs/obstacle_cost.cpp
+  src/costs/angular_velocity_cost.cpp)
 
 target_include_directories(nav2_frenet_ilqr_controller_costs
   PUBLIC

nav2_frenet_ilqr_controller/costs.xml

@@ -13,5 +13,9 @@
       <description>frenet trajectory planner cost checker for obstacle costs</description>
     </class>
 
+    <class type="nav2_frenet_ilqr_controller::costs::AngularVelocityCost" base_class_type="nav2_frenet_ilqr_controller::costs::RclcppNodeCost">
+      <description>frenet trajectory planner cost checker for angular velocity costs</description>
+    </class>
+
   </library>
 </class_libraries>

nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/costs/angular_velocity_cost.hpp

@@ -0,0 +1,42 @@
+#ifndef NAV2_FRENET_ILQR_CONTROLLER__COSTS__ANGULAR_VELOCITY_COST_HPP_
+#define NAV2_FRENET_ILQR_CONTROLLER__COSTS__ANGULAR_VELOCITY_COST_HPP_
+
+#include <memory>
+#include "nav2_util/node_utils.hpp"
+#include "nav2_frenet_ilqr_controller/costs/rclcpp_node_cost.hpp"
+
+using nav2_util::declare_parameter_if_not_declared;
+using rcl_interfaces::msg::ParameterType;
+
+namespace nav2_frenet_ilqr_controller
+{
+namespace costs
+{
+
+class AngularVelocityCost : public RclcppNodeCost
+{
+
+public:
+  AngularVelocityCost();
+  void initialize(
+    const std::string & cost_plugin_name,
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & node,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
+  double cost(
+    const FrenetTrajectory & frenet_trajectory,
+    const CartesianTrajectory & cartesian_trajectory) override;
+
+protected:
+  double K_angular_velocity_;
+};
+
+}
+}
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  nav2_frenet_ilqr_controller::costs::AngularVelocityCost,
+  nav2_frenet_ilqr_controller::costs::RclcppNodeCost)
+
+#endif

nav2_frenet_ilqr_controller/src/costs/angular_velocity_cost.cpp

@@ -0,0 +1,63 @@
+#include "nav2_frenet_ilqr_controller/costs/angular_velocity_cost.hpp"
+
+namespace nav2_frenet_ilqr_controller
+{
+namespace costs
+{
+
+AngularVelocityCost::AngularVelocityCost()
+: RclcppNodeCost()
+{
+}
+
+void AngularVelocityCost::initialize(
+  const std::string & cost_plugin_name,
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  RclcppNodeCost::initialize(cost_plugin_name, parent, costmap_ros);
+
+  auto node = parent.lock();
+
+  declare_parameter_if_not_declared(
+    node, cost_plugin_name_ + ".K_angular_velocity", rclcpp::ParameterValue(2.0));
+
+  node->get_parameter(
+    cost_plugin_name_ + ".K_angular_velocity",
+    K_angular_velocity_);
+
+  RCLCPP_INFO(
+    node->get_logger(), "%s.K_angular_velocity : %f",
+    cost_plugin_name_.c_str(), K_angular_velocity_);
+}
+
+double AngularVelocityCost::cost(
+  const FrenetTrajectory & /*frenet_trajectory*/,
+  const CartesianTrajectory & cartesian_trajectory)
+{
+  double trajectory_cost = 0;
+
+  if (cartesian_trajectory.size() < 2) {
+    return 0.0;
+  }
+
+  for (auto cartesian_state_it = cartesian_trajectory.begin();
+    cartesian_state_it != std::prev(cartesian_trajectory.end()); ++cartesian_state_it)
+  {
+    const double & theta1 = (*cartesian_state_it)[6];
+    const double & theta2 = (*std::next(cartesian_state_it))[6];
+
+    trajectory_cost += std::abs(theta1 - theta2);
+  }
+
+  return K_angular_velocity_ * trajectory_cost / cartesian_trajectory.size();
+}
+
+}
+}
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  nav2_frenet_ilqr_controller::costs::AngularVelocityCost,
+  nav2_frenet_ilqr_controller::costs::RclcppNodeCost)

nav2_frenet_ilqr_controller/src/costs/obstacle_cost.cpp

@@ -19,7 +19,7 @@ void ObstacleCost::initialize(
 
   auto node = parent.lock();
   declare_parameter_if_not_declared(
-    node, cost_plugin_name_ + ".K_obstacle", rclcpp::ParameterValue(2));
+    node, cost_plugin_name_ + ".K_obstacle", rclcpp::ParameterValue(2.0));
 
   node->get_parameter(
     cost_plugin_name_ + ".K_obstacle",

nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp

@@ -275,11 +275,6 @@ geometry_msgs::msg::TwistStamped FrenetILQRController::computeVelocityCommands(
     robot_cartesian_state,
     waypoint_list);
 
-  // get yaw angles from velocities along x axis and y axis in cartesian coordinate system
-  for (auto & cartesian_state : planned_cartesian_trajectory) {
-    cartesian_state[6] = std::atan2(cartesian_state[4], cartesian_state[1]);
-  }
-
   nav_msgs::msg::Path frenet_plan = convertFromCartesianTrajectory(
     transformed_plan.header.frame_id,
     planned_cartesian_trajectory);