Merge remote-tracking branch 'tier4/RJD-1057-remove-functions-forward…

…ed-to-entity-base-middle' into RJD-1057-remove-functions-forwarded-to-entity-base-refactor Signed-off-by: Mateusz Palczuk <[email protected]>
tier4 · Jan 8, 2025 · 61d9686 · 61d9686
2 parents 6532eb6 + 5fa7281
commit 61d9686
Show file tree

Hide file tree

Showing 10 changed files with 28 additions and 25 deletions.
diff --git a/docs/developer_guide/TrafficSimulator.md b/docs/developer_guide/TrafficSimulator.md
@@ -80,14 +80,14 @@ private:
   void update()
   {
     const auto entity = getEntity("ego");
-    if (entity->isInPosition(
+    if (entity->isNearbyPosition(
       traffic_simulator::helper::constructLaneletPose(34615, 10.0), 5))
     {
       api_.requestAcquirePosition("ego",
         traffic_simulator::helper::constructLaneletPose(35026, 0.0) );
       api_.setTargetSpeed("npc2", 13, true);
     }
-    if (entity->isInPosition(
+    if (entity->isNearbyPosition(
       traffic_simulator::helper::constructLaneletPose(34579, 0.0), 5))
     {
       api_.setTargetSpeed("npc2", 3, true);

diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp
@@ -40,7 +40,7 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode
   void onUpdate() override
   {
     const auto ego_entity = api_.getEntity("ego");
-    if (ego_entity->isInPosition(
+    if (ego_entity->isNearbyPosition(
           traffic_simulator::helper::constructCanonicalizedLaneletPose(
             34513, 30, 0, api_.getHdmapUtils()),
           3.0)) {

diff --git a/...ock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp b/...ock_scenarios/src/follow_trajectory/follow_polyline_trajectory_with_do_nothing_plugin.cpp
@@ -69,26 +69,27 @@ class FollowPolylineTrajectoryWithDoNothingPluginScenario
       stop(cpp_mock_scenarios::Result::FAILURE);
     }
     // LCOV_EXCL_STOP
-    if (equals(api_.getCurrentTime(), 0.0, 0.01) && !ego_entity->isInPosition(spawn_pose, 0.1)) {
+    if (
+      equals(api_.getCurrentTime(), 0.0, 0.01) && !ego_entity->isNearbyPosition(spawn_pose, 0.1)) {
       stop(cpp_mock_scenarios::Result::FAILURE);
     }
     if (
       equals(api_.getCurrentTime(), 1.0, 0.01) &&
-      !ego_entity->isInPosition(trajectory_start_pose, 0.1)) {
+      !ego_entity->isNearbyPosition(trajectory_start_pose, 0.1)) {
       stop(cpp_mock_scenarios::Result::FAILURE);
     }
     if (
       equals(api_.getCurrentTime(), 1.5, 0.01) &&
-      !ego_entity->isInPosition(trajectory_waypoint_pose, 0.1)) {
+      !ego_entity->isNearbyPosition(trajectory_waypoint_pose, 0.1)) {
       stop(cpp_mock_scenarios::Result::FAILURE);
     }
     if (
       equals(api_.getCurrentTime(), 2.0, 0.01) &&
-      !ego_entity->isInPosition(trajectory_goal_pose, 0.1)) {
+      !ego_entity->isNearbyPosition(trajectory_goal_pose, 0.1)) {
       stop(cpp_mock_scenarios::Result::FAILURE);
     }
     if (equals(api_.getCurrentTime(), 2.5, 0.01)) {
-      if (ego_entity->isInPosition(trajectory_goal_pose, 0.1)) {
+      if (ego_entity->isNearbyPosition(trajectory_goal_pose, 0.1)) {
         stop(cpp_mock_scenarios::Result::SUCCESS);
       } else {
         stop(cpp_mock_scenarios::Result::FAILURE);

diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp
@@ -158,7 +158,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode
       constexpr lanelet::Id lanelet_id = 34392;
       if (
         !api_.isEntityExist(entity_name) &&
-        !ego_entity->isInPosition(
+        !ego_entity->isNearbyPosition(
           traffic_simulator::helper::constructCanonicalizedLaneletPose(
             34576, 25.0, 0.0, api_.getHdmapUtils()),
           5.0)) {
@@ -189,7 +189,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode
     const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose(
       34621, 10, 0.0, api_.getHdmapUtils());
     constexpr auto entity_name = "spawn_nearby_ego";
-    if (ego_entity->isInPosition(trigger_position, 20.0) && !api_.isEntityExist(entity_name)) {
+    if (ego_entity->isNearbyPosition(trigger_position, 20.0) && !api_.isEntityExist(entity_name)) {
       api_.spawn(
         entity_name,
         traffic_simulator::pose::transformRelativePoseToGlobal(
@@ -201,13 +201,13 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode
         traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing());
     }
 
-    if (!ego_entity->isInPosition(trigger_position, 20.0) && api_.isEntityExist(entity_name)) {
+    if (!ego_entity->isNearbyPosition(trigger_position, 20.0) && api_.isEntityExist(entity_name)) {
       api_.despawn(entity_name);
     }
 
     const auto ego_goal_position = traffic_simulator::helper::constructCanonicalizedLaneletPose(
       34606, 0.0, 0.0, api_.getHdmapUtils());
-    if (ego_entity->isInPosition(ego_goal_position, 1.0)) {
+    if (ego_entity->isNearbyPosition(ego_goal_position, 1.0)) {
       api_.despawn("ego");
       stop(cpp_mock_scenarios::Result::SUCCESS);
     }

diff --git a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp
@@ -41,7 +41,7 @@ class SpawnInMapFrameScenario : public cpp_mock_scenarios::CppScenarioNode
     const auto map_pose = traffic_simulator::pose::toMapPose(
       traffic_simulator::helper::constructCanonicalizedLaneletPose(
         120545, 0.0, 0.0, api_.getHdmapUtils()));
-    if (api_.getEntity("ego")->isInPosition(map_pose, 0.1)) {
+    if (api_.getEntity("ego")->isNearbyPosition(map_pose, 0.1)) {
       stop(cpp_mock_scenarios::Result::SUCCESS);
     } else {
       stop(cpp_mock_scenarios::Result::FAILURE);

diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp
@@ -50,7 +50,8 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode
     // SUCCESS
     if (
       npc_entity->requestSynchronize("ego", ego_target, npc_target, 0, 0.5) &&
-      ego_entity->isInPosition(ego_target, 1.0) && npc_entity->isInPosition(npc_target, 1.0) &&
+      ego_entity->isNearbyPosition(ego_target, 1.0) &&
+      npc_entity->isNearbyPosition(npc_target, 1.0) &&
       npc_entity->getCurrentTwist().linear.x < 0.5) {
       stop(cpp_mock_scenarios::Result::SUCCESS);
     }

diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp
@@ -50,7 +50,8 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode
     // SUCCESS
     if (
       npc_entity->requestSynchronize("ego", ego_target, npc_target, 2, 0.5) &&
-      ego_entity->isInPosition(ego_target, 1.0) && npc_entity->isInPosition(npc_target, 1.0) &&
+      ego_entity->isNearbyPosition(ego_target, 1.0) &&
+      npc_entity->isNearbyPosition(npc_target, 1.0) &&
       npc_entity->getCurrentTwist().linear.x < 2.5) {
       stop(cpp_mock_scenarios::Result::SUCCESS);
     }

diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp
@@ -57,7 +57,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode
     }
     const auto ego_entity = api_.getEntity("ego");
     const auto npc2_entity = api_.getEntity("npc2");
-    if (ego_entity->isInPosition(
+    if (ego_entity->isNearbyPosition(
           traffic_simulator::helper::constructCanonicalizedLaneletPose(
             34615, 10.0, 0.0, api_.getHdmapUtils()),
           5)) {
@@ -68,7 +68,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode
         npc2_entity->requestSpeedChange(13, true);
       }
     }
-    if (ego_entity->isInPosition(
+    if (ego_entity->isNearbyPosition(
           traffic_simulator::helper::constructCanonicalizedLaneletPose(
             34579, 0.0, 0.0, api_.getHdmapUtils()),
           5)) {
@@ -79,7 +79,7 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode
         npc2_entity->requestSpeedChange(3, true);
       }
     }
-    if (npc2_entity->isInPosition(
+    if (npc2_entity->isNearbyPosition(
           traffic_simulator::helper::constructCanonicalizedLaneletPose(
             34513, 0.0, 0.0, api_.getHdmapUtils()),
           5)) {

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
@@ -128,10 +128,10 @@ class EntityBase : public std::enable_shared_from_this<EntityBase>
 
   /*   */ auto isStopped() const -> bool;
 
-  /*   */ auto isInPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const
+  /*   */ auto isNearbyPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const
     -> bool;
 
-  /*   */ auto isInPosition(
+  /*   */ auto isNearbyPosition(
     const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool;
 
   /*   */ auto isInLanelet() const -> bool { return status_->isInLanelet(); };

diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp
@@ -95,16 +95,16 @@ auto EntityBase::getCanonicalizedLaneletPose(const double matching_distance) con
     matching_distance, hdmap_utils_ptr_);
 }
 
-auto EntityBase::isInPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const
-  -> bool
+auto EntityBase::isNearbyPosition(
+  const geometry_msgs::msg::Pose & pose, const double tolerance) const -> bool
 {
   return math::geometry::getDistance(getMapPose(), pose) < tolerance;
 }
 
-auto EntityBase::isInPosition(
+auto EntityBase::isNearbyPosition(
   const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool
 {
-  return isInPosition(static_cast<geometry_msgs::msg::Pose>(lanelet_pose), tolerance);
+  return isNearbyPosition(static_cast<geometry_msgs::msg::Pose>(lanelet_pose), tolerance);
 }
 
 auto EntityBase::isInLanelet(const lanelet::Id lanelet_id, std::optional<double> tolerance) const
@@ -765,7 +765,7 @@ auto EntityBase::requestSynchronize(
   }
 
   ///@brief Check if the entity has already arrived to the target lanelet.
-  if (isInPosition(entity_target, tolerance)) {
+  if (isNearbyPosition(entity_target, tolerance)) {
     if (getCurrentTwist().linear.x < target_speed + getMaxAcceleration() * step_time_) {
     } else {
       RCLCPP_WARN_ONCE(