lidar_sensor.hpp

Go to the documentation of this file.

// 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.
 
#ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__LIDAR__LIDAR_SENSOR_HPP_
#define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__LIDAR__LIDAR_SENSOR_HPP_
 
#include <simulation_interface/simulation_api_schema.pb.h>
 
#include <agnocast_wrapper/agnocast_wrapper.hpp>
#include <get_parameter/get_parameter.hpp>
#include <memory>
#include <queue>
#include <rclcpp/rclcpp.hpp>
#include <scenario_simulator_exception/exception.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <simple_sensor_simulator/sensor_simulation/lidar/lidar_noise_model_v1.hpp>
#include <simple_sensor_simulator/sensor_simulation/lidar/raycaster.hpp>
#include <string>
#include <vector>
 
namespace simple_sensor_simulator
{
class LidarSensorBase
{
protected:
  double previous_simulation_time_;
 
  simulation_api_schema::LidarConfiguration configuration_;
 
  Raycaster raycaster_;
  std::set<std::string> detected_objects_;
 
  explicit LidarSensorBase(
    const double current_simulation_time,
    const simulation_api_schema::LidarConfiguration & configuration)
  : previous_simulation_time_(current_simulation_time), configuration_(configuration)
  {
  }
 
public:
  virtual ~LidarSensorBase() = default;
 
  virtual auto update(
    const double current_simulation_time, const std::vector<traffic_simulator_msgs::EntityStatus> &,
    const rclcpp::Time & current_ros_time) -> void = 0;
 
  auto getDetectedObjects() const -> const std::set<std::string> & { return detected_objects_; }
};
 
template <typename T>
class LidarSensor : public LidarSensorBase
{
  const agnocast_wrapper::PublisherPtr<T> publisher_ptr_;
 
  std::queue<std::pair<sensor_msgs::msg::PointCloud2, double>> queue_pointcloud_;
 
  std::unique_ptr<LidarNoiseModelV1> noise_model_v1_ = nullptr;
 
  auto raycast(const std::vector<traffic_simulator_msgs::EntityStatus> &, const rclcpp::Time &)
    -> T;
 
public:
  explicit LidarSensor(
    const double current_simulation_time,
    const simulation_api_schema::LidarConfiguration & configuration,
    const agnocast_wrapper::PublisherPtr<T> & publisher_ptr)
  : LidarSensorBase(current_simulation_time, configuration), publisher_ptr_(publisher_ptr)
  {
    raycaster_.setDirection(configuration);
    const std::string topic_name = publisher_ptr_->get_topic_name();
    const std::string version_parameter_name = topic_name + ".noise.model.version";
 
    if (auto & parameter_node = common::getParameterNode();
        parameter_node.has_parameter(version_parameter_name)) {
      const auto version = common::getParameter<int>(version_parameter_name);
      if (version == 1) {
        const auto seed = common::getParameter<int>(topic_name + ".seed");
        noise_model_v1_ = std::make_unique<LidarNoiseModelV1>(topic_name, seed);
      } else {
        throw common::Error(
          "Unexpected noise model version for LiDAR sensor: ", version,
          ". Expected version is 1 for now.");
      }
    } else {
      // If parameter doesn't exist, no noise model is used
    }
  }
 
  auto update(
    const double current_simulation_time,
    const std::vector<traffic_simulator_msgs::EntityStatus> & status,
    const rclcpp::Time & current_ros_time) -> void override
  {
    if (
      current_simulation_time - previous_simulation_time_ - configuration_.scan_duration() >=
      -0.002) {
      previous_simulation_time_ = current_simulation_time;
      queue_pointcloud_.push(
        std::make_pair(raycast(status, current_ros_time), current_simulation_time));
    } else {
      detected_objects_.clear();
    }
 
    if (
      not queue_pointcloud_.empty() and
      current_simulation_time - queue_pointcloud_.front().second >=
        configuration_.lidar_sensor_delay()) {
      auto pointcloud = agnocast_wrapper::create_message(publisher_ptr_);
      *pointcloud = queue_pointcloud_.front().first;
      queue_pointcloud_.pop();
      publisher_ptr_->publish(std::move(pointcloud));
    }
  }
 
private:
  // Override
  Raycaster raycaster_;
};
 
template <>
auto LidarSensor<sensor_msgs::msg::PointCloud2>::raycast(
  const std::vector<traffic_simulator_msgs::EntityStatus> &, const rclcpp::Time &)
  -> sensor_msgs::msg::PointCloud2;
}  // namespace simple_sensor_simulator
 
#endif  // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__LIDAR__LIDAR_SENSOR_HPP_