Add tests and some fixes

.gitignore

@@ -52,4 +52,7 @@ build/
 install/
 log/
 
+# coverage result
+lcov/
+
 # Don't ignore src/ because full-text search won't work.

.lcovrc

@@ -0,0 +1,163 @@
+#
+# /etc/lcovrc - system-wide defaults for LCOV
+#
+# To change settings for a single user, place a customized copy of this file
+# at location ~/.lcovrc
+#
+
+# Specify an external style sheet file (same as --css-file option of genhtml)
+#genhtml_css_file = gcov.css
+
+# Specify coverage rate limits (in %) for classifying file entries
+# HI:   hi_limit <= rate <= 100         graph color: green
+# MED: med_limit <= rate <  hi_limit    graph color: orange
+# LO:         0  <= rate <  med_limit   graph color: red
+genhtml_hi_limit = 90
+genhtml_med_limit = 75
+
+# Width of line coverage field in source code view
+genhtml_line_field_width = 12
+
+# Width of branch coverage field in source code view
+genhtml_branch_field_width = 16
+
+# Width of overview image (used by --frames option of genhtml)
+genhtml_overview_width = 80
+
+# Resolution of overview navigation: this number specifies the maximum
+# difference in lines between the position a user selected from the overview
+# and the position the source code window is scrolled to (used by --frames
+# option of genhtml)
+genhtml_nav_resolution = 4
+
+# Clicking a line in the overview image should show the source code view at
+# a position a bit further up so that the requested line is not the first
+# line in the window. This number specifies that offset in lines (used by
+# --frames option of genhtml)
+genhtml_nav_offset = 10
+
+# Do not remove unused test descriptions if non-zero (same as
+# --keep-descriptions option of genhtml)
+genhtml_keep_descriptions = 0
+
+# Do not remove prefix from directory names if non-zero (same as --no-prefix
+# option of genhtml)
+genhtml_no_prefix = 0
+
+# Do not create source code view if non-zero (same as --no-source option of
+# genhtml)
+genhtml_no_source = 0
+
+# Replace tabs with number of spaces in source view (same as --num-spaces
+# option of genhtml)
+genhtml_num_spaces = 8
+
+# Highlight lines with converted-only data if non-zero (same as --highlight
+# option of genhtml)
+genhtml_highlight = 0
+
+# Include color legend in HTML output if non-zero (same as --legend option of
+# genhtml)
+genhtml_legend = 0
+
+# Use FILE as HTML prolog for generated pages (same as --html-prolog option of
+# genhtml)
+#genhtml_html_prolog = FILE
+
+# Use FILE as HTML epilog for generated pages (same as --html-epilog option of
+# genhtml)
+#genhtml_html_epilog = FILE
+
+# Use custom filename extension for pages (same as --html-extension option of
+# genhtml)
+#genhtml_html_extension = html
+
+# Compress all generated html files with gzip.
+#genhtml_html_gzip = 1
+
+# Include sorted overview pages (can be disabled by the --no-sort option of
+# genhtml)
+genhtml_sort = 1
+
+# Include function coverage data display (can be disabled by the
+# --no-func-coverage option of genhtml)
+#genhtml_function_coverage = 1
+
+# Include branch coverage data display (can be disabled by the
+# --no-branch-coverage option of genhtml)
+genhtml_branch_coverage = 1
+
+# Specify the character set of all generated HTML pages
+genhtml_charset=UTF-8
+
+# Allow HTML markup in test case description text if non-zero
+genhtml_desc_html=0
+
+# Specify the precision for coverage rates
+#genhtml_precision=1
+
+# Location of the gcov tool (same as --gcov-info option of geninfo)
+#geninfo_gcov_tool = gcov
+
+# Adjust test names to include operating system information if non-zero
+#geninfo_adjust_testname = 0
+
+# Calculate checksum for each source code line if non-zero (same as --checksum
+# option of geninfo if non-zero, same as --no-checksum if zero)
+#geninfo_checksum = 1
+
+# Specify whether to capture coverage data for external source files (can
+# be overridden by the --external and --no-external options of geninfo/lcov)
+geninfo_external = 0
+
+# Enable libtool compatibility mode if non-zero (same as --compat-libtool option
+# of geninfo if non-zero, same as --no-compat-libtool if zero)
+#geninfo_compat_libtool = 0
+
+# Use gcov's --all-blocks option if non-zero
+#geninfo_gcov_all_blocks = 1
+
+# Specify compatiblity modes (same as --compat option of geninfo).
+#geninfo_compat = libtool=on, hammer=auto, split_crc=auto
+
+# Adjust path to source files by removing or changing path components that
+# match the specified pattern (Perl regular expression format)
+#geninfo_adjust_src_path = /tmp/build => /usr/src
+
+# Specify if geninfo should try to automatically determine the base-directory
+# when collecting coverage data.
+geninfo_auto_base = 1
+
+# Directory containing gcov kernel files
+# lcov_gcov_dir = /proc/gcov
+
+# Location of the insmod tool
+lcov_insmod_tool = /sbin/insmod
+
+# Location of the modprobe tool
+lcov_modprobe_tool = /sbin/modprobe
+
+# Location of the rmmod tool
+lcov_rmmod_tool = /sbin/rmmod
+
+# Location for temporary directories
+lcov_tmp_dir = /tmp
+
+# Show full paths during list operation if non-zero (same as --list-full-path
+# option of lcov)
+lcov_list_full_path = 0
+
+# Specify the maximum width for list output. This value is ignored when
+# lcov_list_full_path is non-zero.
+lcov_list_width = 80
+
+# Specify the maximum percentage of file names which may be truncated when
+# choosing a directory prefix in list output. This value is ignored when
+# lcov_list_full_path is non-zero.
+lcov_list_truncate_max = 20
+
+# Specify if function coverage data should be collected and processed.
+lcov_function_coverage = 1
+
+# Specify if branch coverage data should be collected and processed.
+lcov_branch_coverage = 1

