Merge branch 'master' into doc/longitudinal-control

curious-jp · web-flow · commit 7f1e297a8993 · 2024-07-31T17:28:02.000+09:00
diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md
@@ -36,6 +36,7 @@
 | `/perception/object_recognition/ground_truth/objects`            | [`autoware_auto_perception_msgs/msg/TrackedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrackedObjects.idl)         | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) |
 | `/perception/obstacle_segmentation/pointcloud`                   | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg)                                                         | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation)                    |
 | `/perception/occupancy_grid_map/map`                             | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg)                                                           | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation)    |
+| `/sensing/imu/imu_data`                                  | [`sensor_msgs/msg/Imu`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Imu.msg)                                                                     |                                                                                                                                                                       |
 | `/perception/traffic_light_recognition/traffic_signals`          | [`autoware_auto_perception_msgs/msg/TrafficSignalArray`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl) | for `architecture_type` equal to `awf/universe`                                                                                                                       |
 | `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg)         | for `architecture_type` greater or equal to `awf/universe/20230906`                                                                                                   |
 | `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg)         |                                                                                                                                                                       |
diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp
@@ -334,6 +334,19 @@ class SimulatorCore
       }());
 
       if (controller.isAutoware()) {
+        core->attachImuSensor(entity_ref, [&]() {
+          simulation_api_schema::ImuSensorConfiguration configuration;
+          configuration.set_entity(entity_ref);
+          configuration.set_frame_id("tamagawa/imu_link");
+          configuration.set_add_gravity(true);
+          configuration.set_use_seed(true);
+          configuration.set_seed(0);
+          configuration.set_noise_standard_deviation_orientation(0.01);
+          configuration.set_noise_standard_deviation_twist(0.01);
+          configuration.set_noise_standard_deviation_acceleration(0.01);
+          return configuration;
+        }());
+
         core->attachLidarSensor([&]() {
           simulation_api_schema::LidarConfiguration configuration;
 
diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt
@@ -35,6 +35,7 @@ include_directories(
 ament_auto_add_library(simple_sensor_simulator_component SHARED
   src/sensor_simulation/detection_sensor/detection_sensor.cpp
   src/sensor_simulation/lidar/lidar_sensor.cpp
+  src/sensor_simulation/imu/imu_sensor.cpp
   src/sensor_simulation/lidar/raycaster.cpp
   src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp
   src/sensor_simulation/occupancy_grid/occupancy_grid_builder.cpp
diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp
@@ -0,0 +1,113 @@
+// Copyright 2024 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.
+
+#ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__IMU_SENSOR_HPP_
+#define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__IMU_SENSOR_HPP_
+
+#include <simulation_api_schema.pb.h>
+
+#include <array>
+#include <geometry_msgs/msg/accel.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <random>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <simulation_interface/conversions.hpp>
+#include <traffic_simulator_msgs/msg/entity_status.hpp>
+
+namespace simple_sensor_simulator
+{
+class ImuSensorBase
+{
+public:
+  explicit ImuSensorBase(const simulation_api_schema::ImuSensorConfiguration & configuration)
+  : add_gravity_(configuration.add_gravity()),
+    noise_standard_deviation_orientation_(configuration.noise_standard_deviation_orientation()),
+    noise_standard_deviation_twist_(configuration.noise_standard_deviation_twist()),
+    noise_standard_deviation_acceleration_(configuration.noise_standard_deviation_acceleration()),
+    random_generator_(configuration.use_seed() ? configuration.seed() : std::random_device{}()),
+    noise_distribution_orientation_(0.0, noise_standard_deviation_orientation_),
+    noise_distribution_twist_(0.0, noise_standard_deviation_twist_),
+    noise_distribution_acceleration_(0.0, noise_standard_deviation_acceleration_),
+    orientation_covariance_(calculateCovariance(noise_standard_deviation_orientation_)),
+    angular_velocity_covariance_(calculateCovariance(noise_standard_deviation_twist_)),
+    linear_acceleration_covariance_(calculateCovariance(noise_standard_deviation_acceleration_))
+  {
+  }
+
+  virtual ~ImuSensorBase() = default;
+
+  virtual auto update(
+    const rclcpp::Time & current_ros_time,
+    const std::vector<traffic_simulator_msgs::EntityStatus> & statuses) const -> bool = 0;
+
+protected:
+  const bool add_gravity_;
+  const double noise_standard_deviation_orientation_;
+  const double noise_standard_deviation_twist_;
+  const double noise_standard_deviation_acceleration_;
+  mutable std::mt19937 random_generator_;
+  mutable std::normal_distribution<> noise_distribution_orientation_;
+  mutable std::normal_distribution<> noise_distribution_twist_;
+  mutable std::normal_distribution<> noise_distribution_acceleration_;
+  const std::array<double, 9> orientation_covariance_;
+  const std::array<double, 9> angular_velocity_covariance_;
+  const std::array<double, 9> linear_acceleration_covariance_;
+
+  auto calculateCovariance(const double stddev) const -> std::array<double, 9>
+  {
+    return {std::pow(stddev, 2), 0, 0, 0, std::pow(stddev, 2), 0, 0, 0, std::pow(stddev, 2)};
+  };
+};
+
+template <typename MessageType>
+class ImuSensor : public ImuSensorBase
+{
+public:
+  explicit ImuSensor(
+    const simulation_api_schema::ImuSensorConfiguration & configuration,
+    const typename rclcpp::Publisher<MessageType>::SharedPtr & publisher)
+  : ImuSensorBase(configuration),
+    entity_name_(configuration.entity()),
+    frame_id_(configuration.frame_id()),
+    publisher_(publisher)
+  {
+  }
+
+  auto update(
+    const rclcpp::Time & current_ros_time,
+    const std::vector<traffic_simulator_msgs::EntityStatus> & statuses) const -> bool override
+  {
+    for (const auto & status : statuses) {
+      if (status.name() == entity_name_) {
+        traffic_simulator_msgs::msg::EntityStatus status_msg;
+        simulation_interface::toMsg(status, status_msg);
+        publisher_->publish(generateMessage(current_ros_time, status_msg));
+        return true;
+      }
+    }
+    return false;
+  }
+
+private:
+  auto generateMessage(
+    const rclcpp::Time & current_ros_time,
+    const traffic_simulator_msgs::msg::EntityStatus & status) const -> const MessageType;
+
+  const std::string entity_name_;
+  const std::string frame_id_;
+  const typename rclcpp::Publisher<MessageType>::SharedPtr publisher_;
+};
+}  // namespace simple_sensor_simulator
+#endif  // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__IMU_SENSOR_HPP_
diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp
@@ -25,6 +25,7 @@
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 #include <simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp>
+#include <simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp>
 #include <simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp>
 #include <simple_sensor_simulator/sensor_simulation/occupancy_grid/occupancy_grid_sensor.hpp>
 #include <simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_detector.hpp>
@@ -114,12 +115,22 @@ class SensorSimulation
     }
   }
 
+  auto attachImuSensor(
+    const double /*current_simulation_time*/,
+    const simulation_api_schema::ImuSensorConfiguration & configuration, rclcpp::Node & node)
+    -> void
+  {
+    imu_sensors_.push_back(std::make_unique<ImuSensor<sensor_msgs::msg::Imu>>(
+      configuration, node.create_publisher<sensor_msgs::msg::Imu>("/sensing/imu/imu_data", 1)));
+  }
+
   auto updateSensorFrame(
     double current_simulation_time, const rclcpp::Time & current_ros_time,
     const std::vector<traffic_simulator_msgs::EntityStatus> &,
     const simulation_api_schema::UpdateTrafficLightsRequest &) -> void;
 
 private:
+  std::vector<std::unique_ptr<ImuSensorBase>> imu_sensors_;
   std::vector<std::unique_ptr<LidarSensorBase>> lidar_sensors_;
   std::vector<std::unique_ptr<DetectionSensorBase>> detection_sensors_;
   std::vector<std::unique_ptr<OccupancyGridSensorBase>> occupancy_grid_sensors_;
diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp
@@ -119,6 +119,9 @@ class ScenarioSimulator : public rclcpp::Node
   auto despawnEntity(const simulation_api_schema::DespawnEntityRequest &)
     -> simulation_api_schema::DespawnEntityResponse;
 
+  auto attachImuSensor(const simulation_api_schema::AttachImuSensorRequest &)
+    -> simulation_api_schema::AttachImuSensorResponse;
+
   auto attachDetectionSensor(const simulation_api_schema::AttachDetectionSensorRequest &)
     -> simulation_api_schema::AttachDetectionSensorResponse;
 
diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp
@@ -0,0 +1,90 @@
+// Copyright 2024 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 <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Transform.h>
+
+#include <algorithm>
+#include <geometry/quaternion/euler_to_quaternion.hpp>
+#include <geometry/quaternion/quaternion_to_euler.hpp>
+#include <simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp>
+
+namespace simple_sensor_simulator
+{
+template <>
+auto ImuSensor<sensor_msgs::msg::Imu>::generateMessage(
+  const rclcpp::Time & current_ros_time,
+  const traffic_simulator_msgs::msg::EntityStatus & status) const -> const sensor_msgs::msg::Imu
+{
+  const auto applyNoise =
+    [&](geometry_msgs::msg::Vector3 & v, std::normal_distribution<> & distribution) {
+      v.x += distribution(random_generator_);
+      v.y += distribution(random_generator_);
+      v.z += distribution(random_generator_);
+    };
+
+  auto imu_msg = sensor_msgs::msg::Imu();
+  imu_msg.header.stamp = current_ros_time;
+  imu_msg.header.frame_id = frame_id_;
+
+  auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(status.pose.orientation);
+  auto twist = status.action_status.twist;
+  auto accel = status.action_status.accel;
+
+  // Apply noise
+  if (noise_standard_deviation_orientation_ > 0.0) {
+    applyNoise(orientation_rpy, noise_distribution_orientation_);
+  }
+  if (noise_standard_deviation_twist_ > 0.0) {
+    applyNoise(twist.angular, noise_distribution_twist_);
+  }
+  if (noise_standard_deviation_acceleration_ > 0.0) {
+    applyNoise(accel.linear, noise_distribution_acceleration_);
+  }
+
+  // Apply gravity
+  if (add_gravity_) {
+    tf2::Quaternion orientation_quaternion;
+    orientation_quaternion.setRPY(orientation_rpy.x, orientation_rpy.y, orientation_rpy.z);
+    tf2::Matrix3x3 rotation_matrix(orientation_quaternion);
+    tf2::Vector3 gravity(0.0, 0.0, -9.81);
+    tf2::Vector3 transformed_gravity = rotation_matrix * gravity;
+    accel.linear.x += transformed_gravity.x();
+    accel.linear.y += transformed_gravity.y();
+    accel.linear.z += transformed_gravity.z();
+  }
+
+  // Set data
+  imu_msg.orientation = math::geometry::convertEulerAngleToQuaternion(orientation_rpy);
+  imu_msg.angular_velocity.x = twist.angular.x;
+  imu_msg.angular_velocity.y = twist.angular.y;
+  imu_msg.angular_velocity.z = twist.angular.z;
+  imu_msg.linear_acceleration.x = accel.linear.x;
+  imu_msg.linear_acceleration.y = accel.linear.y;
+  imu_msg.linear_acceleration.z = accel.linear.z;
+
+  // Set covariance
+  std::copy(
+    orientation_covariance_.begin(), orientation_covariance_.end(),
+    imu_msg.orientation_covariance.data());
+  std::copy(
+    angular_velocity_covariance_.begin(), angular_velocity_covariance_.end(),
+    imu_msg.angular_velocity_covariance.data());
+  std::copy(
+    linear_acceleration_covariance_.begin(), linear_acceleration_covariance_.end(),
+    imu_msg.linear_acceleration_covariance.data());
+  return imu_msg;
+}
+}  // namespace simple_sensor_simulator
diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/sensor_simulation.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/sensor_simulation.cpp
@@ -24,8 +24,11 @@ auto SensorSimulation::updateSensorFrame(
   const std::vector<traffic_simulator_msgs::EntityStatus> & entities,
   const simulation_api_schema::UpdateTrafficLightsRequest & update_traffic_lights_request) -> void
 {
-  std::vector<std::string> lidar_detected_objects = {};
+  for (auto & sensor : imu_sensors_) {
+    sensor->update(current_ros_time, entities);
+  }
 
+  std::vector<std::string> lidar_detected_objects = {};
   for (auto & sensor : lidar_sensors_) {
     sensor->update(current_simulation_time, entities, current_ros_time);
     for (const auto & object : sensor->getDetectedObjects()) {
diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp
@@ -38,6 +38,7 @@ ScenarioSimulator::ScenarioSimulator(const rclcpp::NodeOptions & options)
     [this](auto &&... xs) { return spawnMiscObjectEntity(std::forward<decltype(xs)>(xs)...); },
     [this](auto &&... xs) { return despawnEntity(std::forward<decltype(xs)>(xs)...); },
     [this](auto &&... xs) { return updateEntityStatus(std::forward<decltype(xs)>(xs)...); },
+    [this](auto &&... xs) { return attachImuSensor(std::forward<decltype(xs)>(xs)...); },
     [this](auto &&... xs) { return attachLidarSensor(std::forward<decltype(xs)>(xs)...); },
     [this](auto &&... xs) { return attachDetectionSensor(std::forward<decltype(xs)>(xs)...); },
     [this](auto &&... xs) { return attachOccupancyGridSensor(std::forward<decltype(xs)>(xs)...); },
@@ -298,6 +299,15 @@ auto ScenarioSimulator::despawnEntity(const simulation_api_schema::DespawnEntity
   return res;
 }
 
+auto ScenarioSimulator::attachImuSensor(const simulation_api_schema::AttachImuSensorRequest & req)
+  -> simulation_api_schema::AttachImuSensorResponse
+{
+  sensor_sim_.attachImuSensor(current_simulation_time_, req.configuration(), *this);
+  auto res = simulation_api_schema::AttachImuSensorResponse();
+  res.mutable_result()->set_success(true);
+  return res;
+}
+
 auto ScenarioSimulator::attachDetectionSensor(
   const simulation_api_schema::AttachDetectionSensorRequest & req)
   -> simulation_api_schema::AttachDetectionSensorResponse
diff --git a/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp b/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp
@@ -67,6 +67,9 @@ class MultiClient
   auto call(const simulation_api_schema::UpdateEntityStatusRequest &)
     -> simulation_api_schema::UpdateEntityStatusResponse;
 
+  auto call(const simulation_api_schema::AttachImuSensorRequest &)
+    -> simulation_api_schema::AttachImuSensorResponse;
+
   auto call(const simulation_api_schema::AttachLidarSensorRequest &)
     -> simulation_api_schema::AttachLidarSensorResponse;
 
diff --git a/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp b/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp
@@ -67,6 +67,7 @@ class MultiServer
   DEFINE_FUNCTION_TYPE(SpawnMiscObjectEntity);
   DEFINE_FUNCTION_TYPE(DespawnEntity);
   DEFINE_FUNCTION_TYPE(UpdateEntityStatus);
+  DEFINE_FUNCTION_TYPE(AttachImuSensor);
   DEFINE_FUNCTION_TYPE(AttachLidarSensor);
   DEFINE_FUNCTION_TYPE(AttachDetectionSensor);
   DEFINE_FUNCTION_TYPE(AttachOccupancyGridSensor);
@@ -78,7 +79,7 @@ class MultiServer
 
   std::tuple<
     Initialize, UpdateFrame, SpawnVehicleEntity, SpawnPedestrianEntity, SpawnMiscObjectEntity,
-    DespawnEntity, UpdateEntityStatus, AttachLidarSensor, AttachDetectionSensor,
+    DespawnEntity, UpdateEntityStatus, AttachImuSensor, AttachLidarSensor, AttachDetectionSensor,
     AttachOccupancyGridSensor, UpdateTrafficLights, AttachPseudoTrafficLightDetector,
     UpdateStepTime>
     functions_;
diff --git a/simulation/simulation_interface/proto/simulation_api_schema.proto b/simulation/simulation_interface/proto/simulation_api_schema.proto
diff --git a/simulation/simulation_interface/src/zmq_multi_client.cpp b/simulation/simulation_interface/src/zmq_multi_client.cpp
diff --git a/simulation/simulation_interface/src/zmq_multi_server.cpp b/simulation/simulation_interface/src/zmq_multi_server.cpp
diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp
