Feature/synchronized action

mock/cpp_mock_scenarios/CMakeLists.txt

@@ -53,6 +53,7 @@ if(BUILD_CPP_MOCK_SCENARIOS)
   add_subdirectory(src/pedestrian)
   add_subdirectory(src/random_scenario)
   add_subdirectory(src/speed_planning)
+  add_subdirectory(src/synchronized_action)
 endif()
 
 install(

mock/cpp_mock_scenarios/src/synchronized_action/CMakeLists.txt

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

mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp

@@ -0,0 +1,103 @@
+// 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 <quaternion_operation/quaternion_operation.h>
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <cpp_mock_scenarios/catalogs.hpp>
+#include <cpp_mock_scenarios/cpp_scenario_node.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <traffic_simulator/api/api.hpp>
+#include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
+
+// headers in STL
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace cpp_mock_scenarios
+{
+class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode
+{
+public:
+  explicit SynchronizedAction(const rclcpp::NodeOptions & option)
+  : cpp_mock_scenarios::CppScenarioNode(
+      "synchronized_action",
+      ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm",
+      __FILE__, false, option)
+  {
+    start();
+  }
+
+private:
+  bool requested = false;
+  const traffic_simulator::CanonicalizedLaneletPose ego_target =
+    getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34510, 0, 0, 0, 0, 0));
+  const traffic_simulator::CanonicalizedLaneletPose npc_target =
+    getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34495, 15, 0, 0, 0, 0));
+
+  void onUpdate() override
+  {
+    // SUCCESS
+    // check if npc is in the target lanelet when ego is in the target lanelet and npc is stopped
+    if (
+      api_.requestSynchronize("npc", ego_target, npc_target, 0.2) &&
+      // api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) &&
+      // api_.getCurrentTwist("npc").linear.x == 0.0) {
+      true) {
+      stop(cpp_mock_scenarios::Result::SUCCESS);
+    }
+
+    // FAILURES
+    if (api_.getCurrentTime() >= 20.0) {
+      stop(cpp_mock_scenarios::Result::FAILURE);
+    }
+    if (api_.checkCollision("ego", "npc")) {
+      stop(cpp_mock_scenarios::Result::FAILURE);
+    }
+  }
+  void onInitialize() override
+  {
+    api_.spawn(
+      "ego",
+      api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34513, 15, 0, 0, 0, 0)),
+      getVehicleParameters());
+    api_.setLinearVelocity("ego", 5);
+    // api_.requestSpeedChange("ego", 5, true);
+    // api_.requestLaneChange("ego", 34513);
+
+    api_.spawn(
+      "npc",
+      api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34564, 5, 0, 0, 0, 0)),
+      getVehicleParameters());
+    api_.setLinearVelocity("npc", 5);
+  }
+
+  auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose)
+    -> traffic_simulator::CanonicalizedLaneletPose
+  {
+    return api_.canonicalize(lanelet_pose);
+  }
+};
+}  // 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::SynchronizedAction>(options);
+  rclcpp::spin(component);
+  rclcpp::shutdown();
+  return 0;
+}

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

@@ -322,6 +322,7 @@ class API
   FORWARD_TO_ENTITY_MANAGER(getTraveledDistance);
   FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight);
   FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights);
+  FORWARD_TO_ENTITY_MANAGER(getIfArrivedToTargetLaneletPose);
   FORWARD_TO_ENTITY_MANAGER(isEgoSpawned);
   FORWARD_TO_ENTITY_MANAGER(isInLanelet);
   FORWARD_TO_ENTITY_MANAGER(isNpcLogicStarted);
@@ -330,6 +331,7 @@ class API
   FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition);
   FORWARD_TO_ENTITY_MANAGER(requestAssignRoute);
   FORWARD_TO_ENTITY_MANAGER(requestSpeedChange);
+  FORWARD_TO_ENTITY_MANAGER(requestSynchronize);
   FORWARD_TO_ENTITY_MANAGER(requestWalkStraight);
   FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate);
   FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate);

simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp

@@ -229,6 +229,16 @@ class EntityBase
 
   /*   */ auto updateTraveledDistance(const double step_time) -> double;
 
+  /*   */ auto getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose & target_lanelet_pose)
+    -> std::optional<double>;
+
+  /*   */ auto getIfArrivedToTargetLaneletPose(
+    const CanonicalizedLaneletPose & target_lanelet_pose, const double threshold) -> bool;
+
+  /*   */ auto requestSynchronize(
+    const CanonicalizedLaneletPose & ego_target, const CanonicalizedLaneletPose & entity_target,
+    const double threshold) -> bool;
+
   virtual auto fillLaneletPose(CanonicalizedEntityStatus & status, bool include_crosswalk)
     -> void final;
 

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

