Merge pull request #823 from tier4/sync-upstream

chore: sync upstream

control/control_validator/src/control_validator.cpp

@@ -163,7 +163,9 @@ void ControlValidator::publishDebugInfo()
 void ControlValidator::validate(const Trajectory & predicted_trajectory)
 {
   if (predicted_trajectory.points.size() < 2) {
-    RCLCPP_ERROR(get_logger(), "predicted_trajectory size is less than 2. Cannot validate.");
+    RCLCPP_ERROR_THROTTLE(
+      get_logger(), *get_clock(), 1000,
+      "predicted_trajectory size is less than 2. Cannot validate.");
     return;
   }
 

control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp

@@ -104,35 +104,38 @@ void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk(
     std::clamp(static_cast<double>(input.longitudinal.jerk), -lon_jerk_lim, lon_jerk_lim);
 }
 
+// Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the
+// filtered steering angle oscillates if the input velocity oscillates.
 void VehicleCmdFilter::limitLateralWithLatAcc(
   [[maybe_unused]] const double dt, AckermannControlCommand & input) const
 {
   const auto lat_acc_lim = getLatAccLim();
 
-  double latacc = calcLatAcc(input);
+  double latacc = calcLatAcc(input, current_speed_);
   if (std::fabs(latacc) > lat_acc_lim) {
-    double v_sq =
-      std::max(static_cast<double>(input.longitudinal.speed * input.longitudinal.speed), 0.001);
+    double v_sq = std::max(static_cast<double>(current_speed_ * current_speed_), 0.001);
     double steer_lim = std::atan(lat_acc_lim * param_.wheel_base / v_sq);
     input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim;
   }
 }
 
+// Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the
+// filtered steering angle oscillates if the input velocity oscillates.
 void VehicleCmdFilter::limitLateralWithLatJerk(
   const double dt, AckermannControlCommand & input) const
 {
-  double curr_latacc = calcLatAcc(input);
-  double prev_latacc = calcLatAcc(prev_cmd_);
+  double curr_latacc = calcLatAcc(input, current_speed_);
+  double prev_latacc = calcLatAcc(prev_cmd_, current_speed_);
 
   const auto lat_jerk_lim = getLatJerkLim();
 
   const double latacc_max = prev_latacc + lat_jerk_lim * dt;
   const double latacc_min = prev_latacc - lat_jerk_lim * dt;
 
   if (curr_latacc > latacc_max) {
-    input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max);
+    input.lateral.steering_tire_angle = calcSteerFromLatacc(current_speed_, latacc_max);
   } else if (curr_latacc < latacc_min) {
-    input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_min);
+    input.lateral.steering_tire_angle = calcSteerFromLatacc(current_speed_, latacc_min);
   }
 }
 
@@ -205,6 +208,11 @@ double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd) const
   return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base;
 }
 
+double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd, const double v) const
+{
+  return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base;
+}
+
 double VehicleCmdFilter::limitDiff(
   const double curr, const double prev, const double diff_lim) const
 {

control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp

@@ -81,6 +81,7 @@ class VehicleCmdFilter
   bool setParameterWithValidation(const VehicleCmdFilterParam & p);
 
   double calcLatAcc(const AckermannControlCommand & cmd) const;
+  double calcLatAcc(const AckermannControlCommand & cmd, const double v) const;
   double calcSteerFromLatacc(const double v, const double latacc) const;
   double limitDiff(const double curr, const double prev, const double diff_lim) const;
 

control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp

@@ -150,7 +150,6 @@ class PubSubNode : public rclcpp::Node
       if (!cmd_history_.empty()) {  // ego moves as commanded.
         msg.twist.twist.linear.x =
           cmd_history_.back()->longitudinal.speed;  // ego moves as commanded.
-      } else {
       }
       pub_odom_->publish(msg);
     }
@@ -238,16 +237,19 @@ class PubSubNode : public rclcpp::Node
     const auto lon_jerk = (lon_acc - cmd_prev->longitudinal.acceleration) / dt;
     const auto lat_acc =
       lon_vel * lon_vel * std::tan(cmd_curr->lateral.steering_tire_angle) / wheelbase;
-    const auto prev_lon_vel = cmd_prev->longitudinal.speed;
+
+    // TODO(Horibe): prev_lat_acc should use the previous velocity, but use the current velocity
+    // since the current filtering logic uses the current velocity.
     const auto prev_lat_acc =
-      prev_lon_vel * prev_lon_vel * std::tan(cmd_prev->lateral.steering_tire_angle) / wheelbase;
+      lon_vel * lon_vel * std::tan(cmd_prev->lateral.steering_tire_angle) / wheelbase;
     const auto lat_jerk = (lat_acc - prev_lat_acc) / dt;
 
     /* debug print */
     // const auto steer = cmd_curr->lateral.steering_tire_angle;
     // PRINT_VALUES(