obstacle_stop_planner_nodes_refine/CMakeLists.txt

@@ -21,7 +21,7 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_EXTENSIONS OFF)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic -O3)
+  add_compile_options(-Wall -Wextra -Wpedantic)
   add_compile_options(-Wno-unused-parameter)
 endif()
 

obstacle_stop_planner_nodes_refine/src/obstacle_stop_planner_node.cpp

@@ -193,7 +193,7 @@ void ObstacleStopPlannerNode::pathCallback(
     return;
   }
 
-  if (!current_velocity_ptr_ && enable_slow_down_) {
+  if (!current_velocity_ptr_) {
     RCLCPP_WARN_THROTTLE(
       get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
       "waiting for current velocity...");
@@ -208,7 +208,12 @@ void ObstacleStopPlannerNode::pathCallback(
     trajectory_set.decimate.header, pointcloud_header_,
     tf_buffer_);
 
-  const auto output_msg = planner_->updatePath(trajectory_set, self_pose, transform_stamped);
+  const auto output_msg = planner_->updatePath(
+    trajectory_set,
+    self_pose,
+    transform_stamped,
+    current_velocity_ptr_->twist.angular.x,
+    this->now());
 
   path_pub_->publish(output_msg);
   // debug_ptr_->publish();

obstacle_stop_planner_nodes_refine/test/test_obstacle_stop_planner_node.cpp

@@ -16,12 +16,64 @@
 
 #include <memory>
 #include <chrono>
+#include <string>
+#include <vector>
 
 #include "gtest/gtest.h"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
 using sensor_msgs::msg::PointCloud2;
 using autoware_planning_msgs::msg::Trajectory;
 
+
+void init_pcl_msg(
+  sensor_msgs::msg::PointCloud2 & msg,
+  const std::string & frame_id,
+  const std::size_t size)
+{
+  msg.height = 1U;
+  msg.is_bigendian = false;
+  msg.is_dense = false;
+  msg.header.frame_id = frame_id;
+  // set the fields
+  sensor_msgs::PointCloud2Modifier modifier(msg);
+  modifier.setPointCloud2Fields(
+    4U,
+    "x", 1U, sensor_msgs::msg::PointField::FLOAT32,
+    "y", 1U, sensor_msgs::msg::PointField::FLOAT32,
+    "z", 1U, sensor_msgs::msg::PointField::FLOAT32,
+    "intensity", 1U, sensor_msgs::msg::PointField::FLOAT32);
+  // allocate memory so that iterators can be used
+  modifier.resize(size);
+}
+
+Trajectory convertPointsToTrajectoryWithYaw(
+  const std::vector<Point3d> & points)
+{
+  Trajectory trajectory;
+  for (size_t i = 0; i < points.size(); i++) {
+    autoware_planning_msgs::msg::TrajectoryPoint traj_point;
+    traj_point.pose.position = autoware_utils::toMsg(points[i]);
+    double yaw = 0;
+    if (i > 0) {
+      const double dx = points[i].x() - points[i - 1].x();
+      const double dy = points[i].y() - points[i - 1].y();
+      yaw = std::atan2(dy, dx);
+    } else if (i == 0 && points.size() > 1) {
+      const double dx = points[i + 1].x() - points[i].x();
+      const double dy = points[i + 1].y() - points[i].y();
+      yaw = std::atan2(dy, dx);
+    }
+    const double roll = 0;
+    const double pitch = 0;
+    tf2::Quaternion quaternion;
+    quaternion.setRPY(roll, pitch, yaw);
+    traj_point.pose.orientation = tf2::toMsg(quaternion);
+    trajectory.points.push_back(traj_point);
+  }
+  return trajectory;
+}
 class ObstacleStopPlannerNodeTest : public ::testing::Test
 {
 public:
@@ -54,6 +106,16 @@ class ObstacleStopPlannerNodeTest : public ::testing::Test
 
     rclcpp::NodeOptions node_options{};
 
+    node_options.append_parameter_override("wheel_radius", 0.39F);
+    node_options.append_parameter_override("wheel_width", 0.42F);
+    node_options.append_parameter_override("wheel_base", 2.74F);
+    node_options.append_parameter_override("wheel_tread", 1.63F);
+    node_options.append_parameter_override("front_overhang", 1.0F);
+    node_options.append_parameter_override("rear_overhang", 1.03F);
+    node_options.append_parameter_override("left_overhang", 0.1F);
+    node_options.append_parameter_override("right_overhang", 0.1F);
+    node_options.append_parameter_override("vehicle_height", 2.5F);
+
     planner_ = std::make_shared<obstacle_stop_planner_nodes::ObstacleStopPlannerNode>(node_options);
   }
 
@@ -76,7 +138,32 @@ TEST_F(ObstacleStopPlannerNodeTest, plan_simple_trajectory)
   executor.add_node(planner_);
 
   // TODO(KS): Publish data
-
+  const uint32_t max_cloud_size = 10;
+
+  sensor_msgs::msg::PointCloud2 test_msg;
+  init_pcl_msg(test_msg, "base_link", max_cloud_size);
+  test_msg.header.stamp = fake_node_->now();
+  dummy_pointcloud_pub_->publish(test_msg);
+
+  // current velocity
+  geometry_msgs::msg::TwistStamped twist_msg;
+  twist_msg.twist.angular.x = 0.0F;
+  dummy_velocity_pub_->publish(twist_msg);
+
+  executor.spin_some();
+
+  // create trajectory
+  std::vector<Point3d> points;
+  points.emplace_back(0.0, 0.0, 0.0);
+  points.emplace_back(1.0, 0.0, 0.0);
+  points.emplace_back(2.0, 0.0, 0.0);
+  points.emplace_back(3.0, 0.0, 0.0);
+  points.emplace_back(4.0, 0.0, 0.0);
+  points.emplace_back(5.0, 0.0, 0.0);
+  auto trajectory = convertPointsToTrajectoryWithYaw(points);
+  trajectory.header.frame_id = "base_link";
+  trajectory.header.stamp = fake_node_->now();
+  dummy_path_pub_->publish(trajectory);
 
   using namespace std::chrono_literals;
   rclcpp::WallRate rate(100ms);
@@ -89,5 +176,7 @@ TEST_F(ObstacleStopPlannerNodeTest, plan_simple_trajectory)
   }
 
   // Check data
-  ASSERT_EQ(0, 1);
+  for (size_t i = 1; i < path_msg_.get().points.size(); i++) {
+    ASSERT_EQ(trajectory.points[i - 1], path_msg_.get().points[i]);
+  }
 }

