ref(traffic_simulator): simple improvements

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// 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.
@@ -15,7 +15,6 @@
 #ifndef OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_
 #define OPENSCENARIO_INTERPRETER__SIMULATOR_CORE_HPP_
 
-#include <geometry/quaternion/get_rotation_matrix.hpp>
 #include <openscenario_interpreter/cmath/hypot.hpp>
 #include <openscenario_interpreter/error.hpp>
 #include <openscenario_interpreter/syntax/boolean.hpp>
@@ -621,24 +620,11 @@ class SimulatorCore
     static auto evaluateRelativeSpeed(
       const std::string & from_entity_name, const std::string & to_entity_name) -> Eigen::Vector3d
     {
-      if (const auto observer = core->getEntity(from_entity_name)) {
-        if (const auto observed = core->getEntity(to_entity_name)) {
-          auto velocity = [](const auto & entity) -> Eigen::Vector3d {
-            auto direction = [](const auto & q) -> Eigen::Vector3d {
-              return Eigen::Quaternion(q.w, q.x, q.y, q.z) * Eigen::Vector3d::UnitX();
-            };
-            return direction(entity->getMapPose().orientation) * entity->getCurrentTwist().linear.x;
-          };
-
-          const Eigen::Matrix3d rotation =
-            math::geometry::getRotationMatrix(observer->getMapPose().orientation);
-
-          return rotation.transpose() * velocity(observed) -
-                 rotation.transpose() * velocity(observer);
-        }
+      if (core->isEntityExist(from_entity_name) && core->isEntityExist(to_entity_name)) {
+        return core->relativeSpeed(from_entity_name, to_entity_name);
+      } else {
+        return Eigen::Vector3d::Constant(std::numeric_limits<double>::quiet_NaN());
       }
-      const auto nan = std::numeric_limits<double>::quiet_NaN();
-      return Eigen::Vector3d(nan, nan, nan);
     }
 
     static auto evaluateAcceleration(const std::string & entity_name) -> double

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

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// 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.
@@ -17,6 +17,7 @@
 
 #include <simulation_api_schema.pb.h>
 
+#include <geometry/quaternion/get_rotation_matrix.hpp>
 #include <simulation_interface/conversions.hpp>
 #include <simulation_interface/zmq_multi_client.hpp>
 #include <std_msgs/msg/float64.hpp>
@@ -257,6 +258,9 @@ class API
     const geometry_msgs::msg::Pose & from_map_pose, const std::string & to_entity_name)
     -> std::optional<geometry_msgs::msg::Pose>;
 
+  auto relativeSpeed(const std::string & from_entity_name, const std::string & to_entity_name)
+    -> Eigen::Vector3d;
+
   auto countLaneChanges(
     const std::string & from_entity_name, const std::string & to_entity_name,
     const RoutingConfiguration & routing_configuration) const -> std::optional<std::pair<int, int>>;

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

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// 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.

simulation/traffic_simulator/include/traffic_simulator/helper/ostream_helpers.hpp

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// 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.

simulation/traffic_simulator/src/api/api.cpp

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// 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.
@@ -443,6 +443,23 @@ auto API::relativePose(
   return pose::relativePose(from_map_pose, to_entity->getMapPose());
 }
 
+auto API::relativeSpeed(const std::string & from_entity_name, const std::string & to_entity_name)
+  -> Eigen::Vector3d
+{
+  auto velocity = [](const auto & entity) -> Eigen::Vector3d {
+    auto direction = [](const auto & q) -> Eigen::Vector3d {
+      return Eigen::Quaternion(q.w, q.x, q.y, q.z) * Eigen::Vector3d::UnitX();
+    };
+    return direction(entity->getMapPose().orientation) * entity->getCurrentTwist().linear.x;
+  };
+
+  const auto observer = getEntity(from_entity_name);
+  const auto observed = getEntity(to_entity_name);
+  const Eigen::Matrix3d rotation =
+    math::geometry::getRotationMatrix(observer->getMapPose().orientation);
+  return rotation.transpose() * velocity(observed) - rotation.transpose() * velocity(observer);
+}
+
 auto API::countLaneChanges(
   const std::string & from_entity_name, const std::string & to_entity_name,
   const RoutingConfiguration & routing_configuration) const -> std::optional<std::pair<int, int>>

simulation/traffic_simulator/src/entity/ego_entity.cpp

@@ -285,7 +285,9 @@ auto EgoEntity::requestSpeedChange(
 }
 
 auto EgoEntity::requestSynchronize(
-  const std::string &, const LaneletPose &, const LaneletPose &, const double, const double) -> bool
+  const std::string & /*target_name*/, const LaneletPose & /*target_sync_pose*/,
+  const LaneletPose & /*entity_targe*/ t, const double /*target_speed*/, const double /*tolerance*/)
+  -> bool
 {
   THROW_SYNTAX_ERROR("Request synchronize is only for non-ego entities.");
 }

simulation/traffic_simulator/src/entity/entity_base.cpp

@@ -765,7 +765,6 @@ auto EntityBase::requestSynchronize(
      target_speed](double) {
       constexpr bool include_adjacent_lanelet{true};
       constexpr bool include_opposite_direction{false};
-      constexpr bool allow_lane_change{true};
 
       RoutingConfiguration lane_changeable_routing_configuration;
       lane_changeable_routing_configuration.allow_lane_change = true;

simulation/traffic_simulator/src/entity/entity_manager.cpp

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// 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.

simulation/traffic_simulator/src/entity/pedestrian_entity.cpp

@@ -53,8 +53,8 @@ void PedestrianEntity::appendDebugMarker(visualization_msgs::msg::MarkerArray &
 
 void PedestrianEntity::requestAssignRoute(const std::vector<LaneletPose> & waypoints)
 {
-  const auto canonicalized_waypoints = pose::canonicalize(waypoints, hdmap_utils_ptr_);
   if (isInLanelet()) {
+    const auto canonicalized_waypoints = pose::canonicalize(waypoints, hdmap_utils_ptr_);
     behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE);
     route_planner_.setWaypoints(canonicalized_waypoints);
     std::vector<geometry_msgs::msg::Pose> goal_poses;

simulation/traffic_simulator/src/entity/vehicle_entity.cpp

@@ -194,8 +194,8 @@ void VehicleEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & map_
 
 void VehicleEntity::requestAssignRoute(const std::vector<LaneletPose> & waypoints)
 {
-  const auto canonicalized_waypoints = pose::canonicalize(waypoints, hdmap_utils_ptr_);
   if (isInLanelet()) {
+    const auto canonicalized_waypoints = pose::canonicalize(waypoints, hdmap_utils_ptr_);
     behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE);
     route_planner_.setWaypoints(canonicalized_waypoints);
     std::vector<geometry_msgs::msg::Pose> goal_poses;

simulation/traffic_simulator/src/helper/ostream_helpers.cpp

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// 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.

simulation/traffic_simulator/src/utils/node_parameters.cpp

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// 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.

simulation/traffic_simulator/test/src/traffic_lights/helper.hpp

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// 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.

simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc. All rights reserved.
+// 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.