-    //   dt, lon_vel, lon_acc, lon_jerk, lat_acc, lat_jerk, steer, max_lon_acc_lim,
-    //   max_lon_jerk_lim, max_lat_acc_lim, max_lat_jerk_lim);
+    //   dt, i_curr, i_prev, steer, lon_vel, prev_lon_vel, lon_acc, lon_jerk, lat_acc, prev_lat_acc,
+    //   prev_lat_acc2, lat_jerk, max_lon_acc_lim, max_lon_jerk_lim, max_lat_acc_lim,
+    //   max_lat_jerk_lim);
 
     // Output command must be smaller than maximum limit.
     // TODO(Horibe): check for each velocity range.
@@ -368,6 +370,16 @@ TEST_P(TestFixture, CheckFilterForSinCmd)
   [[maybe_unused]] auto b = std::system("ros2 node info /test_vehicle_cmd_gate_filter_pubsub");
   [[maybe_unused]] auto c = std::system("ros2 node info /vehicle_cmd_gate");
 
+  // std::cerr << "speed signal: " << cmd_generator_.p_.velocity.max << " * sin(2pi * "
+  //           << cmd_generator_.p_.velocity.freq << " * dt + " << cmd_generator_.p_.velocity.bias
+  //           << ")" << std::endl;
+  // std::cerr << "accel signal: " << cmd_generator_.p_.acceleration.max << " * sin(2pi * "
+  //           << cmd_generator_.p_.acceleration.freq << " * dt + "
+  //           << cmd_generator_.p_.acceleration.bias << ")" << std::endl;
+  // std::cerr << "steer signal: " << cmd_generator_.p_.steering.max << " * sin(2pi * "
+  //           << cmd_generator_.p_.steering.freq << " * dt + " << cmd_generator_.p_.steering.bias
+  //           << ")" << std::endl;
+
   for (size_t i = 0; i < 100; ++i) {
     const bool reset_clock = (i == 0);
     const auto cmd = cmd_generator_.calcSinWaveCommand(reset_clock);
@@ -384,17 +396,23 @@ TEST_P(TestFixture, CheckFilterForSinCmd)
 };
 
 // High frequency, large value
-CmdParams p1 = {/*steer*/ {10, 1, 0}, /*velocity*/ {10, 1.2, 0}, /*acc*/ {5, 1.5, 2}};
+CmdParams p1 = {/*steer*/ {0.5, 1, 0}, /*velocity*/ {10, 0.0, 0}, /*acc*/ {5, 1.5, 2}};
 INSTANTIATE_TEST_SUITE_P(TestParam1, TestFixture, ::testing::Values(p1));
 
 // High frequency, normal value
-CmdParams p2 = {/*steer*/ {1.5, 2, 1}, /*velocity*/ {5, 1.0, 0}, /*acc*/ {2.0, 3.0, 2}};
+CmdParams p2 = {/*steer*/ {0.5, 2, 1}, /*velocity*/ {5, 1.0, 0}, /*acc*/ {2.0, 3.0, 2}};
 INSTANTIATE_TEST_SUITE_P(TestParam2, TestFixture, ::testing::Values(p2));
 
 // High frequency, small value
-CmdParams p3 = {/*steer*/ {1.5, 3, 2}, /*velocity*/ {2, 3, 0}, /*acc*/ {0.5, 3, 2}};
+CmdParams p3 = {/*steer*/ {0.5, 3, 2}, /*velocity*/ {2, 3, 0}, /*acc*/ {0.5, 3, 2}};
 INSTANTIATE_TEST_SUITE_P(TestParam3, TestFixture, ::testing::Values(p3));
 
 // Low frequency
-CmdParams p4 = {/*steer*/ {10, 0.1, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}};
+CmdParams p4 = {/*steer*/ {0.5, 0.1, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}};
 INSTANTIATE_TEST_SUITE_P(TestParam4, TestFixture, ::testing::Values(p4));
+
+// Large steer, large velocity -> this test fails.
+// Lateral acceleration and lateral jerk affect both steer and velocity, and if both steer and
+// velocity changes significantly, the current logic cannot adequately handle the situation.
+// CmdParams p5 = {/*steer*/ {10.0, 1.0, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}};
+// INSTANTIATE_TEST_SUITE_P(TestParam5, TestFixture, ::testing::Values(p5));

control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp

@@ -56,12 +56,20 @@ AckermannControlCommand genCmd(double s, double sr, double v, double a)
   return cmd;
 }
 
