diff --git a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/box2d.hpp b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/box2d.hpp
index abba42a56c1d0..1c17e49582c5d 100644
--- a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/box2d.hpp
+++ b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/box2d.hpp
@@ -1,120 +1,135 @@
-#ifndef OBSTACLE_VELOCITY_PLANNER_BOX2D_HPP_
-#define OBSTACLE_VELOCITY_PLANNER_BOX2D_HPP_
+// Copyright 2022 Tier IV, Inc.
+//
+// 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 OBSTACLE_VELOCITY_PLANNER__BOX2D_HPP_
+#define OBSTACLE_VELOCITY_PLANNER__BOX2D_HPP_
 
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
 #include <cmath>
+#include <limits>
 #include <vector>
 
 class Box2d
 {
 public:
-    Box2d(const geometry_msgs::msg::Pose & center_pose, const double length, const double width);
+  Box2d(const geometry_msgs::msg::Pose & center_pose, const double length, const double width);
 
-    bool HasOverlap(const Box2d & box) const ;
+  bool HasOverlap(const Box2d & box) const;
 
-    void InitCorners();
+  void InitCorners();
 
-    /**
+  /**
    * @brief Getter of the center of the box
    * @return The center of the box
    */
-    const geometry_msgs::msg::Point& center() const { return center_; }
-
-    /**
-     * @brief Getter of the x-coordinate of the center of the box
-     * @return The x-coordinate of the center of the box
-     */
-    double center_x() const { return center_.x; }
-
-    /**
-     * @brief Getter of the y-coordinate of the center of the box
-     * @return The y-coordinate of the center of the box
-     */
-    double center_y() const { return center_.y; }
-
-    /**
-     * @brief Getter of the length
-     * @return The length of the heading-axis
-     */
-    double length() const { return length_; }
-
-    /**
-     * @brief Getter of the width
-     * @return The width of the box taken perpendicularly to the heading
-     */
-    double width() const { return width_; }
-
-    /**
-     * @brief Getter of half the length
-     * @return Half the length of the heading-axis
-     */
-    double half_length() const { return half_length_; }
-
-    /**
-     * @brief Getter of half the width
-     * @return Half the width of the box taken perpendicularly to the heading
-     */
-    double half_width() const { return half_width_; }
-
-    /**
-     * @brief Getter of the heading
-     * @return The counter-clockwise angle between the x-axis and the heading-axis
-     */
-    double heading() const { return heading_; }
-
-    /**
-     * @brief Getter of the cosine of the heading
-     * @return The cosine of the heading
-     */
-    double cos_heading() const { return cos_heading_; }
-
-    /**
-     * @brief Getter of the sine of the heading
-     * @return The sine of the heading
-     */
-    double sin_heading() const { return sin_heading_; }
-
-    /**
+  const geometry_msgs::msg::Point & center() const { return center_; }
+
+  /**
+   * @brief Getter of the x-coordinate of the center of the box
+   * @return The x-coordinate of the center of the box
+   */
+  double center_x() const { return center_.x; }
+
+  /**
+   * @brief Getter of the y-coordinate of the center of the box
+   * @return The y-coordinate of the center of the box
+   */
+  double center_y() const { return center_.y; }
+
+  /**
+   * @brief Getter of the length
+   * @return The length of the heading-axis
+   */
+  double length() const { return length_; }
+
+  /**
+   * @brief Getter of the width
+   * @return The width of the box taken perpendicularly to the heading
+   */
+  double width() const { return width_; }
+
+  /**
+   * @brief Getter of half the length
+   * @return Half the length of the heading-axis
+   */
+  double half_length() const { return half_length_; }
+
+  /**
+   * @brief Getter of half the width
+   * @return Half the width of the box taken perpendicularly to the heading
+   */
+  double half_width() const { return half_width_; }
+
+  /**
+   * @brief Getter of the heading
+   * @return The counter-clockwise angle between the x-axis and the heading-axis
+   */
+  double heading() const { return heading_; }
+
+  /**
+   * @brief Getter of the cosine of the heading
+   * @return The cosine of the heading
+   */
+  double cos_heading() const { return cos_heading_; }
+
+  /**
+   * @brief Getter of the sine of the heading
+   * @return The sine of the heading
+   */
+  double sin_heading() const { return sin_heading_; }
+
+  /**
    * @brief Getter of the area of the box
    * @return The product of its length and width
    */
-    double area() const { return length_ * width_; }
+  double area() const { return length_ * width_; }
 
-    /**
-     * @brief Getter of the size of the diagonal of the box
-     * @return The diagonal size of the box
-     */
-    double diagonal() const { return std::hypot(length_, width_); }
+  /**
+   * @brief Getter of the size of the diagonal of the box
+   * @return The diagonal size of the box
+   */
+  double diagonal() const { return std::hypot(length_, width_); }
 
-    /**
-     * @brief Getter of the corners of the box
-     * @param corners The vector where the corners are listed
-     */
-    std::vector<geometry_msgs::msg::Point> GetAllCorners() const { return corners_; }
+  /**
+   * @brief Getter of the corners of the box
+   * @param corners The vector where the corners are listed
+   */
+  std::vector<geometry_msgs::msg::Point> GetAllCorners() const { return corners_; }
 
-    double max_x() const { return max_x_; }
-    double min_x() const { return min_x_; }
-    double max_y() const { return max_y_; }
-    double min_y() const { return min_y_; }
+  double max_x() const { return max_x_; }
+  double min_x() const { return min_x_; }
+  double max_y() const { return max_y_; }
+  double min_y() const { return min_y_; }
 
 private:
-    geometry_msgs::msg::Point center_;
-    double length_ = 0.0;
-    double width_ = 0.0;
-    double half_length_ = 0.0;
-    double half_width_ = 0.0;
-    double heading_ = 0.0;
-    double cos_heading_ = 1.0;
-    double sin_heading_ = 0.0;
-
-    std::vector<geometry_msgs::msg::Point> corners_;
-
-    double max_x_ = std::numeric_limits<double>::lowest();
-    double min_x_ = std::numeric_limits<double>::max();
-    double max_y_ = std::numeric_limits<double>::lowest();
-    double min_y_ = std::numeric_limits<double>::max();
+  geometry_msgs::msg::Point center_;
+  double length_ = 0.0;
+  double width_ = 0.0;
+  double half_length_ = 0.0;
+  double half_width_ = 0.0;
+  double heading_ = 0.0;
+  double cos_heading_ = 1.0;
+  double sin_heading_ = 0.0;
+
+  std::vector<geometry_msgs::msg::Point> corners_;
+
+  double max_x_ = std::numeric_limits<double>::lowest();
+  double min_x_ = std::numeric_limits<double>::max();
+  double max_y_ = std::numeric_limits<double>::lowest();
+  double min_y_ = std::numeric_limits<double>::max();
 };
 
-#endif // OBSTACLE_VELOCITY_PLANNER_BOX2D_HPP_
+#endif  // OBSTACLE_VELOCITY_PLANNER__BOX2D_HPP_
diff --git a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common/s_boundary.hpp b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common/s_boundary.hpp
index e9e2a523e8094..959a29699b4b9 100644
--- a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common/s_boundary.hpp
+++ b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common/s_boundary.hpp
@@ -11,8 +11,8 @@
 // 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 OBSTACLE_AVOIDANCE_PLANNER_ST_BOUNDARY_HPP_
-#define OBSTACLE_AVOIDANCE_PLANNER_ST_BOUNDARY_HPP_
+#ifndef OBSTACLE_VELOCITY_PLANNER__COMMON__S_BOUNDARY_HPP_
+#define OBSTACLE_VELOCITY_PLANNER__COMMON__S_BOUNDARY_HPP_
 
 #include <limits>
 #include <vector>
@@ -20,14 +20,14 @@
 class SBoundary
 {
 public:
-    SBoundary(const double _max_s, const double _min_s) : max_s(_max_s), min_s(_min_s) {}
-    SBoundary() : max_s(std::numeric_limits<double>::max()), min_s(0.0) {}
+  SBoundary(const double _max_s, const double _min_s) : max_s(_max_s), min_s(_min_s) {}
+  SBoundary() : max_s(std::numeric_limits<double>::max()), min_s(0.0) {}
 
-    double max_s = std::numeric_limits<double>::max();
-    double min_s = 0.0;
-    bool is_object = false;
+  double max_s = std::numeric_limits<double>::max();
+  double min_s = 0.0;
+  bool is_object = false;
 };
 
 using SBoundaries = std::vector<SBoundary>;
 
-#endif // OBSTACLE_AVOIDANCE_PLANNER_ST_BOUNDARY_HPP_
+#endif  // OBSTACLE_VELOCITY_PLANNER__COMMON__S_BOUNDARY_HPP_
diff --git a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common/st_point.hpp b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common/st_point.hpp
index 2c3398a2ba6f6..fad8a85ee4571 100644
--- a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common/st_point.hpp
+++ b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common/st_point.hpp
@@ -11,21 +11,21 @@
 // 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 OBSTACLE_AVOIDANCE_PLANNER_ST_POINT_HPP_
-#define OBSTACLE_AVOIDANCE_PLANNER_ST_POINT_HPP_
+#ifndef OBSTACLE_VELOCITY_PLANNER__COMMON__ST_POINT_HPP_
+#define OBSTACLE_VELOCITY_PLANNER__COMMON__ST_POINT_HPP_
 
 #include <vector>
 
 class STPoint
 {
 public:
-    STPoint(const double _s, const double _t) : s(_s), t(_t) {}
-    STPoint() : s(0.0), t(0.0) {}
+  STPoint(const double _s, const double _t) : s(_s), t(_t) {}
+  STPoint() : s(0.0), t(0.0) {}
 
-    double s;
-    double t;
+  double s;
+  double t;
 };
 
 using STPoints = std::vector<STPoint>;
 
