Code cleaning

Signed-off-by: Paweł Lech <[email protected]>

mock/cpp_mock_scenarios/CMakeLists.txt

@@ -53,7 +53,7 @@ if(BUILD_CPP_MOCK_SCENARIOS)
   add_subdirectory(src/pedestrian)
   add_subdirectory(src/random_scenario)
   add_subdirectory(src/speed_planning)
-  add_subdirectory(src/teleport_action)
+  add_subdirectory(src/respawn_ego)
 
 endif()
 

mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt

@@ -0,0 +1,14 @@
+ament_auto_add_executable(respawn_ego
+  respawn_ego.cpp
+)
+target_link_libraries(respawn_ego cpp_scenario_node)
+
+install(TARGETS
+respawn_ego
+  DESTINATION lib/cpp_mock_scenarios
+)
+
+if(BUILD_TESTING)
+  include(../../cmake/add_cpp_mock_scenario_test.cmake)
+  add_cpp_mock_scenario_test(${PROJECT_NAME} "respawn_ego" "20.0")
+endif()

mock/cpp_mock_scenarios/src/teleport_action/teleport_action.cpp → mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp

@@ -12,33 +12,28 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <quaternion_operation/quaternion_operation.h>
-
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <cpp_mock_scenarios/catalogs.hpp>
 #include <cpp_mock_scenarios/cpp_scenario_node.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <traffic_simulator/api/api.hpp>
-#include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
-#include <iostream>
-
 #include <memory>
+#include <rclcpp/rclcpp.hpp>
 #include <string>
+#include <traffic_simulator/api/api.hpp>
 #include <vector>
 
-
-
 namespace cpp_mock_scenarios
 {
-class TeleportActionScenario : public cpp_mock_scenarios::CppScenarioNode
+class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode
 {
 public:
-  explicit TeleportActionScenario(const rclcpp::NodeOptions & option)
+  explicit RespawnEgoScenario(const rclcpp::NodeOptions & option)
   : cpp_mock_scenarios::CppScenarioNode(
-      "teleport_action", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map",
+      "respawn_ego", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map",
       "lanelet2_map.osm", __FILE__, false, option),
-      goal_pose{api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))},
-      new_position_subscriber{create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(),
+    goal_pose{
+      api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))},
+    new_position_subscriber{create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(),
       [this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) {
         geometry_msgs::msg::PoseStamped goal_msg;
         goal_msg.header.frame_id = "map";
@@ -58,25 +53,25 @@ class TeleportActionScenario : public cpp_mock_scenarios::CppScenarioNode
   }
 
   void onInitialize() override
-  {    
+  {
     spawnEgoEntity(
       api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34621, 10, 0, 0, 0, 0)),
-      {goal_pose},
-      getVehicleParameters());
+      {goal_pose}, getVehicleParameters());
   }
 
 private:
-  traffic_simulator::lanelet_pose::CanonicalizedLaneletPose goal_pose;
+  const traffic_simulator::lanelet_pose::CanonicalizedLaneletPose goal_pose;
 
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr new_position_subscriber;
+  const rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    new_position_subscriber;
 };
 }  // namespace cpp_mock_scenarios
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
   rclcpp::NodeOptions options;
-  auto component = std::make_shared<cpp_mock_scenarios::TeleportActionScenario>(options);
+  auto component = std::make_shared<cpp_mock_scenarios::RespawnEgoScenario>(options);
   rclcpp::spin(component);
   rclcpp::shutdown();
   return 0;

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

@@ -163,34 +163,40 @@ class API
   {
     return entity_manager_ptr_->getHdmapUtils()->toMapPose(pose).pose;
   }
-
-  void respawn(const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & message, const geometry_msgs::msg::PoseStamped goal_msg)
+
+  void respawn(
+    const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose,
+    const geometry_msgs::msg::PoseStamped goal_pose)
   {
+    if (name != "ego") {
+      throw std::runtime_error("Respawning entities other than EGO is not supported");
+    }
+
     simulation_api_schema::DespawnEntityRequest req;
     req.set_name(name);
 
-    // auto res = zeromq_client_.call(req);
-
     if (auto res = zeromq_client_.call(req); res.result().success()) {
       simulation_api_schema::SpawnVehicleEntityRequest spawn_req;
       spawn_req.set_is_ego(true);
-      simulation_interface::toProto(toMapPose(message.pose.pose), *spawn_req.mutable_pose());
+      simulation_interface::toProto(toMapPose(new_pose.pose.pose), *spawn_req.mutable_pose());
       spawn_req.set_initial_speed(0.0);
-      simulation_interface::toProto(entity_manager_ptr_->getVehicleParameters(name), *spawn_req.mutable_parameters());
+      simulation_interface::toProto(
+        entity_manager_ptr_->getVehicleParameters(name), *spawn_req.mutable_parameters());
       spawn_req.mutable_parameters()->set_name(name);
       spawn_req.set_asset_key("");
 
       zeromq_client_.call(spawn_req).result().success();
     }
 
     entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute();
-    entity_manager_ptr_->asFieldOperatorApplication(name).plan(std::vector<geometry_msgs::msg::PoseStamped>{goal_msg});
+    entity_manager_ptr_->asFieldOperatorApplication(name).plan(
+      std::vector<geometry_msgs::msg::PoseStamped>{goal_pose});
 
     while (!entity_manager_ptr_->asFieldOperatorApplication(name).engageable()) {
       updateFrame();
       std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
     }
-     entity_manager_ptr_->asFieldOperatorApplication(name).engage();
+    entity_manager_ptr_->asFieldOperatorApplication(name).engage();
   }
 
   template <typename Pose>