@@ -298,6 +298,7 @@ class EntityManager
   FORWARD_TO_ENTITY(getDistanceToRightLaneBound, const);
   FORWARD_TO_ENTITY(getEntityStatusBeforeUpdate, const);
   FORWARD_TO_ENTITY(getEntityType, const);
+  FORWARD_TO_ENTITY(getIfArrivedToTargetLaneletPose,);
   FORWARD_TO_ENTITY(fillLaneletPose, const);
   FORWARD_TO_ENTITY(getLaneletPose, const);
   FORWARD_TO_ENTITY(getLinearJerk, const);
@@ -311,6 +312,7 @@ class EntityManager
   FORWARD_TO_ENTITY(requestAssignRoute, );
   FORWARD_TO_ENTITY(requestFollowTrajectory, );
   FORWARD_TO_ENTITY(requestLaneChange, );
+  FORWARD_TO_ENTITY(requestSynchronize, );
   FORWARD_TO_ENTITY(requestWalkStraight, );
   FORWARD_TO_ENTITY(activateOutOfRangeJob, );
   FORWARD_TO_ENTITY(setAccelerationLimit, );

simulation/traffic_simulator/src/entity/entity_base.cpp

@@ -818,5 +818,93 @@ auto EntityBase::updateTraveledDistance(const double step_time) -> double
   return traveled_distance_;
 }
 
+auto EntityBase::getDistanceToTargetLaneletPose(
+  const CanonicalizedLaneletPose & target_lanelet_pose) -> std::optional<double>
+{
+  constexpr double matching_distance = 100.0;  // may be better to use a parameter
+  const auto entity_lanelet_pose_ = getLaneletPose(matching_distance);
+  const auto target_lanelet_pose_ = target_lanelet_pose;
+
+  // check if the entity is on the lanelet
+  if (entity_lanelet_pose_) {
+    const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance(
+      static_cast<LaneletPose>(entity_lanelet_pose_.value()),
+      static_cast<LaneletPose>(target_lanelet_pose_));
+
+    return entity_distance_to_target;
+  }
+
+  // may be give error here too?
+  return std::nullopt;
+}
+
+auto EntityBase::getIfArrivedToTargetLaneletPose(
+  const CanonicalizedLaneletPose & target_lanelet_pose, const double threshold) -> bool
+{
+  if (!getDistanceToTargetLaneletPose(target_lanelet_pose).has_value()) {
+    // error here
+  }
+  const auto entity_lanelet_pose_ = getLaneletPose();
+  const auto target_lanelet_pose_ = target_lanelet_pose;
+
+  const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance(
+    static_cast<LaneletPose>(entity_lanelet_pose_.value()),
+    static_cast<LaneletPose>(target_lanelet_pose_));
+
+  if (entity_distance_to_target <= threshold) {
+    return true;
+  }
+  return false;
+}
+
+/** WARNING***************
+ * This should not be called from ego entity
+ */
+auto EntityBase::requestSynchronize(
+  const CanonicalizedLaneletPose & ego_target, const CanonicalizedLaneletPose & entity_target,
+  const double threshold) -> bool
+{
+  if (getIfArrivedToTargetLaneletPose(entity_target, threshold)) {
+    return true;
+  }
+
+  job_list_.append(
+    [this, ego_target, entity_target](double) -> bool {
+      // WARNING****************
+      // not checking job_duration_ at all
+      const auto entity_distance = getDistanceToTargetLaneletPose(entity_target);
+      const auto ego_distance = this->hdmap_utils_ptr_->getLongitudinalDistance(
+        static_cast<LaneletPose>(other_status_.find("ego")->second.getLaneletPose()),
+        static_cast<LaneletPose>(ego_target));
+
+      if (!entity_distance.has_value() ){
+        THROW_SEMANTIC_ERROR(
+          "Failed to get entity's distance to target lanelet pose. Please check entity distance exists.");
+      }
+      if (!ego_distance.has_value() ){
+        THROW_SEMANTIC_ERROR(
+          "Failed to get ego's distance to target lanelet pose. Please check ego distance exists.");
+      }
+
+      const auto ego_velocity = other_status_.find("ego")->second.getTwist().linear.x;
+      // be better to use acceleration,jerk to estimate the arrival time
+
+      RCLCPP_ERROR(rclcpp::get_logger("simulation"), "#####################");
+      // estimate ego's arrival time to the target point
+      // can estimate it more precisely by using accel,jerk
+      const auto ego_arrival_time = (ego_velocity != 0) ? ego_distance.value() / ego_velocity : 0;
+
+      // calculate the speed of entity to synchronize with ego
+      // really just a simple example, kind of like a joke
+      const auto entity_velocity_to_synchronize = entity_distance.value() / ego_arrival_time;
+
+      this->requestSpeedChange(entity_velocity_to_synchronize, true);
+      return true;
+    },
+    // after this im not sure it is correct, just an draft
+    [this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE);
+  return false;
+}
+
 }  // namespace entity
 }  // namespace traffic_simulator