Applied best practices in Cpp (#31)

Author: CihatAltiparmak

frenet_trajectory_planner/demos/plan_through_waypoint_demo.cpp

@@ -363,7 +363,7 @@ int main()
   auto frenet_trajectory_planner = frenet_trajectory_planner::FrenetTrajectoryPlanner();
   auto planned_cartesian_trajectory = frenet_trajectory_planner.planByWaypoint(
     robot_cartesian_state,
-    truncated_waypoint_list, 1.0);
+    truncated_waypoint_list);
 
   for (const auto & cartesian_state : planned_cartesian_trajectory) {
     std::cout << cartesian_state[0] << ", " << cartesian_state[3] << std::endl;

frenet_trajectory_planner/include/frenet_trajectory_planner/frenet_frame_converter.hpp

@@ -37,7 +37,7 @@ void FrenetFrameConverter::createSegments(const std::vector<CartesianPoint> & wa
   // assert that waypoint_list size must be at least 2
   CartesianPoint last_waypoint = waypoint_list[0];
   for (auto waypoint_it = waypoint_list.begin() + 1; (waypoint_it + 1) != waypoint_list.end();
-    waypoint_it++)
+    ++waypoint_it)
   {
     CartesianPoint qim1 = last_waypoint;
     CartesianPoint qi = *(waypoint_it);
@@ -98,7 +98,7 @@ CartesianTrajectory FrenetFrameConverter::convertFrenet2Cartesian(
       current_longitutal_length + segments_.at(current_segment_index)->getArclength())
     {
       current_longitutal_length += segments_.at(current_segment_index)->getArclength();
-      current_segment_index++;
+      ++current_segment_index;
 
       if (current_segment_index >= segments_.size()) {
         return cartesian_trajectory;

frenet_trajectory_planner/include/frenet_trajectory_planner/frenet_trajectory_planner.hpp

@@ -24,8 +24,7 @@ class FrenetTrajectoryPlanner
 
   CartesianTrajectory planByWaypoint(
     const CartesianState & robot_cartesian_state,
-    const std::vector<CartesianPoint> & waypoint_list,
-    const double max_time_per_point);
+    const std::vector<CartesianPoint> & waypoint_list);
 
   void addPolicy(const std::shared_ptr<policies::Policy> & policy);
   void addCost(const std::shared_ptr<costs::Cost> & cost);
@@ -63,8 +62,7 @@ FrenetTrajectoryPlanner::FrenetTrajectoryPlanner(
 
 CartesianTrajectory FrenetTrajectoryPlanner::planByWaypoint(
   const CartesianState & robot_cartesian_state,
-  const std::vector<CartesianPoint> & waypoint_list,
-  const double max_time_per_point)
+  const std::vector<CartesianPoint> & waypoint_list)
 {
   auto frenet_frame_converter = std::make_shared<FrenetFrameConverter>();
   frenet_frame_converter->createSegments(waypoint_list);
@@ -76,7 +74,7 @@ CartesianTrajectory FrenetTrajectoryPlanner::planByWaypoint(
     frenet_frame_converter->convertCartesian2FrenetForSegment(robot_cartesian_state, 0);
 
   FrenetTrajectory planned_frenet_trajectory = {};
-  for (int i = 0; i < frenet_trajectory_planner_config_.number_of_time_intervals; i++) {
+  for (int i = 0; i < frenet_trajectory_planner_config_.number_of_time_intervals; ++i) {
     // TODO (CihatAltiparmak) : eliminate some trajectories in frenet level
     auto all_frenet_trajectories =
       frenet_trajectory_generator_->getAllPossibleFrenetTrajectories(

ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp

@@ -45,13 +45,13 @@ class NewtonOptimizer : public Optimizer
     const std::vector<typename RobotModel::StateT> & x_tracked,
     const std::vector<typename RobotModel::StateT> & x_trajectory);
 
-  void setIterationNumber(const int iteration_number);
+  void setIterationNumber(const size_t iteration_number);
   void setAlpha(const double alpha);
 
 private:
   RobotModel robot_model_;
   double alpha_;
-  int iteration_number_;
+  size_t iteration_number_;
 };
 
 template<typename RobotModel>
@@ -67,18 +67,18 @@ std::vector<MatrixXd> NewtonOptimizer<RobotModel>::backwardPass(
   const MatrixXd & R, const double dt)
 {
 
-  auto trajectory_size = x_feasible.size();
   auto state_dimension = Q.rows();
   auto input_dimension = R.rows();
 
   MatrixXd P_tilda = MatrixXd::Identity(state_dimension + 1, state_dimension + 1);
   P_tilda.topLeftCorner(state_dimension, state_dimension) = Q;
 
-  std::vector<MatrixXd> K_gain(trajectory_size);
+  std::vector<MatrixXd> K_gain(x_feasible.size(),
+    MatrixXd::Zero(input_dimension, state_dimension + 1));
 
-  for (int i = trajectory_size - 2; i >= 0; i--) {
+  for (auto i = std::distance(x_feasible.begin(), std::prev(x_feasible.end(), 2)); i >= 0; i--) {
     auto x_offset =
-      robot_model_.applySystemDynamics(x_feasible[i], u_feasible[i], dt) - x_feasible[i + 1];
+      robot_model_.applySystemDynamics(x_feasible.at(i), u_feasible[i], dt) - x_feasible[i + 1];
 
     MatrixXd A_tilda = MatrixXd::Identity(state_dimension + 1, state_dimension + 1);
     auto A = robot_model_.getStateMatrix(x_feasible[i], u_feasible[i], dt);
@@ -117,7 +117,10 @@ std::tuple<std::vector<typename RobotModel::StateT>,
 
   // assert trajectory_size > 0
   x_tracked[0] = x_feasible[0];
-  for (int i = 0; i < trajectory_size - 1; i++) {
+  for (typename std::vector<typename RobotModel::StateT>::difference_type i = 0,
+    max_difference = std::distance(x_feasible.begin(), std::prev(x_feasible.end(), 1));
+    i < max_difference; ++i)
+  {
     auto x_error = x_tracked[i] - x_feasible[i];
     Vector<double, 4> z_error;
     z_error << x_error, alpha;
@@ -149,17 +152,19 @@ std::vector<typename RobotModel::InputT> NewtonOptimizer<RobotModel>::optimize(
   const Matrix2d & R, const double dt)
 {
   // assert trajectory_size > 0
-  const auto trajectory_size = x_trajectory.size();
+
   double alpha = alpha_;
 
   std::vector<typename RobotModel::StateT> x_best_trajectory;
   std::vector<typename RobotModel::InputT> u_best_trajectory;
-  std::vector<typename RobotModel::InputT> u_optimized(trajectory_size - 1,
+  std::vector<typename RobotModel::InputT> u_optimized(std::distance(
+      x_trajectory.begin(),
+      std::prev(x_trajectory.end(), 1)),
     RobotModel::InputT::Zero());
 
   double best_trajectory_cost = std::numeric_limits<double>::infinity();
   double previous_best_trajectory_cost = best_trajectory_cost;
-  for (int i = 0; i < iteration_number_; i++) {
+  for (size_t i = 0; i < iteration_number_; ++i) {
     auto K_gain_list = this->backwardPass(x_trajectory, u_optimized, Q, R, dt);
     auto [x_tracked, u_tracked] = this->forwardPass(
       x_trajectory, u_optimized, K_gain_list, dt,
@@ -191,18 +196,18 @@ double NewtonOptimizer<RobotModel>::cost(
   const std::vector<typename RobotModel::StateT> & x_tracked,
   const std::vector<typename RobotModel::StateT> & x_trajectory)
 {
-
-  auto trajectory_size = x_trajectory.size();
   double trajectory_cost = 0;
-  for (size_t i = 0; i < trajectory_size; i++) {
+  for (typename std::vector<typename RobotModel::StateT>::size_type i = 0; i < x_trajectory.size();
+    ++i)
+  {
     trajectory_cost += (x_tracked[i] - x_trajectory[i]).squaredNorm();
   }
 
   return trajectory_cost;
 }
 
 template<typename RobotModel>
-void NewtonOptimizer<RobotModel>::setIterationNumber(const int iteration_number)
+void NewtonOptimizer<RobotModel>::setIterationNumber(const size_t iteration_number)
 {
   iteration_number_ = iteration_number;
 }

nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp

@@ -273,7 +273,7 @@ geometry_msgs::msg::TwistStamped FrenetILQRController::computeVelocityCommands(
     params_->frenet_trajectory_planner_config);
   auto planned_cartesian_trajectory = frenet_trajectory_planner_.planByWaypoint(
     robot_cartesian_state,
-    waypoint_list, 1.0);
+    waypoint_list);
 
   // get yaw angles from velocities along x axis and y axis in cartesian coordinate system
   for (auto & cartesian_state : planned_cartesian_trajectory) {

nav2_frenet_ilqr_controller/src/parameter_handler.cpp

@@ -173,9 +173,17 @@ ParameterHandler::dynamicParametersCallback(
     } else if (name == plugin_name_ + ".frenet_trajectory_planner.time_interval") {
       params_.frenet_trajectory_planner_config.time_interval = parameter.as_double();
     } else if (name == plugin_name_ + ".frenet_trajectory_planner.number_of_time_intervals") {
-      params_.frenet_trajectory_planner_config.number_of_time_intervals = parameter.as_int();
+      if (parameter.as_int() < 1) {
+        params_.frenet_trajectory_planner_config.number_of_time_intervals = 2;
+      } else {
+        params_.frenet_trajectory_planner_config.number_of_time_intervals = parameter.as_int();
+      }
     } else if (name == plugin_name_ + ".ilqr_trajectory_tracker.iteration_number") {
-      params_.iteration_number = parameter.as_int();
+      if (parameter.as_int() < 0) {
+        params_.iteration_number = 20;
+      } else {
+        params_.iteration_number = parameter.as_int();
+      }
     } else if (name == plugin_name_ + ".ilqr_trajectory_tracker.alpha") {
       params_.alpha = parameter.as_double();
     }