Merge pull request tier4#1323 from tier4/fix/spawn_position_of_map_pose

Fill x/y value when spawning entity in map frame.

Author: yamacir-kit

mock/cpp_mock_scenarios/CMakeLists.txt

@@ -53,6 +53,7 @@ if(BUILD_CPP_MOCK_SCENARIOS)
   add_subdirectory(src/move_backward)
   add_subdirectory(src/pedestrian)
   add_subdirectory(src/random_scenario)
+  add_subdirectory(src/spawn)
   add_subdirectory(src/speed_planning)
   add_subdirectory(src/traffic_source)
   add_subdirectory(src/synchronized_action)

mock/cpp_mock_scenarios/src/spawn/CMakeLists.txt

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

mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp

@@ -0,0 +1,71 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <cpp_mock_scenarios/catalogs.hpp>
+#include <cpp_mock_scenarios/cpp_scenario_node.hpp>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <traffic_simulator/api/api.hpp>
+#include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
+#include <vector>
+
+namespace cpp_mock_scenarios
+{
+class SpawnInMapFrameScenario : public cpp_mock_scenarios::CppScenarioNode
+{
+public:
+  explicit SpawnInMapFrameScenario(const rclcpp::NodeOptions & option)
+  : cpp_mock_scenarios::CppScenarioNode(
+      "stop_at_crosswalk", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map",
+      "lanelet2_map.osm", __FILE__, false, option)
+  {
+    start();
+  }
+
+private:
+  void onUpdate() override
+  {
+    const auto map_pose = traffic_simulator::pose::toMapPose(
+      traffic_simulator::helper::constructCanonicalizedLaneletPose(
+        120545, 0.0, 0.0, api_.getHdmapUtils()));
+    if (api_.reachPosition("ego", map_pose, 0.1)) {
+      stop(cpp_mock_scenarios::Result::SUCCESS);
+    } else {
+      stop(cpp_mock_scenarios::Result::FAILURE);
+    }
+  }
+
+  void onInitialize() override
+  {
+    api_.spawn(
+      "ego",
+      traffic_simulator::pose::toMapPose(
+        traffic_simulator::helper::constructCanonicalizedLaneletPose(
+          120545, 0.0, 0.0, api_.getHdmapUtils())),
+      getVehicleParameters());
+  }
+};
+}  // 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::SpawnInMapFrameScenario>(options);
+  rclcpp::spin(component);
+  rclcpp::shutdown();
+  return 0;
+}

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

@@ -480,6 +480,7 @@ class EntityManager
         entity_status.pose = toMapPose(pose);
         return CanonicalizedEntityStatus(entity_status, pose);
       } else if constexpr (std::is_same_v<std::decay_t<Pose>, geometry_msgs::msg::Pose>) {
+        entity_status.pose = pose;
         const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose(
           pose, parameters.bounding_box, include_crosswalk, matching_distance, hdmap_utils_ptr_);
         return CanonicalizedEntityStatus(entity_status, canonicalized_lanelet_pose);