-#endif // OBSTACLE_AVOIDANCE_PLANNER_ST_POINT_HPP_
+#endif  // OBSTACLE_VELOCITY_PLANNER__COMMON__ST_POINT_HPP_
diff --git a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/node.hpp b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/node.hpp
index 8e7118058e56a..bfa8eb2921343 100644
--- a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/node.hpp
+++ b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/node.hpp
@@ -11,28 +11,29 @@
 // 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 OBSTACLE_VELOCITY_PLANNER_NODE_HPP_
-#define OBSTACLE_VELOCITY_PLANNER_NODE_HPP_
+
+#ifndef OBSTACLE_VELOCITY_PLANNER__NODE_HPP_
+#define OBSTACLE_VELOCITY_PLANNER__NODE_HPP_
 
 #include "obstacle_velocity_planner/box2d.hpp"
 #include "obstacle_velocity_planner/common/s_boundary.hpp"
 #include "obstacle_velocity_planner/common/st_point.hpp"
 #include "obstacle_velocity_planner/velocity_optimizer.hpp"
 
-#include <tier4_autoware_utils/ros/self_pose_listener.hpp>
-#include <tier4_autoware_utils/system/stop_watch.hpp>
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/self_pose_listener.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
-#include <tier4_debug_msgs/msg/float32_stamped.hpp>
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
-#include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
+#include <tier4_debug_msgs/msg/float32_stamped.hpp>
+#include <tier4_planning_msgs/msg/velocity_limit.hpp>
 
 #include <boost/optional.hpp>
 
@@ -67,7 +68,7 @@ struct ObjectData
 class ObstacleVelocityPlanner : public rclcpp::Node
 {
 public:
-  ObstacleVelocityPlanner(const rclcpp::NodeOptions & node_options);
+  explicit ObstacleVelocityPlanner(const rclcpp::NodeOptions & node_options);
 
 private:
   // Callback Functions
@@ -79,13 +80,14 @@ class ObstacleVelocityPlanner : public rclcpp::Node
 
   void trajectoryCallback(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg);
 
-  void smoothedTrajectoryCallback(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg);
+  void smoothedTrajectoryCallback(
+    const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg);
 
-  void onExternalVelocityLimit(
-    const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg);
+  void onExternalVelocityLimit(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg);
 
   // Member Functions
-  double getClosestStopDistance(const TrajectoryData & ego_traj_data, const std::vector<double> & resolutions);
+  double getClosestStopDistance(
+    const TrajectoryData & ego_traj_data, const std::vector<double> & resolutions);
 
   std::tuple<double, double> calcInitialMotion(
     const autoware_auto_planning_msgs::msg::Trajectory & input_traj, const size_t input_closest,
@@ -112,19 +114,25 @@ class ObstacleVelocityPlanner : public rclcpp::Node
     const std::vector<double> & query_index, const bool use_spline_for_pose = false);
 
   boost::optional<SBoundaries> getSBoundaries(
-    const geometry_msgs::msg::Pose & current_pose, const TrajectoryData & ego_traj_data, const std::vector<double> & time_vec);
+    const geometry_msgs::msg::Pose & current_pose, const TrajectoryData & ego_traj_data,
+    const std::vector<double> & time_vec);
 
   boost::optional<SBoundaries> getSBoundaries(
-    const TrajectoryData & ego_traj_data, const autoware_auto_perception_msgs::msg::PredictedObject & object, const rclcpp::Time & obj_base_time, const std::vector<double> & time_vec);
+    const TrajectoryData & ego_traj_data,
+    const autoware_auto_perception_msgs::msg::PredictedObject & object,
+    const rclcpp::Time & obj_base_time, const std::vector<double> & time_vec);
 
   boost::optional<SBoundaries> getSBoundaries(
-    const TrajectoryData & ego_traj_data, const std::vector<double> & time_vec, const double safety_distance,
+    const TrajectoryData & ego_traj_data, const std::vector<double> & time_vec,
+    const double safety_distance,
     const autoware_auto_perception_msgs::msg::PredictedObject & object,
     const double dist_to_collision_point);
 
   boost::optional<SBoundaries> getSBoundaries(
     const TrajectoryData & ego_traj_data, const std::vector<double> & time_vec,
-    const double safety_distance, const autoware_auto_perception_msgs::msg::PredictedObject & object,const rclcpp::Time & obj_base_time,
+    const double safety_distance,
+    const autoware_auto_perception_msgs::msg::PredictedObject & object,
+    const rclcpp::Time & obj_base_time,
     const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path);
 
   bool checkOnMapObject(
@@ -142,11 +150,13 @@ class ObstacleVelocityPlanner : public rclcpp::Node
     const autoware_auto_planning_msgs::msg::Trajectory & traj);
 
   boost::optional<autoware_auto_perception_msgs::msg::PredictedPath> resampledPredictedPath(
-    const autoware_auto_perception_msgs::msg::PredictedObject & object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time, const std::vector<double> & resolutions, const double horizon);
+    const autoware_auto_perception_msgs::msg::PredictedObject & object,
+    const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time,
+    const std::vector<double> & resolutions, const double horizon);
 
   boost::optional<geometry_msgs::msg::Pose> getCurrentObjectPoseFromPredictedPath(
-  const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const rclcpp::Time & obj_base_time,
-  const rclcpp::Time & current_time);
+    const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+    const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time);
 
   boost::optional<double> getDistanceToCollisionPoint(
     const TrajectoryData & ego_traj_data, const ObjectData & obj_data,
@@ -167,13 +177,16 @@ class ObstacleVelocityPlanner : public rclcpp::Node
 
   void publishDebugTrajectory(
     const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t closest_idx,
-    const std::vector<double> & time_vec, const SBoundaries & s_boundaries, const VelocityOptimizer::OptimizationResult & opt_result);
+    const std::vector<double> & time_vec, const SBoundaries & s_boundaries,
+    const VelocityOptimizer::OptimizationResult & opt_result);
 
   // ROS related members
   // Subscriber
   rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr trajectory_sub_;
-  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr smoothed_trajectory_sub_;
-  rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr objects_sub_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
+    smoothed_trajectory_sub_;
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr
+    objects_sub_;
   rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_;
   rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_map_;
   rclcpp::Subscription<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr
@@ -183,9 +196,9 @@ class ObstacleVelocityPlanner : public rclcpp::Node
   rclcpp::Publisher<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr trajectory_pub_;
   rclcpp::Publisher<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr boundary_pub_;
   rclcpp::Publisher<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr optimized_sv_pub_;
-  rclcpp::Publisher<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr optimized_st_graph_pub_;
-  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
-    distance_to_closest_obj_pub_;
+  rclcpp::Publisher<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
+    optimized_st_graph_pub_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr distance_to_closest_obj_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr debug_calculation_time_;
 
   // Calculation time watcher
@@ -262,4 +275,4 @@ class ObstacleVelocityPlanner : public rclcpp::Node
   double stop_dist_to_prohibit_engage_;
 };
 
-#endif  // OBSTACLE_VELOCITY_PLANNER_NODE_HPP_
+#endif  // OBSTACLE_VELOCITY_PLANNER__NODE_HPP_
diff --git a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/resample.hpp b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/resample.hpp
index bf85b474a95f3..0ae7dbe9be810 100644
--- a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/resample.hpp
+++ b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/resample.hpp
@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2022 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef OBSTACLE_VELOCITY_PLANNER_RESAMPLE_HPP_
-#define OBSTACLE_VELOCITY_PLANNER_RESAMPLE_HPP_
+#ifndef OBSTACLE_VELOCITY_PLANNER__RESAMPLE_HPP_
+#define OBSTACLE_VELOCITY_PLANNER__RESAMPLE_HPP_
 
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
@@ -27,20 +27,24 @@
 namespace resampling
 {
 std::vector<rclcpp::Duration> resampledValidRelativeTimeVector(
-  const rclcpp::Time & start_time, const rclcpp::Time & obj_base_time, const std::vector<double> & rel_time_vec, const double duration);
+  const rclcpp::Time & start_time, const rclcpp::Time & obj_base_time,
+  const std::vector<double> & rel_time_vec, const double duration);
 
 autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath(
-  const autoware_auto_perception_msgs::msg::PredictedPath & input_path, const std::vector<rclcpp::Duration> & rel_time_vec);
+  const autoware_auto_perception_msgs::msg::PredictedPath & input_path,
+  const std::vector<rclcpp::Duration> & rel_time_vec);
 
 geometry_msgs::msg::Pose lerpByPose(
   const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t);
 
 boost::optional<geometry_msgs::msg::Pose> lerpByTimeStamp(
-  const autoware_auto_perception_msgs::msg::PredictedPath & path, const rclcpp::Duration & rel_time);
+  const autoware_auto_perception_msgs::msg::PredictedPath & path,
+  const rclcpp::Duration & rel_time);
 
 autoware_auto_planning_msgs::msg::Trajectory applyLinearInterpolation(
-  const std::vector<double> & base_index, const autoware_auto_planning_msgs::msg::Trajectory & base_trajectory,
+  const std::vector<double> & base_index,
+  const autoware_auto_planning_msgs::msg::Trajectory & base_trajectory,
   const std::vector<double> & out_index, const bool use_spline_for_pose = false);
 }  // namespace resampling
 
-#endif  // OBSTACLE_VELOCITY_PLANNER_RESAMPLE_HPP_
+#endif  // OBSTACLE_VELOCITY_PLANNER__RESAMPLE_HPP_
diff --git a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/velocity_optimizer.hpp b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/velocity_optimizer.hpp
index 78744374bf788..b3b9346afb1ad 100644
--- a/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/velocity_optimizer.hpp
+++ b/planning/obstacle_velocity_planner/include/obstacle_velocity_planner/velocity_optimizer.hpp
@@ -11,8 +11,8 @@
 // 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 OBSTACLE_VELOCITY_PLANNER_VELOCITY_OPTIMIZER_HPP_