obstacle_stop_planner_refine/CMakeLists.txt

@@ -21,7 +21,7 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_EXTENSIONS OFF)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic -O3)
+  add_compile_options(-Wall -Wextra -Wpedantic)
   add_compile_options(-Wno-unused-parameter)
 endif()
 

obstacle_stop_planner_refine/include/obstacle_stop_planner/control/adaptive_cruise_control.hpp

@@ -48,13 +48,15 @@ class AdaptiveCruiseController
     const Pose self_pose, const Point2d & nearest_collision_point,
     const rclcpp::Time nearest_collision_point_time,
     const autoware_perception_msgs::msg::DynamicObjectArray::SharedPtr object_ptr,
-    const geometry_msgs::msg::TwistStamped::SharedPtr current_velocity_ptr,
-    const Trajectory & input_trajectory);
+    const double current_velocity,
+    const Trajectory & input_trajectory,
+    const rclcpp::Time & current_time);
 
 private:
   // rclcpp::Publisher<autoware_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr pub_debug_;
 
-  rclcpp::Node * node_;
+  // rclcpp::Node * node_;
+  rclcpp::Time current_time_;
   /*
    * Parameter
    */

obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_stop_planner.hpp

@@ -72,7 +72,9 @@ class OBSTACLE_STOP_PLANNER_PUBLIC ObstacleStopPlanner
   Trajectory updatePath(
     const TrajectorySet & input_path,
     const Pose & self_pose,
-    const geometry_msgs::msg::TransformStamped & transform_stamped);
+    const geometry_msgs::msg::TransformStamped & transform_stamped,
+    const double current_velocity,
+    const rclcpp::Time & current_time);
 
   void updatePointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pc);
   void updateExpandStopRange(const double expand_stop_range);
