Split constructor to some methods

simulation/traffic_simulator/include/traffic_simulator/api/api.hpp

@@ -51,51 +51,33 @@ class API
   : configuration_(configuration),
     node_parameters_(
       rclcpp::node_interfaces::get_node_parameters_interface(std::forward<NodeT>(node))),
-    entity_manager_ptr_(
-      std::make_shared<entity::EntityManager>(node, configuration, node_parameters_)),
-    traffic_lights_ptr_(std::make_shared<TrafficLights>(
-      node, entity_manager_ptr_->getHdmapUtils(),
-      getROS2Parameter<std::string>("architecture_type", "awf/universe/20240605"))),
-    traffic_controller_ptr_(std::make_shared<traffic::TrafficController>(
-      [this](const std::string & name) { despawn(name); }, entity_manager_ptr_,
-      configuration_.auto_sink_entity_types)),
     clock_pub_(rclcpp::create_publisher<rosgraph_msgs::msg::Clock>(
       node, "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(),
       rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
     debug_marker_pub_(rclcpp::create_publisher<visualization_msgs::msg::MarkerArray>(
       node, "debug_marker", rclcpp::QoS(100), rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
-    real_time_factor_subscriber(rclcpp::create_subscription<std_msgs::msg::Float64>(
+    clock_(getROS2Parameter<bool>("use_sim_time", true), std::forward<decltype(xs)>(xs)...),
+    zeromq_client_(
+      simulation_interface::protocol, configuration.simulator_host,
+      getROS2Parameter<int>("port", 5555)),
+    entity_manager_ptr_(
+      std::make_shared<entity::EntityManager>(node, configuration, node_parameters_)),
+    traffic_controller_ptr_(std::make_shared<traffic::TrafficController>(
+      [this](const std::string & name) { despawn(name); }, entity_manager_ptr_,
+      configuration.auto_sink_entity_types)),
+    traffic_lights_ptr_(std::make_shared<TrafficLights>(
+      node, entity_manager_ptr_->getHdmapUtils(),
+      getROS2Parameter<std::string>("architecture_type", "awf/universe/20240605"))),
+    real_time_factor_subscriber_(rclcpp::create_subscription<std_msgs::msg::Float64>(
       node, "/real_time_factor", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(),
       [this](const std_msgs::msg::Float64 & message) {
-        /**
-         * @note Pausing the simulation by setting the realtime_factor_ value to 0 is not supported and causes the simulation crash.
-         * For that reason, before performing the action, it needs to be ensured that the incoming request data is a positive number.
-         */
-        if (message.data >= 0.001) {
-          clock_.realtime_factor = message.data;
-          simulation_api_schema::UpdateStepTimeRequest request;
-          request.set_simulation_step_time(clock_.getStepTime());
-          zeromq_client_.call(request);
-        }
-      })),
-    clock_(node->get_parameter("use_sim_time").as_bool(), std::forward<decltype(xs)>(xs)...),
-    zeromq_client_(
-      simulation_interface::protocol, configuration_.simulator_host, getZMQSocketPort(*node))
+        return setSimulationStepTime(message.data);
+      }))
   {
+    entity_manager_ptr_->setVerbose(configuration_.verbose);
     entity_manager_ptr_->setTrafficLights(traffic_lights_ptr_);
-    setVerbose(configuration_.verbose);
-
-    if (not configuration_.standalone_mode) {
-      simulation_api_schema::InitializeRequest request;
-      request.set_initialize_time(clock_.getCurrentSimulationTime());
-      request.set_lanelet2_map_path(configuration_.lanelet2_map_path().string());
-      request.set_realtime_factor(clock_.realtime_factor);
-      request.set_step_time(clock_.getStepTime());
-      simulation_interface::toProto(
-        clock_.getCurrentRosTime(), *request.mutable_initialize_ros_time());
-      if (not zeromq_client_.call(request).result().success()) {
-        throw common::SimulationError("Failed to initialize simulator by InitializeRequest");
-      }
+    if (not init()) {
+      throw common::SimulationError("Failed to initialize simulator by InitializeRequest");
     }
   }
 
@@ -106,15 +88,12 @@ class API
     return getParameter<ParameterT>(node_parameters_, std::forward<Ts>(xs)...);
   }
 
-  template <typename Node>
-  int getZMQSocketPort(Node & node)
-  {
-    if (!node.has_parameter("port")) node.declare_parameter("port", 5555);
-    return node.get_parameter("port").as_int();
-  }
+  auto init() -> bool;
 
   auto setVerbose(const bool verbose) -> void;
 
+  auto setSimulationStepTime(const double step_time) -> bool;
+
   auto startNpcLogic() -> void;
 
   auto isNpcLogicStarted() const -> bool;

simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp

@@ -54,12 +54,10 @@ class EntityManager
     NodeT && node, const Configuration & configuration,
     const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters)
   : configuration_(configuration),
-    node_topics_interface(rclcpp::node_interfaces::get_node_topics_interface(node)),
+    clock_ptr_(node->get_clock()),
     node_parameters_(node_parameters),
     broadcaster_(node),
     base_link_broadcaster_(node),
-    clock_ptr_(node->get_clock()),
-    npc_logic_started_(false),
     entity_status_array_pub_ptr_(rclcpp::create_publisher<EntityStatusWithTrajectoryArray>(
       node, "entity/status", EntityMarkerQoS(),
       rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),

simulation/traffic_simulator/src/api/api.cpp

@@ -18,8 +18,40 @@
 namespace traffic_simulator
 {
 // global
+auto API::init() -> bool
+{
+  if (not configuration_.standalone_mode) {
+    simulation_api_schema::InitializeRequest request;
+    request.set_initialize_time(clock_.getCurrentSimulationTime());
+    request.set_lanelet2_map_path(configuration_.lanelet2_map_path().string());
+    request.set_realtime_factor(clock_.realtime_factor);
+    request.set_step_time(clock_.getStepTime());
+    simulation_interface::toProto(
+      clock_.getCurrentRosTime(), *request.mutable_initialize_ros_time());
+    return zeromq_client_.call(request).result().success();
+  } else {
+    return true;
+  }
+}
+
 auto API::setVerbose(const bool verbose) -> void { entity_manager_ptr_->setVerbose(verbose); }
 
+auto API::setSimulationStepTime(const double step_time) -> bool
+{
+  /**
+   * @note Pausing the simulation by setting the realtime_factor_ value to 0 is not supported and causes the simulation crash.
+   * For that reason, before performing the action, it needs to be ensured that the incoming request data is a positive number.
+   */
+  if (step_time >= 0.001) {
+    clock_.realtime_factor = step_time;
+    simulation_api_schema::UpdateStepTimeRequest request;
+    request.set_simulation_step_time(clock_.getStepTime());
+    return zeromq_client_.call(request).result().success();
+  } else {
+    return false;
+  }
+}
+
 auto API::startNpcLogic() -> void
 {
   if (entity_manager_ptr_->isNpcLogicStarted()) {