-#define OBSTACLE_VELOCITY_PLANNER_VELOCITY_OPTIMIZER_HPP_
+#ifndef OBSTACLE_VELOCITY_PLANNER__VELOCITY_OPTIMIZER_HPP_
+#define OBSTACLE_VELOCITY_PLANNER__VELOCITY_OPTIMIZER_HPP_
 
 #include "obstacle_velocity_planner/common/s_boundary.hpp"
 #include "osqp_interface/osqp_interface.hpp"
@@ -68,4 +68,4 @@ class VelocityOptimizer
   autoware::common::osqp::OSQPInterface qp_solver_;
 };
 
-#endif  // OBSTACLE_VELOCITY_PLANNER_VELOCITY_OPTIMIZER_HPP_
+#endif  // OBSTACLE_VELOCITY_PLANNER__VELOCITY_OPTIMIZER_HPP_
diff --git a/planning/obstacle_velocity_planner/package.xml b/planning/obstacle_velocity_planner/package.xml
index ad27ee8773d85..aa1267f98a408 100644
--- a/planning/obstacle_velocity_planner/package.xml
+++ b/planning/obstacle_velocity_planner/package.xml
@@ -10,22 +10,22 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
-  <depend>autoware_auto_mapping_msgs</depend>
-  <depend>tier4_planning_msgs</depend>
-  <depend>tier4_autoware_utils</depend>
-  <depend>tier4_debug_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
+  <depend>lanelet2_extension</depend>
   <depend>nav_msgs</depend>
   <depend>osqp_interface</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
-  <depend>lanelet2_extension</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_debug_msgs</depend>
+  <depend>tier4_planning_msgs</depend>
   <depend>vehicle_info_util</depend>
   <depend>visualization_msgs</depend>
 
diff --git a/planning/obstacle_velocity_planner/scripts/trajectory_visualizer.py b/planning/obstacle_velocity_planner/scripts/trajectory_visualizer.py
index e245aa6926f5a..4d56b400f75c9 100755
--- a/planning/obstacle_velocity_planner/scripts/trajectory_visualizer.py
+++ b/planning/obstacle_velocity_planner/scripts/trajectory_visualizer.py
@@ -15,27 +15,28 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from autoware_auto_planning_msgs.msg import Trajectory
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import TwistStamped
+from matplotlib import animation
+import matplotlib.pyplot as plt
+import message_filters
+import numpy as np
 import rclpy
 from rclpy.node import Node
 from tf2_ros.buffer import Buffer
 from tf2_ros.transform_listener import TransformListener
-from autoware_auto_planning_msgs.msg import Trajectory
-from geometry_msgs.msg import Pose, TwistStamped, Twist
-import matplotlib.pyplot as plt
-from matplotlib import animation
-import numpy as np
-import message_filters
 
 PLOT_MAX_ARCLENGTH = 200
-PATH_ORIGIN_FRAME = 'map'
-SELF_POSE_FRAME = 'base_link'
+PATH_ORIGIN_FRAME = "map"
+SELF_POSE_FRAME = "base_link"
 
 
 class TrajectoryVisualizer(Node):
-
     def __init__(self):
 
-        super().__init__('trajectory_visualizer')
+        super().__init__("trajectory_visualizer")
 
         self.fig = plt.figure()
 
@@ -51,7 +52,8 @@ def __init__(self):
 
         self.tf_buffer = Buffer(node=self)
         self.tf_listener = TransformListener(
-            self.tf_buffer, self, spin_thread=True)  # For get self-position
+            self.tf_buffer, self, spin_thread=True
+        )  # For get self-position
         self.self_pose = Pose()
         self.self_pose_received = False
         self.localization_twist = Twist()
@@ -64,39 +66,37 @@ def __init__(self):
         self.optimized_st_graph = Trajectory()
 
         self.sub_localization_twist = self.create_subscription(
-            TwistStamped, '/localization/twist',
-            self.CallbackLocalizationTwist, 1)
+            TwistStamped, "/localization/twist", self.CallbackLocalizationTwist, 1
+        )
         self.sub_vehicle_twist = self.create_subscription(
-            TwistStamped, '/vehicle/status/twist',
-            self.CallbackVehicleTwist, 1)
-
-        topic_header = '/planning/scenario_planning/lane_driving/motion_planning/'
-        traj0 = 'obstacle_velocity_planner/optimized_sv_trajectory'
-        self.substatus0 = message_filters.Subscriber(self, Trajectory,
-                                                     topic_header + traj0)
-        traj1 = '/planning/scenario_planning/lane_driving/trajectory'
-        self.substatus1 = message_filters.Subscriber(self, Trajectory,
-                                                     traj1)
-        traj2 = 'surround_obstacle_checker/trajectory'
-        self.substatus2 = message_filters.Subscriber(self, Trajectory,
-                                                     topic_header + traj2)
-        traj3 = 'obstacle_velocity_planner/boundary'
-        self.substatus3 = message_filters.Subscriber(self, Trajectory,
-                                                     topic_header + traj3)
-        traj4 = 'obstacle_velocity_planner/optimized_st_graph'
-        self.substatus4 = message_filters.Subscriber(self, Trajectory,
-                                                     topic_header + traj4)
+            TwistStamped, "/vehicle/status/twist", self.CallbackVehicleTwist, 1
+        )
+
+        topic_header = "/planning/scenario_planning/lane_driving/motion_planning/"
+        traj0 = "obstacle_velocity_planner/optimized_sv_trajectory"
+        self.sub_status0 = message_filters.Subscriber(self, Trajectory, topic_header + traj0)
+        traj1 = "/planning/scenario_planning/lane_driving/trajectory"
+        self.sub_status1 = message_filters.Subscriber(self, Trajectory, traj1)
+        traj2 = "surround_obstacle_checker/trajectory"
+        self.sub_status2 = message_filters.Subscriber(self, Trajectory, topic_header + traj2)
+        traj3 = "obstacle_velocity_planner/boundary"
+        self.sub_status3 = message_filters.Subscriber(self, Trajectory, topic_header + traj3)
+        traj4 = "obstacle_velocity_planner/optimized_st_graph"
+        self.sub_status4 = message_filters.Subscriber(self, Trajectory, topic_header + traj4)
 
         self.ts1 = message_filters.ApproximateTimeSynchronizer(
-            [self.substatus0, self.substatus1, self.substatus2], 30, 0.5)
+            [self.sub_status0, self.sub_status1, self.sub_status2], 30, 0.5
+        )
         self.ts1.registerCallback(self.CallbackMotionVelOptTraj)
         self.ts2 = message_filters.ApproximateTimeSynchronizer(
-            [self.substatus3, self.substatus4], 30, 0.5)
+            [self.sub_status3, self.sub_status4], 30, 0.5
+        )
         self.ts2.registerCallback(self.CallbackMotionVelObsTraj)
 
         # Main Process
         self.ani = animation.FuncAnimation(
-            self.fig, self.plotTrajectoryVelocity, interval=100, blit=True)
+            self.fig, self.plotTrajectoryVelocity, interval=100, blit=True
+        )
         self.setPlotTrajectoryVelocity()
 
         plt.show()
@@ -120,7 +120,7 @@ def CallbackMotionVelObsTraj(self, cmd1, cmd2):
     def CallbackSVTraj(self, cmd):
         self.sv_trajectory = cmd
         self.update_sv_traj = True
-        
+
     def CallbackTraj(self, cmd):
         self.trajectory = cmd
         self.update_traj = True
@@ -139,45 +139,53 @@ def CallbackOptimizedSTGraph(self, cmd):
 
     def setPlotTrajectoryVelocity(self):
         self.ax1 = plt.subplot(2, 1, 1)  # row, col, index(<raw*col)
-        self.im0, = self.ax1.plot(
-            [], [], label='Optimized SV Velocity', marker='', color='purple')
-        self.im1, = self.ax1.plot(
-            [], [], label='Input Velocity', marker='', color='black')
-        self.im2, = self.ax1.plot(
-            [], [], label='Output Velocity', marker='', color='blue')
-        self.im3, = self.ax1.plot(
-            [], [], label='localization twist vx', color='r', marker='*', ls=':', markersize=10)
-        self.im4, = self.ax1.plot(
-            [], [], label='vehicle twist vx', color='k', marker='+', ls=':', markersize=10)
-        self.ax1.set_title('velocity on trajectory')
+        (self.im0,) = self.ax1.plot(
+            [], [], label="Optimized SV Velocity", marker="", color="purple"
+        )
+        (self.im1,) = self.ax1.plot([], [], label="Input Velocity", marker="", color="black")
+        (self.im2,) = self.ax1.plot([], [], label="Output Velocity", marker="", color="blue")
+        (self.im3,) = self.ax1.plot(
+            [], [], label="localization twist vx", color="r", marker="*", ls=":", markersize=10
+        )
+        (self.im4,) = self.ax1.plot(
+            [], [], label="vehicle twist vx", color="k", marker="+", ls=":", markersize=10
+        )
+        self.ax1.set_title("velocity on trajectory")
         self.ax1.legend()
         self.ax1.set_xlim([0, PLOT_MAX_ARCLENGTH])
-        self.ax1.set_ylabel('vel [m/s]')
+        self.ax1.set_ylabel("vel [m/s]")
 
         self.ax2 = plt.subplot(2, 1, 2)
-        self.im5, = self.ax2.plot(
-            [], [], label='Original', marker='', color='black', linewidth=3.0)
-        self.im6, = self.ax2.plot(
-            [], [], label='Optimum', marker='', color='blue')
-        self.im7, = self.ax2.plot(
-            [], [], label='Boundary', marker='', color='green')
-        self.im8, = self.ax2.plot(
-            [], [], label='Optimized ST Graph', marker='', color='purple')
-        self.ax2.set_title('st graph')
+        (self.im5,) = self.ax2.plot(
+            [], [], label="Original", marker="", color="black", linewidth=3.0
+        )
+        (self.im6,) = self.ax2.plot([], [], label="Optimum", marker="", color="blue")
+        (self.im7,) = self.ax2.plot([], [], label="Boundary", marker="", color="green")
+        (self.im8,) = self.ax2.plot([], [], label="Optimized ST Graph", marker="", color="purple")
+        self.ax2.set_title("st graph")
         self.ax2.legend()
         self.ax2.set_xlim([0, 30])
         self.ax2.set_ylim([0, PLOT_MAX_ARCLENGTH])