@@ -94,9 +96,6 @@ class OBSTACLE_STOP_PLANNER_PUBLIC ObstacleStopPlanner
   std::unique_ptr<obstacle_stop_planner::AdaptiveCruiseController> acc_controller_;
   std::unique_ptr<obstacle_stop_planner::StopController> stop_control_;
   std::unique_ptr<obstacle_stop_planner::SlowDownController> slow_down_control_;
-  // rclcpp::Time prev_col_point_time_;
-  // pcl::PointXYZ prev_col_point_;
-  geometry_msgs::msg::TwistStamped::SharedPtr current_velocity_ptr_;
   autoware_perception_msgs::msg::DynamicObjectArray::SharedPtr object_ptr_;
 };
 }  // namespace obstacle_stop_planner

obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp

@@ -115,9 +115,9 @@ struct OBSTACLE_STOP_PLANNER_PUBLIC AdaptiveCruiseControlParameter
   //!< @brief coefficient D in PID control (used when delta_dist <0)
   double d_coeff_neg;
 
-  static constexpr double d_coeff_valid_time = 1.0;
-  static constexpr double d_coeff_valid_diff_vel = 20.0;
-  static constexpr double d_max_vel_norm = 3.0;
+  const double d_coeff_valid_time = 1.0;
+  const double d_coeff_valid_diff_vel = 20.0;
+  const double d_max_vel_norm = 3.0;
 };
 }  // namespace obstacle_stop_planner