From f7d75ecffe5c8f1b0a687f59403c08e53364a07a Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Mon, 16 Dec 2024 18:42:13 +0100 Subject: [PATCH] ref(traffic_simulator): simple improvements --- .../simulator_core.hpp | 24 ++++--------------- .../include/traffic_simulator/api/api.hpp | 6 ++++- .../entity/entity_manager.hpp | 2 +- .../helper/ostream_helpers.hpp | 2 +- simulation/traffic_simulator/src/api/api.cpp | 19 ++++++++++++++- .../src/entity/ego_entity.cpp | 4 +++- .../src/entity/entity_base.cpp | 1 - .../src/entity/entity_manager.cpp | 2 +- .../src/entity/pedestrian_entity.cpp | 2 +- .../src/entity/vehicle_entity.cpp | 2 +- .../src/helper/ostream_helpers.cpp | 2 +- .../src/utils/node_parameters.cpp | 2 +- .../test/src/traffic_lights/helper.hpp | 2 +- .../traffic_lights/test_traffic_lights.cpp | 2 +- 14 files changed, 40 insertions(+), 32 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index ec9dcb3d122..ec576aaa623 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/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 #include #include #include @@ -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::quiet_NaN()); } - const auto nan = std::numeric_limits::quiet_NaN(); - return Eigen::Vector3d(nan, nan, nan); } static auto evaluateAcceleration(const std::string & entity_name) -> double diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index d7b60281156..3c5754250fc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/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 +#include #include #include #include @@ -257,6 +258,9 @@ class API const geometry_msgs::msg::Pose & from_map_pose, const std::string & to_entity_name) -> std::optional; + 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>; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 2a6b7eb6d71..0011bb917fd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/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. diff --git a/simulation/traffic_simulator/include/traffic_simulator/helper/ostream_helpers.hpp b/simulation/traffic_simulator/include/traffic_simulator/helper/ostream_helpers.hpp index cb85144062b..aa71ea9da53 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/helper/ostream_helpers.hpp +++ b/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. diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 7793392502e..1920297db7e 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/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> diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index c89dd0a42b0..f511c3d261b 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/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."); } diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 87ee366ae3c..7b841625980 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/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; diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 733cdd4009a..e525ae2f7f9 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/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. diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index 0223591191d..cc8727b2680 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/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 & 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 goal_poses; diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index 33566764a23..c3332502e87 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/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 & 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 goal_poses; diff --git a/simulation/traffic_simulator/src/helper/ostream_helpers.cpp b/simulation/traffic_simulator/src/helper/ostream_helpers.cpp index b4588549dee..f9e9d5f28e8 100644 --- a/simulation/traffic_simulator/src/helper/ostream_helpers.cpp +++ b/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. diff --git a/simulation/traffic_simulator/src/utils/node_parameters.cpp b/simulation/traffic_simulator/src/utils/node_parameters.cpp index 6c8aa63dfbe..8132aac7188 100644 --- a/simulation/traffic_simulator/src/utils/node_parameters.cpp +++ b/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. diff --git a/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp b/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp index a85c91a2c75..7709ec5a522 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp +++ b/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. diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp index ec5d0bc7beb..887c7bb0721 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp +++ b/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.