-        self.ax2.set_xlabel('t [s]')
-        self.ax2.set_ylabel('s [m]')
-
-        return self.im0, self.im1, self.im2, self.im3, self.im4, \
-            self.im5, self.im6, self.im7, self.im8
+        self.ax2.set_xlabel("t [s]")
+        self.ax2.set_ylabel("s [m]")
+
+        return (
+            self.im0,
+            self.im1,
+            self.im2,
+            self.im3,
+            self.im4,
+            self.im5,
+            self.im6,
+            self.im7,
+            self.im8,
+        )
 
     def plotTrajectoryVelocity(self, data):
         self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME)
         if self.self_pose_received is False:
-            print('plot start but self pose is not received')
-        self.get_logger().info('plot called')
+            print("plot start but self pose is not received")
+        self.get_logger().info("plot called")
 
         # copy
         trajectory = self.trajectory
@@ -206,16 +214,14 @@ def plotTrajectoryVelocity(self, data):
 
                 opt_zero_vel_id = -1
                 for i in range(opt_closest, len(trajectory.points)):
-                    if(trajectory.points[i].longitudinal_velocity_mps < 1e-3):
+                    if trajectory.points[i].longitudinal_velocity_mps < 1e-3:
                         opt_zero_vel_id = i
                         break
                 else:
                     opt_zero_vel_id = len(trajectory.points) - 1
 
-                opt_pos = self.CalcPartArcLength(
-                    trajectory, opt_closest, opt_zero_vel_id + 1)
-                opt_time = self.CalcTime(
-                    trajectory, opt_closest, opt_zero_vel_id + 1)
+                opt_pos = self.CalcPartArcLength(trajectory, opt_closest, opt_zero_vel_id + 1)
+                opt_time = self.CalcTime(trajectory, opt_closest, opt_zero_vel_id + 1)
                 self.im6.set_data(opt_time, opt_pos)
 
         if self.update_max_traj:
@@ -228,16 +234,14 @@ def plotTrajectoryVelocity(self, data):
             if max_closest >= 0:
                 max_zero_vel_id = -1
                 for i in range(max_closest, len(max_trajectory.points)):
-                    if(max_trajectory.points[i].longitudinal_velocity_mps < 1e-3):
+                    if max_trajectory.points[i].longitudinal_velocity_mps < 1e-3:
                         max_zero_vel_id = i
                         break
                 else:
                     max_zero_vel_id = len(max_trajectory.points) - 1
 
-                max_pos = self.CalcPartArcLength(
-                    max_trajectory, max_closest, max_zero_vel_id + 1)
-                max_time = self.CalcTime(
-                    max_trajectory, max_closest, max_zero_vel_id + 1)
+                max_pos = self.CalcPartArcLength(max_trajectory, max_closest, max_zero_vel_id + 1)
+                max_time = self.CalcTime(max_trajectory, max_closest, max_zero_vel_id + 1)
                 self.im5.set_data(max_time, max_pos)
 
         if self.update_boundary:
@@ -261,14 +265,23 @@ def plotTrajectoryVelocity(self, data):
         # change y-range
         self.ax1.set_ylim([-1.0, 50.0])
 
-        return self.im0, self.im1, self.im2, self.im3, self.im4, \
-            self.im5, self.im6, self.im7, self.im8
+        return (
+            self.im0,
+            self.im1,
+            self.im2,
+            self.im3,
+            self.im4,
+            self.im5,
+            self.im6,
+            self.im7,
+            self.im8,
+        )
 
     def CalcArcLength(self, traj):
         return self.CalcPartArcLength(traj, 0, len(traj.points))
 
     def CalcPartArcLength(self, traj, start, end):
-        assert(start <= end)
+        assert start <= end
         s_arr = []
         ds = 0.0
         s_sum = 0.0
@@ -322,8 +335,7 @@ def ToVelList(self, traj):
 
     def updatePose(self, from_link, to_link):
         try:
-            tf = self.tf_buffer.lookup_transform(
-                from_link, to_link, rclpy.time.Time())
+            tf = self.tf_buffer.lookup_transform(from_link, to_link, rclpy.time.Time())
             self.self_pose.position.x = tf.transform.translation.x
             self.self_pose.position.y = tf.transform.translation.y
             self.self_pose.position.z = tf.transform.translation.z
@@ -335,16 +347,15 @@ def updatePose(self, from_link, to_link):
             return
         except BaseException:
             self.get_logger().warn(
-                'lookup transform failed: from {} to {}'.format(
-                    from_link, to_link))
+                "lookup transform failed: from {} to {}".format(from_link, to_link)
+            )
             return
 
     def calcClosestTrajectory(self, path):
         closest = -1
         min_dist_squared = 1.0e10
         for i in range(0, len(path.points)):
-            dist_sq = self.calcSquaredDist2d(
-                self.self_pose, path.points[i].pose)
+            dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose)
             if dist_sq < min_dist_squared:
                 min_dist_squared = dist_sq
                 closest = i
@@ -368,5 +379,5 @@ def main(args=None):
         rclpy.shutdown()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()
diff --git a/planning/obstacle_velocity_planner/src/box2d.cpp b/planning/obstacle_velocity_planner/src/box2d.cpp
index 0f184c4aa11fd..c990b907287c4 100644
--- a/planning/obstacle_velocity_planner/src/box2d.cpp
+++ b/planning/obstacle_velocity_planner/src/box2d.cpp
@@ -1,78 +1,100 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// 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 "obstacle_velocity_planner/box2d.hpp"
 
-#include <tf2/utils.h>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
+#include <tf2/utils.h>
+
 Box2d::Box2d(const geometry_msgs::msg::Pose & center_pose, const double length, const double width)
- : center_(center_pose.position), length_(length), width_(width), half_length_(length/2.0), half_width_(width/2.0)
+: center_(center_pose.position),
+  length_(length),
+  width_(width),
+  half_length_(length / 2.0),
+  half_width_(width / 2.0)
 {
-    max_x_ = std::numeric_limits<double>::min();
-    max_y_ = std::numeric_limits<double>::min();
-    min_x_ = std::numeric_limits<double>::max();
-    min_y_ = std::numeric_limits<double>::max();
-    heading_ = tf2::getYaw(center_pose.orientation);
-    cos_heading_ = std::cos(heading_);
-    sin_heading_ = std::sin(heading_);
-    InitCorners();
+  max_x_ = std::numeric_limits<double>::min();
+  max_y_ = std::numeric_limits<double>::min();
+  min_x_ = std::numeric_limits<double>::max();
+  min_y_ = std::numeric_limits<double>::max();
+  heading_ = tf2::getYaw(center_pose.orientation);
+  cos_heading_ = std::cos(heading_);
+  sin_heading_ = std::sin(heading_);
+  InitCorners();
 }
 
-void Box2d::InitCorners() {
-    const double dx1 = cos_heading_ * half_length_;
-    const double dy1 = sin_heading_ * half_length_;
-    const double dx2 = sin_heading_ * half_width_;
-    const double dy2 = -cos_heading_ * half_width_;
+void Box2d::InitCorners()
+{
+  const double dx1 = cos_heading_ * half_length_;
+  const double dy1 = sin_heading_ * half_length_;
+  const double dx2 = sin_heading_ * half_width_;
+  const double dy2 = -cos_heading_ * half_width_;
 
-    const auto p1 = tier4_autoware_utils::createPoint(center_.x + dx1 + dx2, center_.y + dy1 + dy2, center_.z);
-    const auto p2 = tier4_autoware_utils::createPoint(center_.x + dx1 - dx2, center_.y + dy1 - dy2, center_.z);
-    const auto p3 = tier4_autoware_utils::createPoint(center_.x - dx1 - dx2, center_.y - dy1 - dy2, center_.z);
-    const auto p4 = tier4_autoware_utils::createPoint(center_.x - dx1 + dx2, center_.y - dy1 + dy2, center_.z);
-    corners_.clear();
-    corners_.resize(4);
-    corners_[0] = p1;
-    corners_[1] = p2;
-    corners_[2] = p3;
-    corners_[3] = p4;
+  const auto p1 =
+    tier4_autoware_utils::createPoint(center_.x + dx1 + dx2, center_.y + dy1 + dy2, center_.z);
+  const auto p2 =
+    tier4_autoware_utils::createPoint(center_.x + dx1 - dx2, center_.y + dy1 - dy2, center_.z);
+  const auto p3 =
+    tier4_autoware_utils::createPoint(center_.x - dx1 - dx2, center_.y - dy1 - dy2, center_.z);
+  const auto p4 =
+    tier4_autoware_utils::createPoint(center_.x - dx1 + dx2, center_.y - dy1 + dy2, center_.z);
+  corners_.clear();
+  corners_.resize(4);
+  corners_[0] = p1;
+  corners_[1] = p2;
+  corners_[2] = p3;
+  corners_[3] = p4;
 
-    for (auto &corner : corners_) {
-        max_x_ = std::fmax(corner.x, max_x_);
-        min_x_ = std::fmin(corner.x, min_x_);
-        max_y_ = std::fmax(corner.y, max_y_);
-        min_y_ = std::fmin(corner.y, min_y_);
-    }
+  for (auto & corner : corners_) {
+    max_x_ = std::fmax(corner.x, max_x_);
+    min_x_ = std::fmin(corner.x, min_x_);
+    max_y_ = std::fmax(corner.y, max_y_);
+    min_y_ = std::fmin(corner.y, min_y_);
+  }
 }
 
-bool Box2d::HasOverlap(const Box2d &box) const {
-    if (box.max_x() < min_x() || box.min_x() > max_x() || box.max_y() < min_y() ||
-        box.min_y() > max_y()) {
-        return false;
-    }
+bool Box2d::HasOverlap(const Box2d & box) const
+{
+  if (
+    box.max_x() < min_x() || box.min_x() > max_x() || box.max_y() < min_y() ||
+    box.min_y() > max_y()) {
+    return false;
+  }
 
-    const double shift_x = box.center_x() - center_.x;
-    const double shift_y = box.center_y() - center_.y;
+  const double shift_x = box.center_x() - center_.x;
+  const double shift_y = box.center_y() - center_.y;
 
-    const double dx1 = cos_heading_ * half_length_;
-    const double dy1 = sin_heading_ * half_length_;
-    const double dx2 = sin_heading_ * half_width_;
-    const double dy2 = -cos_heading_ * half_width_;
-    const double dx3 = box.cos_heading() * box.half_length();
-    const double dy3 = box.sin_heading() * box.half_length();
-    const double dx4 = box.sin_heading() * box.half_width();
-    const double dy4 = -box.cos_heading() * box.half_width();
+  const double dx1 = cos_heading_ * half_length_;
+  const double dy1 = sin_heading_ * half_length_;
+  const double dx2 = sin_heading_ * half_width_;
+  const double dy2 = -cos_heading_ * half_width_;
+  const double dx3 = box.cos_heading() * box.half_length();
+  const double dy3 = box.sin_heading() * box.half_length();
+  const double dx4 = box.sin_heading() * box.half_width();
+  const double dy4 = -box.cos_heading() * box.half_width();
 
-    return std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=
+  return std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=
            std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
-           std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) +
-           half_length_ &&
-           std::abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=
+             std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) + half_length_ &&
+         std::abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=
            std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
-           std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) +
-           half_width_ &&
-           std::abs(shift_x * box.cos_heading() + shift_y * box.sin_heading()) <=
+             std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) + half_width_ &&
+         std::abs(shift_x * box.cos_heading() + shift_y * box.sin_heading()) <=
            std::abs(dx1 * box.cos_heading() + dy1 * box.sin_heading()) +
