Update ReachPositionCondition to use cmath::hypot

Signed-off-by: yamacir-kit <httperror@404-notfound.jp>

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp

@@ -50,8 +50,6 @@ struct ReachPositionCondition : private SimulatorCore::CoordinateSystemConversio
 
   std::vector<std::valarray<double>> results;  // for description
 
-  const bool consider_z;
-
   explicit ReachPositionCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
 
   auto description() const -> String;

openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <openscenario_interpreter/cmath/hypot.hpp>
 #include <openscenario_interpreter/reader/attribute.hpp>
 #include <openscenario_interpreter/reader/element.hpp>
 #include <openscenario_interpreter/simulator_core.hpp>
@@ -31,12 +32,7 @@ ReachPositionCondition::ReachPositionCondition(
   position(readElement<Position>("Position", node, scope)),
   compare(Rule::lessThan),
   triggering_entities(triggering_entities),
-  results(triggering_entities.entity_refs.size(), {Double::nan()}),
-  consider_z([]() {
-    rclcpp::Node node{"get_parameter", "simulation"};
-    node.declare_parameter("consider_pose_by_road_slope", false);
-    return node.get_parameter("consider_pose_by_road_slope").as_bool();
-  }())
+  results(triggering_entities.entity_refs.size(), {Double::nan()})
 {
 }
 
@@ -53,35 +49,29 @@ auto ReachPositionCondition::description() const -> String
   return description.str();
 }
 
-// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z)
-static double hypot(const double x, const double y, const double z, const bool consider_z)
-{
-  return consider_z ? std::hypot(x, y, z) : std::hypot(x, y);
-}
-
 auto ReachPositionCondition::evaluate() -> Object
 {
   // TODO USE DistanceCondition::distance
   const auto distance = overload(
     [&](const WorldPosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
+      return hypot(pose.position.x, pose.position.y, pose.position.z);
     },
     [&](const RelativeWorldPosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
+      return hypot(pose.position.x, pose.position.y, pose.position.z);
     },
     [&](const RelativeObjectPosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
+      return hypot(pose.position.x, pose.position.y, pose.position.z);
     },
     [&](const LanePosition & position, auto && triggering_entity) {
       const auto pose = makeNativeRelativeWorldPosition(
         triggering_entity, static_cast<geometry_msgs::msg::Pose>(position));
-      return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z);
+      return hypot(pose.position.x, pose.position.y, pose.position.z);
     });
 
   results.clear();