Merge pull request #1163 from tier4/fix/RJD-834_fix_follow_trajectory…

…_action_autoware_cooperation

fix(follow_trajectory_action): fix cooperation with Autoware, fix speed limits

simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp

@@ -25,8 +25,6 @@ struct FollowPolylineTrajectoryAction : public PedestrianActionNode
 {
   std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> polyline_trajectory;
 
-  std::optional<double> target_speed;
-
   using PedestrianActionNode::PedestrianActionNode;
 
   auto calculateWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray;

simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.hpp

@@ -25,8 +25,6 @@ struct FollowPolylineTrajectoryAction : public VehicleActionNode
 {
   std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> polyline_trajectory;
 
-  std::optional<double> target_speed;
-
   using VehicleActionNode::VehicleActionNode;
 
   auto calculateWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;

simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

@@ -56,16 +56,27 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList
 
 auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
 {
+  auto getTargetSpeed = [&]() -> double {
+    if (target_speed.has_value()) {
+      return target_speed.value();
+    } else {
+      return entity_status->getTwist().linear.x;
+    }
+  };
+
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
       not getInput<decltype(target_speed)>("target_speed", target_speed) or
       not polyline_trajectory) {
     return BT::NodeStatus::FAILURE;
+  } else if (std::isnan(entity_status->getTime())) {
+    THROW_SIMULATION_ERROR(
+      "Time in entity_status is NaN - FollowTrajectoryAction does not support such case.");
   } else if (
     const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus(
       static_cast<traffic_simulator::EntityStatus>(*entity_status), *polyline_trajectory,
-      behavior_parameter, step_time, target_speed)) {
+      behavior_parameter, hdmap_utils, step_time, getTargetSpeed())) {
     setOutput(
       "updated_status",
       std::make_shared<traffic_simulator::CanonicalizedEntityStatus>(

simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

@@ -56,16 +56,27 @@ auto FollowPolylineTrajectoryAction::providedPorts() -> BT::PortsList
 
 auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
 {
+  auto getTargetSpeed = [&]() -> double {
+    if (target_speed.has_value()) {
+      return target_speed.value();
+    } else {
+      return entity_status->getTwist().linear.x;
+    }
+  };
+
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
       not getInput<decltype(target_speed)>("target_speed", target_speed) or
       not polyline_trajectory) {
     return BT::NodeStatus::FAILURE;
+  } else if (std::isnan(entity_status->getTime())) {
+    THROW_SIMULATION_ERROR(
+      "Time in entity_status is NaN - FollowTrajectoryAction does not support such case.");
   } else if (
     const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus(
       static_cast<traffic_simulator::EntityStatus>(*entity_status), *polyline_trajectory,
-      behavior_parameter, step_time, target_speed)) {
+      behavior_parameter, hdmap_utils, step_time, getTargetSpeed())) {
     setOutput(
       "updated_status",
       std::make_shared<traffic_simulator::CanonicalizedEntityStatus>(

simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp

@@ -131,9 +131,6 @@ class ScenarioSimulator : public rclcpp::Node
   auto updateTrafficLights(const simulation_api_schema::UpdateTrafficLightsRequest &)
     -> simulation_api_schema::UpdateTrafficLightsResponse;
 
-  auto followPolylineTrajectory(const simulation_api_schema::FollowPolylineTrajectoryRequest &)
-    -> simulation_api_schema::FollowPolylineTrajectoryResponse;
-
   auto attachPseudoTrafficLightDetector(
     const simulation_api_schema::AttachPseudoTrafficLightDetectorRequest &)
     -> simulation_api_schema::AttachPseudoTrafficLightDetectorResponse;

simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp

@@ -40,8 +40,6 @@ class EgoEntitySimulation
 public:
   const std::unique_ptr<concealer::Autoware> autoware;
 
-  traffic_simulator_msgs::msg::PolylineTrajectory polyline_trajectory;
-
 private:
   const VehicleModelType vehicle_model_type_;
 
@@ -92,6 +90,10 @@ class EgoEntitySimulation
     const std::shared_ptr<hdmap_utils::HdMapUtils> &, const rclcpp::Parameter & use_sim_time,
     const bool consider_acceleration_by_road_slope);
 
+  auto overwrite(
+    const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time,
+    double step_time, bool npc_logic_started) -> void;
+
   auto update(double time, double step_time, bool npc_logic_started) -> void;
 
   auto requestSpeedChange(double value) -> void;

simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp

@@ -44,7 +44,6 @@ ScenarioSimulator::ScenarioSimulator(const rclcpp::NodeOptions & options)
     [this](auto &&... xs) { return attachDetectionSensor(std::forward<decltype(xs)>(xs)...); },
     [this](auto &&... xs) { return attachOccupancyGridSensor(std::forward<decltype(xs)>(xs)...); },
     [this](auto &&... xs) { return updateTrafficLights(std::forward<decltype(xs)>(xs)...); },
-    [this](auto &&... xs) { return followPolylineTrajectory(std::forward<decltype(xs)>(xs)...); },
     [this](auto &&... xs) {
       return attachPseudoTrafficLightDetector(std::forward<decltype(xs)>(xs)...);
     },
@@ -159,8 +158,16 @@ auto ScenarioSimulator::updateEntityStatus(
     try {
       if (isEgo(status.name())) {
         assert(ego_entity_simulation_ && "Ego is spawned but ego_entity_simulation_ is nullptr!");
-        ego_entity_simulation_->update(
-          current_scenario_time_ + step_time_, step_time_, req.npc_logic_started());
+        if (req.overwrite_ego_status()) {
+          traffic_simulator_msgs::msg::EntityStatus ego_status_msg;
+          simulation_interface::toMsg(status, ego_status_msg);
+          ego_entity_simulation_->overwrite(
+            ego_status_msg, current_scenario_time_ + step_time_, step_time_,
+            req.npc_logic_started());
+        } else {
+          ego_entity_simulation_->update(
+            current_scenario_time_ + step_time_, step_time_, req.npc_logic_started());
+        }
         simulation_api_schema::EntityStatus ego_status;
         simulation_interface::toProto(ego_entity_simulation_->getStatus(), ego_status);
         entity_status_.at(status.name()) = ego_status;
@@ -329,21 +336,6 @@ auto ScenarioSimulator::updateTrafficLights(
   return res;
 }
 
-auto ScenarioSimulator::followPolylineTrajectory(
-  const simulation_api_schema::FollowPolylineTrajectoryRequest & request)
-  -> simulation_api_schema::FollowPolylineTrajectoryResponse
-{
-  auto response = simulation_api_schema::FollowPolylineTrajectoryResponse();
-  if (ego_entity_simulation_ and isEgo(request.name())) {
-    ego_entity_simulation_->polyline_trajectory =
-      simulation_interface::toROS2Message(request.trajectory());
-    response.mutable_result()->set_success(true);
-  } else {
-    response.mutable_result()->set_success(false);
-  }
-  return response;
-}
-
 auto ScenarioSimulator::attachPseudoTrafficLightDetector(
   const simulation_api_schema::AttachPseudoTrafficLightDetectorRequest & req)
   -> simulation_api_schema::AttachPseudoTrafficLightDetectorResponse

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -15,7 +15,6 @@
 #include <concealer/autoware_universe.hpp>
 #include <filesystem>
 #include <simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp>
-#include <traffic_simulator/behavior/follow_trajectory.hpp>
 #include <traffic_simulator/helper/helper.hpp>
 
 namespace vehicle_simulation
@@ -203,110 +202,114 @@ void EgoEntitySimulation::requestSpeedChange(double value)
   vehicle_model_ptr_->setState(v);
 }
 
+void EgoEntitySimulation::overwrite(
+  const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time,
+  double step_time, bool npc_logic_started)
+{
+  autoware->rethrow();
+
+  if (npc_logic_started) {
+    using quaternion_operation::convertQuaternionToEulerAngle;
+    using quaternion_operation::getRotationMatrix;
+
+    auto world_relative_position = [&]() -> Eigen::VectorXd {
+      auto v = Eigen::VectorXd(3);
+      v(0) = status.pose.position.x - initial_pose_.position.x;
+      v(1) = status.pose.position.y - initial_pose_.position.y;
+      v(2) = status.pose.position.z - initial_pose_.position.z;
+      return getRotationMatrix(initial_pose_.orientation).transpose() * v;
+    }();
+
+    const auto yaw = [&]() {
+      const auto q = Eigen::Quaterniond(
+        getRotationMatrix(initial_pose_.orientation).transpose() *
+        getRotationMatrix(status.pose.orientation));
+      geometry_msgs::msg::Quaternion relative_orientation;
+      relative_orientation.x = q.x();
+      relative_orientation.y = q.y();
+      relative_orientation.z = q.z();
+      relative_orientation.w = q.w();
+      return convertQuaternionToEulerAngle(relative_orientation).z -
+             (previous_linear_velocity_ ? *previous_angular_velocity_ : 0) * step_time;
+    }();
+
+    switch (auto state = Eigen::VectorXd(vehicle_model_ptr_->getDimX()); vehicle_model_type_) {
+      case VehicleModelType::DELAY_STEER_ACC:
+      case VehicleModelType::DELAY_STEER_ACC_GEARED:
+      case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED:
+        state(5) = status.action_status.accel.linear.x;
+        [[fallthrough]];
+
+      case VehicleModelType::DELAY_STEER_VEL:
+        state(4) = 0;
+        [[fallthrough]];
+
+      case VehicleModelType::IDEAL_STEER_ACC:
+      case VehicleModelType::IDEAL_STEER_ACC_GEARED:
+        state(3) = status.action_status.twist.linear.x;
+        [[fallthrough]];
+
+      case VehicleModelType::IDEAL_STEER_VEL:
+        state(0) = world_relative_position(0);
+        state(1) = world_relative_position(1);
+        state(2) = yaw;
+        vehicle_model_ptr_->setState(state);
+        break;
+
+      default:
+        THROW_SEMANTIC_ERROR(
+          "Unsupported simulation model ", toString(vehicle_model_type_), " specified");
+    }
+  }
+  updateStatus(current_scenario_time, step_time);
+  updatePreviousValues();
+}
+
 void EgoEntitySimulation::update(
   double current_scenario_time, double step_time, bool npc_logic_started)
 {
   autoware->rethrow();
 
   if (npc_logic_started) {
-    if (auto status = traffic_simulator::follow_trajectory::makeUpdatedStatus(
-          status_, polyline_trajectory, traffic_simulator_msgs::msg::BehaviorParameter(),
-          step_time);
-        status) {
-      using quaternion_operation::convertQuaternionToEulerAngle;
-      using quaternion_operation::getRotationMatrix;
-
-      auto world_relative_position = [&]() -> Eigen::VectorXd {
-        auto v = Eigen::VectorXd(3);
-        v(0) = status->pose.position.x - initial_pose_.position.x;
-        v(1) = status->pose.position.y - initial_pose_.position.y;
-        v(2) = status->pose.position.z - initial_pose_.position.z;
-        return getRotationMatrix(initial_pose_.orientation).transpose() * v;
-      }();
-
-      const auto yaw = [&]() {
-        const auto q = Eigen::Quaterniond(
-          getRotationMatrix(initial_pose_.orientation).transpose() *
-          getRotationMatrix(status->pose.orientation));
-        geometry_msgs::msg::Quaternion relative_orientation;
-        relative_orientation.x = q.x();
-        relative_orientation.y = q.y();
-        relative_orientation.z = q.z();
-        relative_orientation.w = q.w();
-        return convertQuaternionToEulerAngle(relative_orientation).z -
-               (previous_linear_velocity_ ? *previous_angular_velocity_ : 0) * step_time;
-      }();
-
-      switch (auto state = Eigen::VectorXd(vehicle_model_ptr_->getDimX()); vehicle_model_type_) {
-        case VehicleModelType::DELAY_STEER_ACC:
-        case VehicleModelType::DELAY_STEER_ACC_GEARED:
-        case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED:
-          state(5) = status->action_status.accel.linear.x;
-          [[fallthrough]];
-
-        case VehicleModelType::DELAY_STEER_VEL:
-          state(4) = 0;
-          [[fallthrough]];
-
-        case VehicleModelType::IDEAL_STEER_ACC:
-        case VehicleModelType::IDEAL_STEER_ACC_GEARED:
-          state(3) = status->action_status.twist.linear.x;
-          [[fallthrough]];
-
-        case VehicleModelType::IDEAL_STEER_VEL:
-          state(0) = world_relative_position(0);
-          state(1) = world_relative_position(1);
-          state(2) = yaw;
-          vehicle_model_ptr_->setState(state);
-          break;
-
-        default:
-          THROW_SEMANTIC_ERROR(
-            "Unsupported simulation model ", toString(vehicle_model_type_), " specified");
-      }
-    } else {
-      auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU());
-
-      auto acceleration_by_slope = [this]() {
-        if (consider_acceleration_by_road_slope_) {
-          // calculate longitudinal acceleration by slope
-          constexpr double gravity_acceleration = -9.81;
-          const double ego_pitch_angle = calculateEgoPitch();
-          const double slope_angle = -ego_pitch_angle;
-          return gravity_acceleration * std::sin(slope_angle);
-        } else {
-          return 0.0;
-        }
-      }();
-
-      switch (vehicle_model_type_) {
-        case VehicleModelType::DELAY_STEER_ACC:
-        case VehicleModelType::DELAY_STEER_ACC_GEARED:
-        case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED:
-        case VehicleModelType::IDEAL_STEER_ACC:
-        case VehicleModelType::IDEAL_STEER_ACC_GEARED:
-          input(0) =
-            autoware->getGearSign() * (autoware->getAcceleration() + acceleration_by_slope);
-          input(1) = autoware->getSteeringAngle();
-          break;
-
-        case VehicleModelType::DELAY_STEER_VEL:
-        case VehicleModelType::IDEAL_STEER_VEL:
-          input(0) = autoware->getVelocity();
-          input(1) = autoware->getSteeringAngle();
-          break;
-
-        default:
-          THROW_SEMANTIC_ERROR(
-            "Unsupported vehicle_model_type ", toString(vehicle_model_type_), "specified");
+    auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU());
+
+    auto acceleration_by_slope = [this]() {
+      if (consider_acceleration_by_road_slope_) {
+        // calculate longitudinal acceleration by slope
+        constexpr double gravity_acceleration = -9.81;
+        const double ego_pitch_angle = calculateEgoPitch();
+        const double slope_angle = -ego_pitch_angle;
+        return gravity_acceleration * std::sin(slope_angle);
+      } else {
+        return 0.0;
       }
-
-      vehicle_model_ptr_->setGear(autoware->getGearCommand().command);
-      vehicle_model_ptr_->setInput(input);
-      vehicle_model_ptr_->update(step_time);
+    }();
+
+    switch (vehicle_model_type_) {
+      case VehicleModelType::DELAY_STEER_ACC:
+      case VehicleModelType::DELAY_STEER_ACC_GEARED:
+      case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED:
+      case VehicleModelType::IDEAL_STEER_ACC:
+      case VehicleModelType::IDEAL_STEER_ACC_GEARED:
+        input(0) = autoware->getGearSign() * (autoware->getAcceleration() + acceleration_by_slope);
+        input(1) = autoware->getSteeringAngle();
+        break;
+
+      case VehicleModelType::DELAY_STEER_VEL:
+      case VehicleModelType::IDEAL_STEER_VEL:
+        input(0) = autoware->getVelocity();
+        input(1) = autoware->getSteeringAngle();
+        break;
+
+      default:
+        THROW_SEMANTIC_ERROR(
+          "Unsupported vehicle_model_type ", toString(vehicle_model_type_), "specified");
     }
-  }
 
+    vehicle_model_ptr_->setGear(autoware->getGearCommand().command);
+    vehicle_model_ptr_->setInput(input);
+    vehicle_model_ptr_->update(step_time);
+  }
   updateStatus(current_scenario_time, step_time);
   updatePreviousValues();
 }
@@ -315,7 +318,8 @@ auto EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus(
   const traffic_simulator_msgs::msg::EntityStatus & status, const double entity_width) const
   -> std::optional<traffic_simulator_msgs::msg::LaneletPose>
 {
-  // @note The lanelet matching algorithm should be equivalent to the one used in EgoEntity::setMapPose
+  // @note The lanelet matching algorithm should be equivalent to the one used in
+  // EgoEntity::setMapPose
   const auto unique_route_lanelets =
     traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets());
   const auto matching_length = [entity_width] { return entity_width * 0.5 + 1.0; }();

simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp

@@ -79,9 +79,6 @@ class MultiClient
   auto call(const simulation_api_schema::UpdateTrafficLightsRequest &)
     -> simulation_api_schema::UpdateTrafficLightsResponse;
 
-  auto call(const simulation_api_schema::FollowPolylineTrajectoryRequest &)
-    -> simulation_api_schema::FollowPolylineTrajectoryResponse;
-
   auto call(const simulation_api_schema::AttachPseudoTrafficLightDetectorRequest &)
     -> simulation_api_schema::AttachPseudoTrafficLightDetectorResponse;