-           std::abs(dx2 * box.cos_heading() + dy2 * box.sin_heading()) +
-           box.half_length() &&
-           std::abs(shift_x * box.sin_heading() - shift_y * box.cos_heading()) <=
+             std::abs(dx2 * box.cos_heading() + dy2 * box.sin_heading()) + box.half_length() &&
+         std::abs(shift_x * box.sin_heading() - shift_y * box.cos_heading()) <=
            std::abs(dx1 * box.sin_heading() - dy1 * box.cos_heading()) +
-           std::abs(dx2 * box.sin_heading() - dy2 * box.cos_heading()) +
-           box.half_width();
+             std::abs(dx2 * box.sin_heading() - dy2 * box.cos_heading()) + box.half_width();
 }
diff --git a/planning/obstacle_velocity_planner/src/node.cpp b/planning/obstacle_velocity_planner/src/node.cpp
index e933ccd7d9055..9da4bfe3c8c82 100644
--- a/planning/obstacle_velocity_planner/src/node.cpp
+++ b/planning/obstacle_velocity_planner/src/node.cpp
@@ -1,4 +1,4 @@
-// Copyright 2020 Tier IV, Inc.
+// Copyright 2022 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -16,10 +16,10 @@
 
 #include "obstacle_velocity_planner/resample.hpp"
 
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
-#include <tier4_autoware_utils/ros/update_param.hpp>
 #include <interpolation/linear_interpolation.hpp>
 #include <interpolation/spline_interpolation.hpp>
