fix(traffic_simulator): fix traffic_lights tests

simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_publisher.hpp

@@ -15,6 +15,8 @@
 #ifndef TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_
 #define TRAFFIC_SIMULATOR__TRAFFIC_LIGHTS__TRAFFIC_LIGHT_PUBLISHER_HPP_
 
+#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
@@ -29,6 +31,14 @@ class TrafficLightPublisherBase
 public:
   virtual auto publish(const TrafficLightsBase & traffic_lights) const -> void = 0;
   virtual ~TrafficLightPublisherBase() = default;
+
+  static auto generateAutowareAutoPerceptionMsg(
+    const rclcpp::Time & current_ros_time, const TrafficLightsBase & traffic_lights)
+    -> autoware_auto_perception_msgs::msg::TrafficSignalArray;
+
+  static auto generateAutowarePerceptionMsg(
+    const rclcpp::Time & current_ros_time, const TrafficLightsBase & traffic_lights)
+    -> autoware_perception_msgs::msg::TrafficSignalArray;
 };
 
 template <typename MessageType>

simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp

@@ -12,22 +12,20 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
-#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <simulation_interface/conversions.hpp>
 #include <traffic_simulator/traffic_lights/traffic_light_publisher.hpp>
 
 namespace traffic_simulator
 {
-template <>
-auto TrafficLightPublisher<autoware_auto_perception_msgs::msg::TrafficSignalArray>::publish(
-  const TrafficLightsBase & traffic_lights) const -> void
+auto TrafficLightPublisherBase::generateAutowareAutoPerceptionMsg(
+  const rclcpp::Time & current_ros_time, const TrafficLightsBase & traffic_lights)
+  -> autoware_auto_perception_msgs::msg::TrafficSignalArray
 {
   const auto states_as_proto_request = traffic_lights.generateUpdateTrafficLightsRequest();
   autoware_auto_perception_msgs::msg::TrafficSignalArray message;
 
   message.header.frame_id = "camera_link";  // DIRTY HACK!!!
-  message.header.stamp = clock_ptr_->now();
+  message.header.stamp = current_ros_time;
 
   using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal;
   using TrafficLightBulbType = TrafficLightType::_lights_type::value_type;
@@ -41,17 +39,17 @@ auto TrafficLightPublisher<autoware_auto_perception_msgs::msg::TrafficSignalArra
     }
     message.signals.push_back(traffic_light_message);
   }
-  traffic_light_state_array_publisher_->publish(message);
+  return message;
 }
 
-template <>
-auto TrafficLightPublisher<autoware_perception_msgs::msg::TrafficSignalArray>::publish(
-  const TrafficLightsBase & traffic_lights) const -> void
+auto TrafficLightPublisherBase::generateAutowarePerceptionMsg(
+  const rclcpp::Time & current_ros_time, const TrafficLightsBase & traffic_lights)
+  -> autoware_perception_msgs::msg::TrafficSignalArray
 {
   const auto states_as_proto_request = traffic_lights.generateUpdateTrafficLightsRequest();
   autoware_perception_msgs::msg::TrafficSignalArray message;
 
-  message.stamp = clock_ptr_->now();
+  message.stamp = current_ros_time;
 
   using TrafficLightType = autoware_perception_msgs::msg::TrafficSignal;
   using TrafficLightBulbType =
@@ -71,7 +69,23 @@ auto TrafficLightPublisher<autoware_perception_msgs::msg::TrafficSignalArray>::p
       }
     }
   }
-  traffic_light_state_array_publisher_->publish(message);
+  return message;
+}
+
+template <>
+auto TrafficLightPublisher<autoware_auto_perception_msgs::msg::TrafficSignalArray>::publish(
+  const TrafficLightsBase & traffic_lights) const -> void
+{
+  traffic_light_state_array_publisher_->publish(
+    generateAutowareAutoPerceptionMsg(clock_ptr_->now(), traffic_lights));
+}
+
+template <>
+auto TrafficLightPublisher<autoware_perception_msgs::msg::TrafficSignalArray>::publish(
+  const TrafficLightsBase & traffic_lights) const -> void
+{
+  traffic_light_state_array_publisher_->publish(
+    generateAutowarePerceptionMsg(clock_ptr_->now(), traffic_lights));
 }
 
 template <>

simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp

@@ -17,6 +17,7 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 #include <std_msgs/msg/header.hpp>
+#include <traffic_simulator/traffic_lights/traffic_light_publisher.hpp>
 #include <traffic_simulator/traffic_lights/traffic_lights.hpp>
 #include <traffic_simulator/traffic_lights/traffic_lights_base.hpp>
 
@@ -422,7 +423,8 @@ TYPED_TEST(TrafficLightsInternalTest, generateAutowarePerceptionMsg)
   this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle");
   this->lights->setTrafficLightsConfidence(this->id, 0.7);
 
-  const auto msg = this->lights->generateAutowarePerceptionMsg();
+  const auto msg = traffic_simulator::TrafficLightPublisherBase::generateAutowarePerceptionMsg(
+    this->node_ptr->get_clock()->now(), *this->lights);
 
   const double expected_time =
     static_cast<double>(getTime(this->node_ptr->get_clock()->now())) * 1e-9;
@@ -452,7 +454,8 @@ TYPED_TEST(TrafficLightsInternalTest, generateAutowareAutoPerceptionMsg)
   this->lights->setTrafficLightsState(this->id, "red solidOn circle, yellow flashing circle");
   this->lights->setTrafficLightsConfidence(this->id, 0.7);
 
-  const auto msg = this->lights->generateAutowareAutoPerceptionMsg();
+  const auto msg = traffic_simulator::TrafficLightPublisherBase::generateAutowareAutoPerceptionMsg(
+    this->node_ptr->get_clock()->now(), *this->lights);
 
   const double expected_time =
     static_cast<double>(getTime(this->node_ptr->get_clock()->now())) * 1e-9;