+// calc from ego velocity
+double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase, const double ego_v)
+{
+  return ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase;
+}
+
+// calc from command velocity
 double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase)
 {
   double v = cmd.longitudinal.speed;
   return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase;
 }
 
+// calc from command velocity
 double calcLatJerk(
   const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd,
   const double wheelbase, const double dt)
@@ -75,14 +83,28 @@ double calcLatJerk(
   return (curr - prev) / dt;
 }
 
+// calc from ego velocity
+double calcLatJerk(
+  const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd,
+  const double wheelbase, const double dt, const double ego_v)
+{
+  const auto prev = ego_v * ego_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase;
+
+  const auto curr = ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase;
+
+  return (curr - prev) / dt;
+}
+
 void test_1d_limit(
-  double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, double STEER_DIFF,
-  const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd)
+  double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM,
+  double STEER_DIFF, const AckermannControlCommand & prev_cmd,
+  const AckermannControlCommand & raw_cmd)
 {
   const double WHEELBASE = 3.0;
   const double DT = 0.1;  // [s]
 
   vehicle_cmd_gate::VehicleCmdFilter filter;
+  filter.setCurrentSpeed(ego_v);
   setFilterParams(
     filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, WHEELBASE);
   filter.setPrevCmd(prev_cmd);
@@ -153,8 +175,8 @@ void test_1d_limit(
   {
     auto filtered_cmd = raw_cmd;
     filter.limitLateralWithLatAcc(DT, filtered_cmd);
-    const double filtered_latacc = calcLatAcc(filtered_cmd, WHEELBASE);
-    const double raw_latacc = calcLatAcc(raw_cmd, WHEELBASE);
+    const double filtered_latacc = calcLatAcc(filtered_cmd, WHEELBASE, ego_v);
+    const double raw_latacc = calcLatAcc(raw_cmd, WHEELBASE, ego_v);
 
     // check if the filtered value does not exceed the limit.
     ASSERT_LT_NEAR(std::abs(filtered_latacc), LAT_A_LIM);
@@ -169,9 +191,9 @@ void test_1d_limit(
   {
     auto filtered_cmd = raw_cmd;
     filter.limitLateralWithLatJerk(DT, filtered_cmd);
-    const double prev_lat_acc = calcLatAcc(prev_cmd, WHEELBASE);
-    const double filtered_lat_acc = calcLatAcc(filtered_cmd, WHEELBASE);
-    const double raw_lat_acc = calcLatAcc(raw_cmd, WHEELBASE);
+    const double prev_lat_acc = calcLatAcc(prev_cmd, WHEELBASE, ego_v);
+    const double filtered_lat_acc = calcLatAcc(filtered_cmd, WHEELBASE, ego_v);
+    const double raw_lat_acc = calcLatAcc(raw_cmd, WHEELBASE, ego_v);
     const double filtered_lateral_jerk = (filtered_lat_acc - prev_lat_acc) / DT;
 
     // check if the filtered value does not exceed the limit.
@@ -211,6 +233,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilter)
   const std::vector<double> lat_a_arr = {0.01, 1.0, 100.0};
   const std::vector<double> lat_j_arr = {0.01, 1.0, 100.0};
   const std::vector<double> steer_diff_arr = {0.01, 1.0, 100.0};
+  const std::vector<double> ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0};
 
   const std::vector<AckermannControlCommand> prev_cmd_arr = {
     genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)};
@@ -226,7 +249,9 @@ TEST(VehicleCmdFilter, VehicleCmdFilter)
             for (const auto & prev_cmd : prev_cmd_arr) {
               for (const auto & raw_cmd : raw_cmd_arr) {
                 for (const auto & steer_diff : steer_diff_arr) {
-                  test_1d_limit(v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd);
+                  for (const auto & ego_v : ego_v_arr) {
+                    test_1d_limit(ego_v, v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd);
+                  }
                 }
               }
             }
@@ -371,66 +396,72 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate)
   // lateral acc lim
   // p.reference_speed_points = std::vector<double>{2.0, 4.0, 10.0};
   // p.lat_acc_lim = std::vector<double>{0.1, 0.2, 0.3};
-  const auto _calcLatAcc = [&](const auto & cmd) { return calcLatAcc(cmd, WHEELBASE); };
+  const auto _calcLatAcc = [&](const auto & cmd, const double ego_v) {
+    return calcLatAcc(cmd, WHEELBASE, ego_v);
+  };
   {
+    // since the lateral acceleration is 0 when the velocity is 0, the target value is 0 only in
+    // this case
     set_speed_and_reset_prev(0.0);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 0.0), 0.0, ep);
 
     set_speed_and_reset_prev(2.0);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 2.0), 0.1, ep);
 
     set_speed_and_reset_prev(3.0);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.15, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 3.0), 0.15, ep);
 
     set_speed_and_reset_prev(5.0);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.1 / 6.0, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 5.0), 0.2 + 0.1 / 6.0, ep);
 
     set_speed_and_reset_prev(8.0);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.4 / 6.0, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 8.0), 0.2 + 0.4 / 6.0, ep);
 
     set_speed_and_reset_prev(10.0);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 10.0), 0.3, ep);
 
     set_speed_and_reset_prev(15.0);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 15.0), 0.3, ep);
   }
 
   // lateral jerk lim
   // p.reference_speed_points = std::vector<double>{2.0, 4.0, 10.0};
   // p.lat_jerk_lim = std::vector<double>{0.9, 0.7, 0.1};