+#include <tier4_autoware_utils/ros/update_param.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <tf2/utils.h>
@@ -124,8 +124,8 @@ ObstacleVelocityPlanner::ObstacleVelocityPlanner(const rclcpp::NodeOptions & nod
 
   delta_yaw_threshold_of_nearest_index_ =
     tier4_autoware_utils::deg2rad(declare_parameter("delta_yaw_threshold_of_nearest_index", 60.0));
-  delta_yaw_threshold_of_object_and_ego_ =
-    tier4_autoware_utils::deg2rad(declare_parameter("delta_yaw_threshold_of_object_and_ego", 180.0));
+  delta_yaw_threshold_of_object_and_ego_ = tier4_autoware_utils::deg2rad(
+    declare_parameter("delta_yaw_threshold_of_object_and_ego", 180.0));
   object_zero_velocity_threshold_ = declare_parameter("object_zero_velocity_threshold", 1.5);
   object_low_velocity_threshold_ = declare_parameter("object_low_velocity_threshold", 3.0);
   external_velocity_limit_ = declare_parameter("external_velocity_limit", 20.0);
@@ -216,7 +216,8 @@ void ObstacleVelocityPlanner::trajectoryCallback(
     time_vec.push_back(dense_time_horizon_);
   }
 
-  for (double t = dense_time_horizon_ + sparse_resampling_time_interval_; t < max_time_horizon_; t += sparse_resampling_time_interval_) {
+  for (double t = dense_time_horizon_ + sparse_resampling_time_interval_; t < max_time_horizon_;
+       t += sparse_resampling_time_interval_) {
     time_vec.push_back(t);
   }
   if (max_time_horizon_ - time_vec.back() < 1e-3) {
@@ -244,7 +245,7 @@ void ObstacleVelocityPlanner::trajectoryCallback(
     return;
   }
 
-  // Transfrom original trajectory to TrajectoryData
+  // Transform original trajectory to TrajectoryData
   const auto base_traj_data = getTrajectoryData(*msg, *closest_idx);
   if (base_traj_data.traj.points.size() < 2) {
     RCLCPP_DEBUG(get_logger(), "The number of points on the trajectory data is too small");
@@ -432,8 +433,9 @@ void ObstacleVelocityPlanner::trajectoryCallback(
   auto resampled_traj =
     resampling::applyLinearInterpolation(base_s, base_traj_data.traj, resampled_opt_position);
   for (size_t i = 0; i < resampled_opt_velocity.size(); ++i) {
-    resampled_traj.points.at(i).longitudinal_velocity_mps =
-      std::min(resampled_opt_velocity.at(i), static_cast<double>(resampled_traj.points.at(i).longitudinal_velocity_mps));
+    resampled_traj.points.at(i).longitudinal_velocity_mps = std::min(
+      resampled_opt_velocity.at(i),
+      static_cast<double>(resampled_traj.points.at(i).longitudinal_velocity_mps));
   }
   for (size_t i = 0; i < resampled_opt_position.size(); ++i) {
     if (resampled_opt_position.at(i) >= closest_stop_dist) {
@@ -453,8 +455,8 @@ void ObstacleVelocityPlanner::trajectoryCallback(
   output.points.back().longitudinal_velocity_mps = 0.0;  // terminal velocity is zero
 
   // Zero Velocity Padding
-  const auto zero_vel_idx =
-    tier4_autoware_utils::searchZeroVelocityIndex(output.points, *closest_idx, output.points.size());
+  const auto zero_vel_idx = tier4_autoware_utils::searchZeroVelocityIndex(
+    output.points, *closest_idx, output.points.size());
   if (zero_vel_idx) {
     for (size_t i = *zero_vel_idx; i < output.points.size(); ++i) {
       output.points.at(i).longitudinal_velocity_mps = 0.0;
@@ -465,13 +467,16 @@ void ObstacleVelocityPlanner::trajectoryCallback(
   trajectory_pub_->publish(output);
 }
 
-double ObstacleVelocityPlanner::getClosestStopDistance(const TrajectoryData & ego_traj_data, const std::vector<double> & resolutions)
+double ObstacleVelocityPlanner::getClosestStopDistance(
+  const TrajectoryData & ego_traj_data, const std::vector<double> & resolutions)
 {
   const auto current_time = get_clock()->now();
   double closest_stop_dist = ego_traj_data.s.back();
-  const auto closest_stop_id = tier4_autoware_utils::searchZeroVelocityIndex(ego_traj_data.traj.points);
-  closest_stop_dist =
-    closest_stop_id ? std::min(closest_stop_dist, ego_traj_data.s.at(*closest_stop_id)) : closest_stop_dist;
+  const auto closest_stop_id =
+    tier4_autoware_utils::searchZeroVelocityIndex(ego_traj_data.traj.points);
+  closest_stop_dist = closest_stop_id
+                        ? std::min(closest_stop_dist, ego_traj_data.s.at(*closest_stop_id))
+                        : closest_stop_dist;
   if (!in_objects_ptr_) {
     return closest_stop_dist;
   }
@@ -482,10 +487,14 @@ double ObstacleVelocityPlanner::getClosestStopDistance(const TrajectoryData & eg
   for (const auto & obj : in_objects_ptr_->objects) {
     // Step1. Ignore obstacles that are not vehicles
     if (
-      obj.classification.at(0).label != autoware_auto_perception_msgs::msg::ObjectClassification::CAR &&
-      obj.classification.at(0).label != autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK &&
-      obj.classification.at(0).label != autoware_auto_perception_msgs::msg::ObjectClassification::BUS &&
-      obj.classification.at(0).label != autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) {
+      obj.classification.at(0).label !=
+        autoware_auto_perception_msgs::msg::ObjectClassification::CAR &&
+      obj.classification.at(0).label !=
+        autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK &&
+      obj.classification.at(0).label !=
+        autoware_auto_perception_msgs::msg::ObjectClassification::BUS &&
+      obj.classification.at(0).label !=
+        autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) {
       continue;
     }
 
@@ -513,8 +522,7 @@ double ObstacleVelocityPlanner::getClosestStopDistance(const TrajectoryData & eg
     obj_data.pose = current_object_pose.get();
     obj_data.length = obj.shape.dimensions.x;
     obj_data.width = obj.shape.dimensions.y;
-    obj_data.time =
-      std::max((obj_base_time - current_time).seconds(), 0.0);
+    obj_data.time = std::max((obj_base_time - current_time).seconds(), 0.0);
     const double current_delta_yaw_threshold = 3.14;
     const auto dist_to_collision_point =
       getDistanceToCollisionPoint(ego_traj_data, obj_data, current_delta_yaw_threshold);
@@ -527,9 +535,7 @@ double ObstacleVelocityPlanner::getClosestStopDistance(const TrajectoryData & eg
     // If the object is on the current ego trajectory,
     // we assume the object travels along ego trajectory
     const double obj_vel = std::fabs(obj.kinematics.initial_twist_with_covariance.twist.linear.x);
-    if (
-      dist_to_collision_point &&
-      obj_vel < object_zero_velocity_threshold_) {
+    if (dist_to_collision_point && obj_vel < object_zero_velocity_threshold_) {
       const double current_stop_point = std::max(*dist_to_collision_point - safety_distance, 0.0);
       closest_stop_dist = std::min(current_stop_point, closest_stop_dist);
     }
@@ -550,19 +556,21 @@ double ObstacleVelocityPlanner::getClosestStopDistance(const TrajectoryData & eg
     dist_to_obj.data = closest_obj_distance;
     distance_to_closest_obj_pub_->publish(dist_to_obj);
 
-    const auto predicted_path =
-      resampledPredictedPath(closest_obj, obj_base_time, current_time, resolutions, max_time_horizon_);
+    const auto predicted_path = resampledPredictedPath(
+      closest_obj, obj_base_time, current_time, resolutions, max_time_horizon_);
 
     // Get current pose from object's predicted path
     const auto current_object_pose =
       getCurrentObjectPoseFromPredictedPath(*predicted_path, obj_base_time, current_time);
-    const size_t nearest_idx =
-    tier4_autoware_utils::findNearestIndex(ego_traj_data.traj.points, current_object_pose.get().position);
+    const size_t nearest_idx = tier4_autoware_utils::findNearestIndex(
+      ego_traj_data.traj.points, current_object_pose.get().position);
     const double ego_yaw = tf2::getYaw(ego_traj_data.traj.points.at(nearest_idx).pose.orientation);
     const double obj_yaw = tf2::getYaw(current_object_pose.get().orientation);
     const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_yaw - ego_yaw);
     RCLCPP_DEBUG(get_logger(), "Closest Object Distance %f", closest_obj_distance);
-    RCLCPP_DEBUG(get_logger(), "Closest Object Velocity %f", closest_obj.kinematics.initial_twist_with_covariance.twist.linear.x);
+    RCLCPP_DEBUG(
+      get_logger(), "Closest Object Velocity %f",
+      closest_obj.kinematics.initial_twist_with_covariance.twist.linear.x);
   }
 
   return closest_stop_dist;
@@ -688,7 +696,8 @@ ObstacleVelocityPlanner::calcInterpolatedTrajectoryPoint(
   {
     const auto & seg_pt = trajectory.points.at(segment_idx);
     const auto & next_pt = trajectory.points.at(segment_idx + 1);
-    traj_p.longitudinal_velocity_mps = interpolate(next_pt.longitudinal_velocity_mps, seg_pt.longitudinal_velocity_mps);
+    traj_p.longitudinal_velocity_mps =
+      interpolate(next_pt.longitudinal_velocity_mps, seg_pt.longitudinal_velocity_mps);
     traj_p.acceleration_mps2 = interpolate(next_pt.acceleration_mps2, seg_pt.acceleration_mps2);
     traj_p.pose.position.x = interpolate(next_pt.pose.position.x, seg_pt.pose.position.x);
     traj_p.pose.position.y = interpolate(next_pt.pose.position.y, seg_pt.pose.position.y);
@@ -699,7 +708,8 @@ ObstacleVelocityPlanner::calcInterpolatedTrajectoryPoint(
 }
 
 bool ObstacleVelocityPlanner::checkHasReachedGoal(
-  const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t closest_idx, const double v0)
+  const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t closest_idx,
+  const double v0)
 {
   // If goal is close and current velocity is low, we don't optimize trajectory
   const auto closest_stop_id = tier4_autoware_utils::searchZeroVelocityIndex(traj.points);
@@ -744,7 +754,8 @@ TrajectoryData ObstacleVelocityPlanner::resampleTrajectoryData(
   }
 
   // Obtain trajectory length until the velocity is zero or stop dist
-  const auto closest_stop_id = tier4_autoware_utils::searchZeroVelocityIndex(base_traj_data.traj.points);
+  const auto closest_stop_id =
+    tier4_autoware_utils::searchZeroVelocityIndex(base_traj_data.traj.points);
   const double closest_stop_dist =
     closest_stop_id ? std::min(stop_dist, base_traj_data.s.at(*closest_stop_id)) : stop_dist;
   const double traj_length = std::min(closest_stop_dist, std::min(base_s.back(), max_traj_length));
@@ -755,7 +766,7 @@ TrajectoryData ObstacleVelocityPlanner::resampleTrajectoryData(
     resampled_s.push_back(s);
   }
 
-  if(resampled_s.empty()) {
+  if (resampled_s.empty()) {
     return TrajectoryData{};
   }
 
@@ -765,7 +776,7 @@ TrajectoryData ObstacleVelocityPlanner::resampleTrajectoryData(
     resampled_s.push_back(traj_length);
   }
 
-  // Resmaple trajectory
+  // Resample trajectory
   const auto resampled_traj = resampleTrajectory(base_s, base_traj_data.traj, resampled_s);
 
   // Store Data
@@ -849,10 +860,14 @@ boost::optional<SBoundaries> ObstacleVelocityPlanner::getSBoundaries(
   for (const auto & obj : in_objects_ptr_->objects) {
     // Step1. Ignore obstacles that are not vehicles
     if (
-      obj.classification.at(0).label != autoware_auto_perception_msgs::msg::ObjectClassification::CAR &&
-      obj.classification.at(0).label != autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK &&
-      obj.classification.at(0).label != autoware_auto_perception_msgs::msg::ObjectClassification::BUS &&
-      obj.classification.at(0).label != autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) {
+      obj.classification.at(0).label !=
+        autoware_auto_perception_msgs::msg::ObjectClassification::CAR &&
+      obj.classification.at(0).label !=
+        autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK &&
+      obj.classification.at(0).label !=
+        autoware_auto_perception_msgs::msg::ObjectClassification::BUS &&
+      obj.classification.at(0).label !=
+        autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) {
       // RCLCPP_DEBUG(get_logger(), "Ignore non-vehicle obstacle");
       continue;
     }
@@ -892,8 +907,9 @@ boost::optional<SBoundaries> ObstacleVelocityPlanner::getSBoundaries(
 }
 
 boost::optional<SBoundaries> ObstacleVelocityPlanner::getSBoundaries(
-  const TrajectoryData & ego_traj_data, const autoware_auto_perception_msgs::msg::PredictedObject & object, const rclcpp::Time & obj_base_time,
-  const std::vector<double> & time_vec)
+  const TrajectoryData & ego_traj_data,
+  const autoware_auto_perception_msgs::msg::PredictedObject & object,
+  const rclcpp::Time & obj_base_time, const std::vector<double> & time_vec)
 {
   const auto current_time = get_clock()->now();
 
@@ -919,8 +935,7 @@ boost::optional<SBoundaries> ObstacleVelocityPlanner::getSBoundaries(
   obj_data.pose = current_object_pose.get();
   obj_data.length = object.shape.dimensions.x;
   obj_data.width = object.shape.dimensions.y;
-  obj_data.time =
-    std::max((obj_base_time - current_time).seconds(), 0.0);
+  obj_data.time = std::max((obj_base_time - current_time).seconds(), 0.0);
   const double current_delta_yaw_threshold = 3.14;
   const auto current_collision_dist =
     getDistanceToCollisionPoint(ego_traj_data, obj_data, current_delta_yaw_threshold);
@@ -943,7 +958,8 @@ boost::optional<SBoundaries> ObstacleVelocityPlanner::getSBoundaries(
   const double obj_vel = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x);
   if (obj_vel > object_low_velocity_threshold_) {
     // RCLCPP_DEBUG(get_logger(), "Off Trajectory Object");
-    return getSBoundaries(ego_traj_data, time_vec, safety_distance, object, obj_base_time, *predicted_path);
+    return getSBoundaries(
+      ego_traj_data, time_vec, safety_distance, object, obj_base_time, *predicted_path);
   }
 
   return boost::none;
@@ -978,7 +994,9 @@ boost::optional<SBoundaries> ObstacleVelocityPlanner::getSBoundaries(
     }
 
     double s_upper_bound =
-      current_s_obj + (current_v_obj * current_v_obj) / (2 * std::fabs(min_object_accel_)) - 0.5 * max_accel_ * t_idling_ * t_idling_ - 0.5 * max_accel_ * max_accel_ * t_idling_ * t_idling_ / std::fabs(min_accel_);
+      current_s_obj + (current_v_obj * current_v_obj) / (2 * std::fabs(min_object_accel_)) -
+      0.5 * max_accel_ * t_idling_ * t_idling_ -
+      0.5 * max_accel_ * max_accel_ * t_idling_ * t_idling_ / std::fabs(min_accel_);
     s_upper_bound = std::max(s_upper_bound, 0.0);
     if (s_upper_bound < s_boundaries.at(i).max_s) {
       s_boundaries.at(i).max_s = s_upper_bound;
@@ -996,15 +1014,14 @@ boost::optional<SBoundaries> ObstacleVelocityPlanner::getSBoundaries(
 
 boost::optional<SBoundaries> ObstacleVelocityPlanner::getSBoundaries(
   const TrajectoryData & ego_traj_data, const std::vector<double> & time_vec,
-  const double safety_distance, const autoware_auto_perception_msgs::msg::PredictedObject & object,const rclcpp::Time & obj_base_time,
+  const double safety_distance, const autoware_auto_perception_msgs::msg::PredictedObject & object,
+  const rclcpp::Time & obj_base_time,
   const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path)
 {
   const auto current_time = get_clock()->now();
-  const double abs_obj_vel = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x);
-  const double v_obj =
-    abs_obj_vel < object_zero_velocity_threshold_
-      ? 0.0
-      : abs_obj_vel;
+  const double abs_obj_vel =
+    std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x);
+  const double v_obj = abs_obj_vel < object_zero_velocity_threshold_ ? 0.0 : abs_obj_vel;
 
   SBoundaries s_boundaries(time_vec.size());
   for (size_t i = 0; i < s_boundaries.size(); ++i) {
@@ -1072,7 +1089,8 @@ boost::optional<double> ObstacleVelocityPlanner::getDistanceToCollisionPoint(
   size_t min_nearest_idx = ego_traj_data.s.size();
   size_t max_nearest_idx = 0;
   for (const auto & obj_p : object_points) {
-    size_t nearest_idx = tier4_autoware_utils::findNearestSegmentIndex(ego_traj_data.traj.points, obj_p);
+    size_t nearest_idx =
+      tier4_autoware_utils::findNearestSegmentIndex(ego_traj_data.traj.points, obj_p);
     min_nearest_idx = std::min(nearest_idx, min_nearest_idx);
     max_nearest_idx = std::max(nearest_idx, max_nearest_idx);
   }
@@ -1101,7 +1119,7 @@ boost::optional<double> ObstacleVelocityPlanner::getDistanceToCollisionPoint(
   const auto collision_idx = getCollisionIdx(ego_traj_data, object_box, start_idx, end_idx);
 
   if (collision_idx) {
-    // TODO: Consider the time difference between ego vehicle and objects
+    // TODO(shimizu) Consider the time difference between ego vehicle and objects
     return getObjectLongitudinalPosition(ego_traj_data, obj_pose);
   }
 
@@ -1144,7 +1162,8 @@ bool ObstacleVelocityPlanner::checkOnMapObject(
 
   // obstacle point
   lanelet::BasicPoint2d search_point(
-    object.kinematics.initial_pose_with_covariance.pose.position.x, object.kinematics.initial_pose_with_covariance.pose.position.y);
+    object.kinematics.initial_pose_with_covariance.pose.position.x,
+    object.kinematics.initial_pose_with_covariance.pose.position.y);
 
   // nearest lanelet
   std::vector<std::pair<double, lanelet::Lanelet>> surrounding_lanelets =
@@ -1160,7 +1179,7 @@ bool ObstacleVelocityPlanner::checkOnMapObject(
   for (const auto & lanelet : surrounding_lanelets) {
     if (lanelet::geometry::inside(lanelet.second, search_point)) {
       has_find_close_lanelet = true;
-      //return true;
+      // return true;
       break;
     }
   }
@@ -1267,7 +1286,8 @@ bool ObstacleVelocityPlanner::checkIsFrontObject(
   size_t nearest_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, point);
 
   // Calculate longitudinal length
-  const double l = tier4_autoware_utils::calcLongitudinalOffsetToSegment(traj.points, nearest_idx, point);
+  const double l =
+    tier4_autoware_utils::calcLongitudinalOffsetToSegment(traj.points, nearest_idx, point);
 
   if (nearest_idx == 0 && l < 0) {
     return false;
@@ -1278,7 +1298,8 @@ bool ObstacleVelocityPlanner::checkIsFrontObject(
 
 boost::optional<autoware_auto_perception_msgs::msg::PredictedPath>
 ObstacleVelocityPlanner::resampledPredictedPath(
-  const autoware_auto_perception_msgs::msg::PredictedObject & object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time,
+  const autoware_auto_perception_msgs::msg::PredictedObject & object,
+  const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time,
   const std::vector<double> & resolutions, const double horizon)
 {
   if (object.kinematics.predicted_paths.empty()) {
@@ -1295,21 +1316,21 @@ ObstacleVelocityPlanner::resampledPredictedPath(
     });
 
   // Resample Predicted Path
-  const double duration = std::min(
-    std::max((obj_base_time - current_time).seconds(), 0.0), horizon);
+  const double duration =
+    std::min(std::max((obj_base_time - current_time).seconds(), 0.0), horizon);
 
   // Calculate relative valid time vector
   // rel_valid_time_vec is relative to obj_base_time.
-  const auto rel_valid_time_vec =
-    resampling::resampledValidRelativeTimeVector(current_time, obj_base_time, resolutions, duration);
+  const auto rel_valid_time_vec = resampling::resampledValidRelativeTimeVector(
+    current_time, obj_base_time, resolutions, duration);
 
   return resampling::resamplePredictedPath(*reliable_path, rel_valid_time_vec);
 }
 
 boost::optional<geometry_msgs::msg::Pose>
 ObstacleVelocityPlanner::getCurrentObjectPoseFromPredictedPath(
-  const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const rclcpp::Time & obj_base_time,
-  const rclcpp::Time & current_time)
+  const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+  const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time)
 {
   for (const auto & obj_p : predicted_path.path) {
     const double object_time = (obj_base_time - current_time).seconds();
@@ -1414,7 +1435,8 @@ ObstacleVelocityPlanner::processOptimizedResult(
 
 void ObstacleVelocityPlanner::publishDebugTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t closest_idx,
-  const std::vector<double> & time_vec, const SBoundaries & s_boundaries, const VelocityOptimizer::OptimizationResult & opt_result)
+  const std::vector<double> & time_vec, const SBoundaries & s_boundaries,
+  const VelocityOptimizer::OptimizationResult & opt_result)
 {
   const std::vector<double> time = opt_result.t;
   // Publish optimized result and boundary
diff --git a/planning/obstacle_velocity_planner/src/resample.cpp b/planning/obstacle_velocity_planner/src/resample.cpp
index 933c1334040fa..6479624a96964 100644
--- a/planning/obstacle_velocity_planner/src/resample.cpp
+++ b/planning/obstacle_velocity_planner/src/resample.cpp
@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2022 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -50,7 +50,8 @@ geometry_msgs::msg::Point toMsg(tf2::Vector3 vec)
 namespace resampling
 {
 std::vector<rclcpp::Duration> resampledValidRelativeTimeVector(
-  const rclcpp::Time & start_time, const rclcpp::Time & obj_base_time, const std::vector<double> & rel_time_vec, const double duration)
+  const rclcpp::Time & start_time, const rclcpp::Time & obj_base_time,
+  const std::vector<double> & rel_time_vec, const double duration)
 {
   const auto prediction_duration = rclcpp::Duration::from_seconds(duration);
   const auto end_time = start_time + prediction_duration;
@@ -58,10 +59,10 @@ std::vector<rclcpp::Duration> resampledValidRelativeTimeVector(
   // NOTE: rel_time_vec is relative time to start_time.
   //       rel_valid_time_vec is relative to obj_base_time, which is time stamp in predicted object.
   std::vector<rclcpp::Duration> rel_valid_time_vec;
-  for(const auto & time : rel_time_vec) {
+  for (const auto & time : rel_time_vec) {
     // absolute target time
     const auto target_time = start_time + rclcpp::Duration::from_seconds(time);
-    if(target_time > end_time) {
+    if (target_time > end_time) {
       break;
     }
 
@@ -78,11 +79,12 @@ std::vector<rclcpp::Duration> resampledValidRelativeTimeVector(
 }
 
 autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath(
-  const autoware_auto_perception_msgs::msg::PredictedPath & input_path, const std::vector<rclcpp::Duration> & rel_time_vec)
+  const autoware_auto_perception_msgs::msg::PredictedPath & input_path,
+  const std::vector<rclcpp::Duration> & rel_time_vec)
 {
   autoware_auto_perception_msgs::msg::PredictedPath resampled_path;
 
-  for(const auto & rel_time : rel_time_vec) {
+  for (const auto & rel_time : rel_time_vec) {
     const auto opt_pose = lerpByTimeStamp(input_path, rel_time);
     if (!opt_pose) {
       continue;
@@ -113,8 +115,6 @@ geometry_msgs::msg::Pose lerpByPose(
 boost::optional<geometry_msgs::msg::Pose> lerpByTimeStamp(
   const autoware_auto_perception_msgs::msg::PredictedPath & path, const rclcpp::Duration & rel_time)
 {
-  geometry_msgs::msg::Pose lerpt_pt;
-
   auto clock{rclcpp::Clock{RCL_ROS_TIME}};
   if (path.path.empty()) {
     RCLCPP_WARN_STREAM_THROTTLE(
@@ -124,10 +124,9 @@ boost::optional<geometry_msgs::msg::Pose> lerpByTimeStamp(
   }
   if (rel_time < rclcpp::Duration::from_seconds(0.0)) {
     RCLCPP_DEBUG_STREAM(
-      rclcpp::get_logger("DynamicAvoidance.resample"),
-      "failed to interpolate path by time!"
-        << std::endl
-        << "query time: " << rel_time.seconds());
+      rclcpp::get_logger("DynamicAvoidance.resample"), "failed to interpolate path by time!"
+                                                         << std::endl
+                                                         << "query time: " << rel_time.seconds());
 
     return {};
   }
@@ -137,7 +136,8 @@ boost::optional<geometry_msgs::msg::Pose> lerpByTimeStamp(
       rclcpp::get_logger("DynamicAvoidance.resample"),
       "failed to interpolate path by time!"
         << std::endl
-        << "path max duration: " << path.path.size() * rclcpp::Duration(path.time_step).seconds() << std::endl
+        << "path max duration: " << path.path.size() * rclcpp::Duration(path.time_step).seconds()
+        << std::endl
         << "query time       : " << rel_time.seconds());
 
     return {};
@@ -167,7 +167,8 @@ inline void convertEulerAngleToMonotonic(std::vector<double> & a)
 }
 
 autoware_auto_planning_msgs::msg::Trajectory applyLinearInterpolation(
-  const std::vector<double> & base_index, const autoware_auto_planning_msgs::msg::Trajectory & base_trajectory,
+  const std::vector<double> & base_index,
+  const autoware_auto_planning_msgs::msg::Trajectory & base_trajectory,
   const std::vector<double> & out_index, const bool use_spline_for_pose)
 {
   std::vector<double> px, py, pz, pyaw, tlx, taz, alx, aaz;
diff --git a/planning/obstacle_velocity_planner/src/velocity_optimizer.cpp b/planning/obstacle_velocity_planner/src/velocity_optimizer.cpp
index 475c4b9b88de9..f5e9b366b38e9 100644
--- a/planning/obstacle_velocity_planner/src/velocity_optimizer.cpp
+++ b/planning/obstacle_velocity_planner/src/velocity_optimizer.cpp
@@ -15,6 +15,7 @@
 #include "obstacle_velocity_planner/velocity_optimizer.hpp"
 
 #include <eigen3/Eigen/Core>
+
 #include <iostream>
 
 VelocityOptimizer::VelocityOptimizer(
@@ -82,10 +83,12 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
   // + over_j_ideal^2
   constexpr double MINIMUM_MAX_S_BOUND = 0.01;
   for (size_t i = 0; i < N; ++i) {
-    const double dt = i < N-1 ? time_vec.at(i+1) - time_vec.at(i) : time_vec.at(N-1) - time_vec.at(N-2);
+    const double dt =
+      i < N - 1 ? time_vec.at(i + 1) - time_vec.at(i) : time_vec.at(N - 1) - time_vec.at(N - 2);
     const double max_s = std::max(s_boundary.at(i).max_s, MINIMUM_MAX_S_BOUND);
-    //const double v_coeff = v0/(2*std::fabs(a_min)) + t_idling;
-    const double v_coeff = v0/(2*std::fabs(a_min)) + a_max * t_idling / std::fabs(a_min) + t_idling;
+    // const double v_coeff = v0/(2*std::fabs(a_min)) + t_idling;
+    const double v_coeff =
+      v0 / (2 * std::fabs(a_min)) + a_max * t_idling / std::fabs(a_min) + t_idling;
     if (s_boundary.at(i).is_object) {
       q.at(IDX_S0 + i) += -max_s_weight_ / max_s * dt;
       q.at(IDX_V0 + i) += -max_s_weight_ / max_s * v_coeff * dt;
@@ -97,17 +100,20 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
   }
 
   for (size_t i = 0; i < N; ++i) {
-    const double dt = i < N-1 ? time_vec.at(i+1) - time_vec.at(i) : time_vec.at(N-1) - time_vec.at(N-2);
+    const double dt =
+      i < N - 1 ? time_vec.at(i + 1) - time_vec.at(i) : time_vec.at(N - 1) - time_vec.at(N - 2);
     const double max_s = std::max(s_boundary.at(i).max_s, MINIMUM_MAX_S_BOUND);
-    P(IDX_OVER_S_SAFETY0 + i, IDX_OVER_S_SAFETY0 + i) += over_s_safety_weight_ / (max_s * max_s) * dt;
+    P(IDX_OVER_S_SAFETY0 + i, IDX_OVER_S_SAFETY0 + i) +=
+      over_s_safety_weight_ / (max_s * max_s) * dt;
     P(IDX_OVER_S_IDEAL0 + i, IDX_OVER_S_IDEAL0 + i) += over_s_ideal_weight_ / (max_s * max_s) * dt;
     P(IDX_OVER_V0 + i, IDX_OVER_V0 + i) += over_v_weight_ / (v_max * v_max) * dt;
     P(IDX_OVER_A0 + i, IDX_OVER_A0 + i) += over_a_weight_ / a_range * dt;
     P(IDX_OVER_J0 + i, IDX_OVER_J0 + i) += over_j_weight_ / j_range * dt;
 
     if (s_boundary.at(i).is_object) {
-      //const double v_coeff = v0/(2*std::fabs(a_min)) + t_idling;
-      const double v_coeff = v0/(2*std::fabs(a_min)) + a_max * t_idling / std::fabs(a_min) + t_idling;
+      // const double v_coeff = v0/(2*std::fabs(a_min)) + t_idling;
+      const double v_coeff =
+        v0 / (2 * std::fabs(a_min)) + a_max * t_idling / std::fabs(a_min) + t_idling;
       P(IDX_S0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * dt;
       P(IDX_V0 + i, IDX_V0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * v_coeff * dt;
       P(IDX_S0 + i, IDX_V0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * dt;
@@ -116,13 +122,14 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
       P(IDX_S0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * dt;
     }
 
-    P(IDX_V0 + i, IDX_V0 + i) += max_v_weight_ / (v_max *v_max) * dt;
+    P(IDX_V0 + i, IDX_V0 + i) += max_v_weight_ / (v_max * v_max) * dt;
   }
 
   // Constraint
   size_t constr_idx = 0;
 
-  // Safety Position Constraint: s_boundary_min < s_i + v_i*t_dangerous - over_s_safety_i < s_boundary_max
+  // Safety Position Constraint: s_boundary_min < s_i + v_i*t_dangerous - over_s_safety_i <
+  // s_boundary_max
   for (size_t i = 0; i < N; ++i, ++constr_idx) {
     A(constr_idx, IDX_S0 + i) = 1.0;  // s_i
     if (s_boundary.at(i).is_object) {
@@ -133,10 +140,12 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
     lower_bound.at(constr_idx) = 0.0;
   }
 
-  // Ideal Position Constraint: s_boundary_min < s_i  + v_i * t_idling + v0*v_i/(2*|a_min|) - over_s_ideal_i < s_boundary_max
+  // Ideal Position Constraint: s_boundary_min < s_i  + v_i * t_idling + v0*v_i/(2*|a_min|) -
+  // over_s_ideal_i < s_boundary_max
   for (size_t i = 0; i < N; ++i, ++constr_idx) {
-    //const double v_coeff = v0/(2*std::fabs(a_min)) + t_idling;
-    const double v_coeff = v0/(2*std::fabs(a_min)) + a_max * t_idling / std::fabs(a_min) + t_idling;
+    // const double v_coeff = v0/(2*std::fabs(a_min)) + t_idling;
+    const double v_coeff =
+      v0 / (2 * std::fabs(a_min)) + a_max * t_idling / std::fabs(a_min) + t_idling;
     A(constr_idx, IDX_S0 + i) = 1.0;  // s_i
     if (s_boundary.at(i).is_object) {
       A(constr_idx, IDX_V0 + i) = v_coeff;  // v_i * (t_idling + v0/(2*|a_min|))
@@ -150,7 +159,7 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
   for (size_t i = 0; i < N; ++i, ++constr_idx) {
     A(constr_idx, IDX_V0 + i) = 1.0;        // v_i
     A(constr_idx, IDX_OVER_V0 + i) = -1.0;  // over_v_i
-    upper_bound.at(constr_idx) = i == N-1 ? 0.0 : v_max;
+    upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : v_max;
     lower_bound.at(constr_idx) = 0.0;
   }
 
@@ -158,22 +167,22 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
   for (size_t i = 0; i < N; ++i, ++constr_idx) {
     A(constr_idx, IDX_A0 + i) = 1.0;        // a_i
     A(constr_idx, IDX_OVER_A0 + i) = -1.0;  // over_a_i
-    upper_bound.at(constr_idx) =  i == N-1 ? 0.0 : a_max;
-    lower_bound.at(constr_idx) =  i == N-1 ? 0.0 : a_min;
+    upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : a_max;
+    lower_bound.at(constr_idx) = i == N - 1 ? 0.0 : a_min;
   }
 
   // Soft Jerk Constraint: j_min < j_i - over_j_i < j_max
   for (size_t i = 0; i < N; ++i, ++constr_idx) {
     A(constr_idx, IDX_J0 + i) = 1.0;        // j_i
     A(constr_idx, IDX_OVER_J0 + i) = -1.0;  // over_j_i
-    upper_bound.at(constr_idx) = i == N-1 ? 0.0 : j_max;
-    lower_bound.at(constr_idx) = i == N-1 ? 0.0 : j_min;
+    upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : j_max;
+    lower_bound.at(constr_idx) = i == N - 1 ? 0.0 : j_min;
   }
 
   // Dynamic Constraint
   // s_i+1 = s_i + v_i * dt + 0.5 * a_i * dt^2 + 1/6 * j_i * dt^3
   for (size_t i = 0; i < N - 1; ++i, ++constr_idx) {
-    const double dt = time_vec.at(i+1) - time_vec.at(i);
+    const double dt = time_vec.at(i + 1) - time_vec.at(i);
     A(constr_idx, IDX_S0 + i + 1) = 1.0;                    // s_i+1
     A(constr_idx, IDX_S0 + i) = -1.0;                       // -s_i
     A(constr_idx, IDX_V0 + i) = -dt;                        // -v_i*dt
@@ -185,7 +194,7 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
 
   // v_i+1 = v_i + a_i * dt + 0.5 * j_i * dt^2
   for (size_t i = 0; i < N - 1; ++i, ++constr_idx) {
-    const double dt = time_vec.at(i+1) - time_vec.at(i);
+    const double dt = time_vec.at(i + 1) - time_vec.at(i);
     A(constr_idx, IDX_V0 + i + 1) = 1.0;         // v_i+1
     A(constr_idx, IDX_V0 + i) = -1.0;            // -v_i
     A(constr_idx, IDX_A0 + i) = -dt;             // -a_i * dt
@@ -196,7 +205,7 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
 
   // a_i+1 = a_i + j_i * dt
   for (size_t i = 0; i < N - 1; ++i, ++constr_idx) {
-    const double dt = time_vec.at(i+1) - time_vec.at(i);
+    const double dt = time_vec.at(i + 1) - time_vec.at(i);
     A(constr_idx, IDX_A0 + i + 1) = 1.0;  // a_i+1
     A(constr_idx, IDX_A0 + i) = -1.0;     // -a_i
     A(constr_idx, IDX_J0 + i) = -dt;      // -j_i * dt