scenario_simulator_v2 C++ API
test_lidar_sensor.hpp
Go to the documentation of this file.
1 // Copyright 2015 TIER IV, Inc. All rights reserved.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef SIMPLE_SENSOR_SIMULATOR__TEST__TEST_LIDAR_SENSOR_HPP_
16 #define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_LIDAR_SENSOR_HPP_
17 
18 #include <gtest/gtest.h>
19 
20 #include <agnocast_wrapper/agnocast_wrapper.hpp>
21 #include <rclcpp/rclcpp.hpp>
22 #include <sensor_msgs/msg/point_cloud2.hpp>
23 #include <set>
25 #include <string>
26 #include <vector>
27 
28 #include "../../utils/helper_functions.hpp"
29 
30 using namespace simple_sensor_simulator;
31 
32 class LidarSensorTest : public ::testing::Test
33 {
34 protected:
36  : config_(utils::constructLidarConfiguration("ego", "awf/universe/20240605", 0.0, 0.5))
37  {
38  rclcpp::init(0, nullptr);
39  node_ = std::make_shared<rclcpp::Node>("lidar_sensor_test_node");
40  makeRosInterface();
41  initializeEntityStatuses();
42 
43  lidar_ = std::make_unique<LidarSensor<sensor_msgs::msg::PointCloud2>>(0.0, config_, publisher_);
44  }
45 
46  ~LidarSensorTest() { rclcpp::shutdown(); }
47 
48  rclcpp::Node::SharedPtr node_;
49  agnocast_wrapper::PublisherPtr<sensor_msgs::msg::PointCloud2> publisher_;
50  agnocast_wrapper::SubscriptionPtr<sensor_msgs::msg::PointCloud2> subscription_;
51 
52  std::vector<EntityStatus> status_;
53 
54  std::unique_ptr<LidarSensorBase> lidar_;
55  simulation_api_schema::LidarConfiguration config_;
56  agnocast_wrapper::MessagePtr<sensor_msgs::msg::PointCloud2> received_msg_;
57 
58  double current_simulation_time_{1.0};
59  rclcpp::Time current_ros_time_{1};
60 
61 private:
62  auto initializeEntityStatuses() -> void
63  {
64  const auto dimensions = utils::makeDimensions(4.5, 2.0, 1.5);
65 
66  const auto ego_status = utils::makeEntity(
67  "ego", EntityType::EGO, utils::makePose(5.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0), dimensions);
68  const auto other1_status = utils::makeEntity(
69  "other1", EntityType::VEHICLE, utils::makePose(-3.0, -3.0, 0.0, 0.0, 0.0, 0.0, 1.0),
70  dimensions);
71  const auto other2_status = utils::makeEntity(
72  "other2", EntityType::VEHICLE, utils::makePose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
73  dimensions);
74 
75  status_ = {ego_status, other1_status, other2_status};
76  }
77 
78  auto makeRosInterface() -> void
79  {
80  publisher_ =
81  agnocast_wrapper::create_publisher<sensor_msgs::msg::PointCloud2>(node_, "lidar_output", 10);
82  subscription_ = agnocast_wrapper::create_subscription<sensor_msgs::msg::PointCloud2>(
83  node_, "lidar_output", 10,
84  [this](const agnocast_wrapper::MessagePtr<sensor_msgs::msg::PointCloud2> msg) {
85  received_msg_ = msg;
86  });
87  }
88 };
89 #endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_LIDAR_SENSOR_HPP_
Definition: test_lidar_sensor.hpp:33
agnocast_wrapper::MessagePtr< sensor_msgs::msg::PointCloud2 > received_msg_
Definition: test_lidar_sensor.hpp:56
agnocast_wrapper::SubscriptionPtr< sensor_msgs::msg::PointCloud2 > subscription_
Definition: test_lidar_sensor.hpp:50
std::unique_ptr< LidarSensorBase > lidar_
Definition: test_lidar_sensor.hpp:54
~LidarSensorTest()
Definition: test_lidar_sensor.hpp:46
std::vector< EntityStatus > status_
Definition: test_lidar_sensor.hpp:52
agnocast_wrapper::PublisherPtr< sensor_msgs::msg::PointCloud2 > publisher_
Definition: test_lidar_sensor.hpp:49
rclcpp::Node::SharedPtr node_
Definition: test_lidar_sensor.hpp:48
LidarSensorTest()
Definition: test_lidar_sensor.hpp:35
simulation_api_schema::LidarConfiguration config_
Definition: test_lidar_sensor.hpp:55
Definition: constants.hpp:19
Definition: helper_functions.hpp:35
auto makePose(const double px, const double py, const double pz, const double ox, const double oy, const double oz, const double ow) -> geometry_msgs::msg::Pose
Definition: helper_functions.hpp:45
auto constructLidarConfiguration(const std::string &entity, const std::string &architecture_type, const double lidar_sensor_delay, const double horizontal_resolution) -> const simulation_api_schema::LidarConfiguration
Definition: helper_functions.hpp:60
auto makeEntity(const std::string &name, const EntityType::Enum type, const EntitySubtype::Enum subtype, const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Vector3 &dimensions) -> EntityStatus
Definition: helper_functions.hpp:125
auto makeDimensions(const double x, const double y, const double z) -> geometry_msgs::msg::Vector3
Definition: helper_functions.hpp:54