-  const auto _calcLatJerk = [&](const auto & cmd) {
-    return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT);
+  const auto _calcLatJerk = [&](const auto & cmd, const double ego_v) {
+    return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT, ego_v);
   };
   {
+    // since the lateral acceleration and jerk is 0 when the velocity is 0, the target value is 0
+    // only in this case
     set_speed_and_reset_prev(0.0);
-    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep);
+    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 0.0), 0.0, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 0.0), DT * 0.0, ep);
 
     set_speed_and_reset_prev(2.0);
-    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep);
+    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 2.0), 0.9, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 2.0), DT * 0.9, ep);
 
     set_speed_and_reset_prev(3.0);
-    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.8, ep);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.8, ep);
+    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 3.0), 0.8, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 3.0), DT * 0.8, ep);
 
     set_speed_and_reset_prev(5.0);
     const auto expect_v5 = 0.7 - 0.6 * (1.0 / 6.0);
-    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v5, ep);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v5, ep);
+    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 5.0), expect_v5, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 5.0), DT * expect_v5, ep);
 
     set_speed_and_reset_prev(8.0);
     const auto expect_v8 = 0.7 - 0.6 * (4.0 / 6.0);
-    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v8, ep);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v8, ep);
+    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 8.0), expect_v8, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 8.0), DT * expect_v8, ep);
 
     set_speed_and_reset_prev(10.0);
-    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep);
+    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 10.0), 0.1, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 10.0), DT * 0.1, ep);
 
     set_speed_and_reset_prev(15.0);
-    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep);
-    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep);
+    EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 15.0), 0.1, ep);
+    EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 15.0), DT * 0.1, ep);
   }
 
   // steering diff lim

launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml

@@ -4,8 +4,8 @@
     <push-ros-namespace namespace="ar_tag_based_localizer"/>
     <group>
       <include file="$(find-pkg-share ar_tag_based_localizer)/launch/ar_tag_based_localizer.launch.xml">
-        <arg name="input_image" value="/sensing/camera/front/image"/>
-        <arg name="input_camera_info" value="/sensing/camera/front/image/info"/>
+        <arg name="input_image" value="/sensing/camera/traffic_light/image_raw"/>
+        <arg name="input_camera_info" value="/sensing/camera/traffic_light/camera_info"/>
         <arg name="output_image" value="/localization/pose_estimator/ar_tag_detected_image"/>
         <arg name="output_pose_with_covariance" value="/localization/pose_estimator/pose_with_covariance"/>
         <arg name="param_file" value="$(find-pkg-share ar_tag_based_localizer)/config/ar_tag_based_localizer.param.yaml"/>

launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml

@@ -15,6 +15,7 @@
   <arg name="external/traffic_signals" default="/perception/traffic_light_recognition/external/traffic_signals"/>
   <arg name="output/traffic_signals" default="/perception/traffic_light_recognition/traffic_signals"/>
   <arg name="use_crosswalk_traffic_light_estimator" default="true" description="output pedestrian's traffic light signals"/>
+  <arg name="crosswalk_traffic_light_estimator_param_file" default="$(find-pkg-share crosswalk_traffic_light_estimator)/config/crosswalk_traffic_light_estimator.param.yaml"/>
   <arg name="image_number" default="1" description="choose image raw number(1-2)"/>
   <arg name="namespace1" default="camera6"/>
   <arg name="namespace2" default="camera7"/>
@@ -143,6 +144,7 @@
       <remap from="~/input/route" to="/planning/mission_planning/route"/>
       <remap from="~/input/classified/traffic_signals" to="$(var fusion/traffic_signals)"/>
       <remap from="~/output/traffic_signals" to="$(var internal/traffic_signals)"/>
+      <param from="$(var crosswalk_traffic_light_estimator_param_file)"/>
     </node>
   </group>