Enable lints in motion_velocity_optimizer (autowarefoundation#148)

harihitode · Dec 9, 2020 · 10a90f1 · 10a90f1
1 parent 6c82f9e
commit 10a90f1
Show file tree

Hide file tree

Showing 15 changed files with 436 additions and 267 deletions.
diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/CMakeLists.txt b/planning/scenario_planning/common/motion_velocity_optimizer/CMakeLists.txt
@@ -5,6 +5,8 @@ project(motion_velocity_optimizer)
 
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -30,11 +32,16 @@ set(MOTION_VELOCITY_OPTIMIZER_HDR
   include/motion_velocity_optimizer/optimizer/optimizer_base.hpp
 )
 
-ament_auto_add_executable(motion_velocity_optimizer 
-  ${MOTION_VELOCITY_OPTIMIZER_SRC} 
+ament_auto_add_executable(motion_velocity_optimizer
+  ${MOTION_VELOCITY_OPTIMIZER_SRC}
   ${MOTION_VELOCITY_OPTIMIZER_HDR}
 )
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
 ament_auto_package(
   INSTALL_TO_SHARE
     launch

diff --git a/...anning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/interpolate.hpp b/...anning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/interpolate.hpp
@@ -12,16 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#pragma once
+#ifndef MOTION_VELOCITY_OPTIMIZER__INTERPOLATE_HPP_
+#define MOTION_VELOCITY_OPTIMIZER__INTERPOLATE_HPP_
+
 #include <cmath>
 #include <iostream>
 #include <vector>
 
 class LinearInterpolate
 {
 public:
-  LinearInterpolate(){};
-  ~LinearInterpolate(){};
+  LinearInterpolate() {}
+  ~LinearInterpolate() {}
   static bool interpolate(
     const std::vector<double> & base_index, const std::vector<double> & base_value,
     const std::vector<double> & return_index, std::vector<double> & return_value);
@@ -37,7 +39,7 @@ class SplineInterpolate
 
 public:
   SplineInterpolate();
-  SplineInterpolate(const std::vector<double> & x);
+  explicit SplineInterpolate(const std::vector<double> & x);
   ~SplineInterpolate();
   void generateSpline(const std::vector<double> & x);
   double getValue(const double & s);
@@ -46,3 +48,5 @@ class SplineInterpolate
     const std::vector<double> & return_index, std::vector<double> & return_value);
   void getValueVector(const std::vector<double> & s_v, std::vector<double> & value_v);
 };
+
+#endif  // MOTION_VELOCITY_OPTIMIZER__INTERPOLATE_HPP_
diff --git a/...motion_velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer.hpp b/...motion_velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer.hpp
@@ -11,34 +11,46 @@
 // 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 "autoware_planning_msgs/msg/trajectory.hpp"
-#include "autoware_planning_msgs/msg/velocity_limit.hpp"
-#include "autoware_debug_msgs/msg/float32_stamped.hpp"
-#include "geometry_msgs/msg/twist_stamped.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "autoware_debug_msgs/msg/float32_multi_array_stamped.hpp"
-#include "tf2/utils.h"
-#include "tf2_ros/transform_listener.h"
+
+#ifndef MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_HPP_
+#define MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_HPP_
+
 #include <iostream>
+#include <memory>
 #include <mutex>
 #include <string>
+#include <vector>
 
 #include "motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp"
 #include "motion_velocity_optimizer/optimizer/optimizer_base.hpp"
 
+#include "autoware_debug_msgs/msg/float32_multi_array_stamped.hpp"
+#include "autoware_debug_msgs/msg/float32_stamped.hpp"
+#include "autoware_planning_msgs/msg/trajectory.hpp"
+#include "autoware_planning_msgs/msg/velocity_limit.hpp"
 #include "osqp_interface/osqp_interface.hpp"
 
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2/utils.h"
+#include "tf2_ros/transform_listener.h"
+
 class MotionVelocityOptimizer : public rclcpp::Node
 {
 public:
   MotionVelocityOptimizer();
   ~MotionVelocityOptimizer();
 
 private:
-  rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_; //!< @brief publisher for output trajectory
-  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_current_velocity_;  //!< @brief subscriber for current velocity
-  rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_current_trajectory_;  //!< @brief subscriber for reference trajectory
-  rclcpp::Subscription<autoware_planning_msgs::msg::VelocityLimit>::SharedPtr sub_external_velocity_limit_;//!< @brief subscriber for external velocity limit
+  // publisher for output trajectory
+  rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_;
+  // subscriber for current velocity
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_current_velocity_;
+  // subscriber for reference trajectory
+  rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_current_trajectory_;
+  rclcpp::Subscription<autoware_planning_msgs::msg::VelocityLimit>::SharedPtr
+  // subscriber for external velocity limit
+    sub_external_velocity_limit_;
   tf2::BufferCore tf_buffer_;  //!< @brief tf butter
   tf2_ros::TransformListener tf_listener_;  //!< @brief tf listener
 
@@ -48,15 +60,20 @@ class MotionVelocityOptimizer : public rclcpp::Node
   std::mutex mutex_;
 
 
-  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_ptr_;           // current vehicle pose
-  geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_;      // current vehicle twist
-  autoware_planning_msgs::msg::Trajectory::ConstSharedPtr base_traj_raw_ptr_;  // current base_waypoints
-  autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr external_velocity_limit_ptr_;  // current external_velocity_limit
+  // current vehicle pose
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_ptr_;
+  // current vehicle twist
+  geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_;
+  // current base_waypoints
+  autoware_planning_msgs::msg::Trajectory::ConstSharedPtr base_traj_raw_ptr_;
+  // current external_velocity_limit
+  autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr external_velocity_limit_ptr_;
   std::shared_ptr<double> external_velocity_limit_filtered_;
 
   autoware_planning_msgs::msg::Trajectory prev_output_;  // previously published trajectory
 
-  enum class InitializeType {
+  enum class InitializeType
+  {
     INIT = 0,
     LARGE_DEVIATION_REPLAN = 1,
     ENGAGING = 2,
@@ -96,7 +113,8 @@ class MotionVelocityOptimizer : public rclcpp::Node
   /* topic callback */
   void callbackCurrentVelocity(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg);
   void callbackCurrentTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
-  void callbackExternalVelocityLimit(const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg);
+  void callbackExternalVelocityLimit(
+    const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg);
   void timerCallback();
 
 
@@ -113,7 +131,8 @@ class MotionVelocityOptimizer : public rclcpp::Node
 
   autoware_planning_msgs::msg::Trajectory optimizeVelocity(
     const autoware_planning_msgs::msg::Trajectory & input, const int input_closest,
-    const autoware_planning_msgs::msg::Trajectory & prev_output_traj, const int prev_output_closest);
+    const autoware_planning_msgs::msg::Trajectory & prev_output_traj,
+    const int prev_output_closest);
 
   void calcInitialMotion(
     const double & base_speed, const autoware_planning_msgs::msg::Trajectory & base_waypoints,
@@ -157,12 +176,19 @@ class MotionVelocityOptimizer : public rclcpp::Node
 
   /* debug */
 
-  rclcpp::Publisher<autoware_debug_msgs::msg::Float32Stamped>::SharedPtr pub_dist_to_stopline_;  //!< @brief publisher for stop distance
+  // publisher for stop distance
+  rclcpp::Publisher<autoware_debug_msgs::msg::Float32Stamped>::SharedPtr pub_dist_to_stopline_;
   rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_raw_;
   rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_vel_lim_;
-  rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_latcc_filtered_;
+  rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr
+    pub_trajectory_latcc_filtered_;
   rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_resampled_;
   rclcpp::Publisher<autoware_debug_msgs::msg::Float32Stamped>::SharedPtr debug_closest_velocity_;
   rclcpp::Publisher<autoware_debug_msgs::msg::Float32Stamped>::SharedPtr debug_closest_acc_;
-  void publishFloat(const double & data, const rclcpp::Publisher<autoware_debug_msgs::msg::Float32Stamped>::SharedPtr pub) const;
+  void publishFloat(
+    const double & data,
+    const rclcpp::Publisher<autoware_debug_msgs::msg::Float32Stamped>::SharedPtr pub)
+  const;
 };
+
+#endif  // MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_HPP_
diff --git a/..._velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp b/..._velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp
@@ -12,7 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#ifndef MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_UTILS_HPP_
+#define MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_UTILS_HPP_
+
 #include <iostream>
+#include <vector>
 
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -32,12 +36,15 @@ double calcSquaredDist2d(
   const autoware_planning_msgs::msg::TrajectoryPoint & b);
 double calcDist2d(const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b);
 double calcDist2d(const geometry_msgs::msg::Pose & a, const geometry_msgs::msg::Pose & b);
-double calcDist2d(const geometry_msgs::msg::PoseStamped & a, const geometry_msgs::msg::PoseStamped & b);
+double calcDist2d(
+  const geometry_msgs::msg::PoseStamped & a,
+  const geometry_msgs::msg::PoseStamped & b);
 double calcDist2d(
   const autoware_planning_msgs::msg::TrajectoryPoint & a,
   const autoware_planning_msgs::msg::TrajectoryPoint & b);
 int calcClosestWaypoint(
-  const autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & point);
+  const autoware_planning_msgs::msg::Trajectory & trajectory,
+  const geometry_msgs::msg::Point & point);
 int calcClosestWaypoint(
   const autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Pose & pose,
   const double delta_yaw_threshold);
@@ -54,8 +61,12 @@ void calcTrajectoryIntervalDistance(
 void setZeroVelocity(autoware_planning_msgs::msg::Trajectory & trajectory);
 double getMaxVelocity(const autoware_planning_msgs::msg::Trajectory & trajectory);
 double getMaxAbsVelocity(const autoware_planning_msgs::msg::Trajectory & trajectory);
-void mininumVelocityFilter(const double & min_vel, autoware_planning_msgs::msg::Trajectory & trajectory);
-void maximumVelocityFilter(const double & max_vel, autoware_planning_msgs::msg::Trajectory & trajectory);
+void mininumVelocityFilter(
+  const double & min_vel,
+  autoware_planning_msgs::msg::Trajectory & trajectory);
+void maximumVelocityFilter(
+  const double & max_vel,
+  autoware_planning_msgs::msg::Trajectory & trajectory);
 void multiplyConstantToTrajectoryVelocity(
   const double & scalar, autoware_planning_msgs::msg::Trajectory & trajectory);
 void insertZeroVelocityAfterIdx(
@@ -70,6 +81,9 @@ void convertEulerAngleToMonotonic(std::vector<double> & a);
 geometry_msgs::msg::Quaternion getQuaternionFromYaw(double yaw);
 bool linearInterpTrajectory(
   const std::vector<double> & base_index,
-  const autoware_planning_msgs::msg::Trajectory & base_trajectory, const std::vector<double> & out_index,
+  const autoware_planning_msgs::msg::Trajectory & base_trajectory,
+  const std::vector<double> & out_index,
   autoware_planning_msgs::msg::Trajectory & out_trajectory);
-}  // namespace vpu
+}  // namespace vpu
+
+#endif  // MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_UTILS_HPP_
diff --git a/...locity_optimizer/include/motion_velocity_optimizer/optimizer/l2_pseudo_jerk_optimizer.hpp b/...locity_optimizer/include/motion_velocity_optimizer/optimizer/l2_pseudo_jerk_optimizer.hpp
@@ -12,16 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MOTION_VELOCITY_OPTIMIZER_L2_PSEUDO_JERK_OPTIMIZER_HPP
-#define MOTION_VELOCITY_OPTIMIZER_L2_PSEUDO_JERK_OPTIMIZER_HPP
+#ifndef MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__L2_PSEUDO_JERK_OPTIMIZER_HPP_
+#define MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__L2_PSEUDO_JERK_OPTIMIZER_HPP_
 
-#include "autoware_planning_msgs/msg/trajectory.hpp"
-#include "osqp_interface/osqp_interface.hpp"
-#include "rclcpp/rclcpp.hpp"
 #include <vector>
 
 #include "motion_velocity_optimizer/optimizer/optimizer_base.hpp"
 
+#include "autoware_planning_msgs/msg/trajectory.hpp"
+#include "osqp_interface/osqp_interface.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
 class L2PseudoJerkOptimizer : public OptimizerBase
 {
 public:
@@ -34,8 +36,8 @@ class L2PseudoJerkOptimizer : public OptimizerBase
     double over_a_weight;
   };
 
-public:
   explicit L2PseudoJerkOptimizer(const OptimizerParam & p);
+
   bool solve(
     const double initial_vel, const double initial_acc, const int closest,
     const autoware_planning_msgs::msg::Trajectory & input,
@@ -50,4 +52,4 @@ class L2PseudoJerkOptimizer : public OptimizerBase
   osqp::OSQPInterface qp_solver_;
 };
 
-#endif  // MOTION_VELOCITY_OPTIMIZER_L2_PSEUDO_JERK_OPTIMIZER_HPP
+#endif  // MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__L2_PSEUDO_JERK_OPTIMIZER_HPP_
diff --git a/...city_optimizer/include/motion_velocity_optimizer/optimizer/linf_pseudo_jerk_optimizer.hpp b/...city_optimizer/include/motion_velocity_optimizer/optimizer/linf_pseudo_jerk_optimizer.hpp
@@ -12,15 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MOTION_VELOCITY_OPTIMIZER_LINF_PSEUDO_JERK_OPTIMIZER_HPP
-#define MOTION_VELOCITY_OPTIMIZER_LINF_PSEUDO_JERK_OPTIMIZER_HPP
+#ifndef MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__LINF_PSEUDO_JERK_OPTIMIZER_HPP_
+#define MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__LINF_PSEUDO_JERK_OPTIMIZER_HPP_
+
+#include <vector>
+
+#include "motion_velocity_optimizer/optimizer/optimizer_base.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "osqp_interface/osqp_interface.hpp"
+
 #include "rclcpp/rclcpp.hpp"
-#include <vector>
 
-#include "motion_velocity_optimizer/optimizer/optimizer_base.hpp"
 
 class LinfPseudoJerkOptimizer : public OptimizerBase
 {
@@ -34,8 +37,9 @@ class LinfPseudoJerkOptimizer : public OptimizerBase
     double over_a_weight;
   };
 
-public:
   explicit LinfPseudoJerkOptimizer(const OptimizerParam & p);
+  virtual ~LinfPseudoJerkOptimizer() = default;
+
   bool solve(
     const double initial_vel, const double initial_acc, const int closest,
     const autoware_planning_msgs::msg::Trajectory & input,
@@ -50,4 +54,4 @@ class LinfPseudoJerkOptimizer : public OptimizerBase
   osqp::OSQPInterface qp_solver_;
 };
 
-#endif  // MOTION_VELOCITY_OPTIMIZER_LINF_PSEUDO_JERK_OPTIMIZER_HPP
+#endif  // MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__LINF_PSEUDO_JERK_OPTIMIZER_HPP_
diff --git a/.../motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/optimizer_base.hpp b/.../motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/optimizer_base.hpp
@@ -12,12 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MOTION_VELOCITY_OPTIMIZER_OPTIMIZER_BASE_HPP
-#define MOTION_VELOCITY_OPTIMIZER_OPTIMIZER_BASE_HPP
-#include "autoware_planning_msgs/msg/trajectory.hpp"
+#ifndef MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__OPTIMIZER_BASE_HPP_
+#define MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__OPTIMIZER_BASE_HPP_
+
 #include <limits>
 #include <vector>
 
+#include "autoware_planning_msgs/msg/trajectory.hpp"
+
 class OptimizerBase
 {
 public:
@@ -29,6 +31,8 @@ class OptimizerBase
   virtual void setAccel(const double max_accel) = 0;
 
   virtual void setDecel(const double min_decel) = 0;
+
+  virtual ~OptimizerBase() = default;
 };
 
-#endif  // MOTION_VELOCITY_OPTIMIZER_OPTIMIZER_BASE_HPP
+#endif  // MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__OPTIMIZER_BASE_HPP_
diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/package.xml b/planning/scenario_planning/common/motion_velocity_optimizer/package.xml
@@ -11,6 +11,9 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>