15 #ifndef TRAFFIC_SIMULATOR__TEST__HELPER_FUNCTIONS_HPP_
16 #define TRAFFIC_SIMULATOR__TEST__HELPER_FUNCTIONS_HPP_
18 #include <ament_index_cpp/get_package_share_directory.hpp>
24 #include <traffic_simulator_msgs/msg/entity_subtype.hpp>
25 #include <traffic_simulator_msgs/msg/entity_type.hpp>
30 constexpr
auto convertDegToRad(
const double deg) ->
double {
return deg / 180.0 * M_PI; }
31 constexpr
auto convertRadToDeg(
const double rad) ->
double {
return rad * 180.0 / M_PI; }
35 return geometry_msgs::build<geometry_msgs::msg::Point>().x(x).y(y).z(z);
40 return traffic_simulator_msgs::build<traffic_simulator_msgs::msg::BoundingBox>()
42 .dimensions(geometry_msgs::build<geometry_msgs::msg::Vector3>().x(4.0).y(2.0).z(1.5));
46 const double x,
const double y,
const double x_offset = 0.0,
const double y_offset = 0.0)
49 return traffic_simulator_msgs::build<traffic_simulator_msgs::msg::BoundingBox>()
50 .center(geometry_msgs::build<geometry_msgs::msg::Point>().x(x_offset).y(y_offset).z(0.0))
51 .dimensions(geometry_msgs::build<geometry_msgs::msg::Vector3>().x(x).y(y).z(0.0));
56 return traffic_simulator_msgs::build<traffic_simulator_msgs::msg::BoundingBox>()
58 .dimensions(geometry_msgs::build<geometry_msgs::msg::Vector3>().x(1.0).y(1.0).z(1.0));
64 geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.0).y(0.0).z(yaw));
67 auto makePose(
const double x,
const double y,
const double z,
const double yaw_deg)
73 return geometry_msgs::build<geometry_msgs::msg::Pose>()
74 .position(geometry_msgs::build<geometry_msgs::msg::Point>().x(x).y(y).z(z))
76 geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.0).y(0.0).z(
82 geometry_msgs::msg::Quaternion orientation = geometry_msgs::msg::Quaternion())
85 return geometry_msgs::build<geometry_msgs::msg::Pose>().position(position).orientation(
91 return std::make_shared<hdmap_utils::HdMapUtils>(
92 ament_index_cpp::get_package_share_directory(
"traffic_simulator") +
93 "/map/standard_map/lanelet2_map.osm",
94 geographic_msgs::build<geographic_msgs::msg::GeoPoint>()
95 .latitude(35.9037067912303)
96 .longitude(139.9337945139059)
102 const auto lanelet_path = ament_index_cpp::get_package_share_directory(
"traffic_simulator") +
103 "/map/" + map_name +
"/lanelet2_map.osm";
108 const lanelet::Id
id = 120659,
const double s = 0.0,
const double offset = 0.0)
119 const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE)
122 return traffic_simulator_msgs::build<traffic_simulator::EntityStatus>()
123 .type(traffic_simulator_msgs::build<traffic_simulator_msgs::msg::EntityType>().type(type))
124 .subtype(traffic_simulator_msgs::build<traffic_simulator_msgs::msg::EntitySubtype>().value(
125 traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN))
132 .lanelet_pose_valid(
true);
137 const double speed = 0.0,
const std::string name =
"default_entity_name",
138 const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE)
141 return traffic_simulator_msgs::build<traffic_simulator::EntityStatus>()
142 .type(traffic_simulator_msgs::build<traffic_simulator_msgs::msg::EntityType>().type(type))
143 .subtype(traffic_simulator_msgs::build<traffic_simulator_msgs::msg::EntitySubtype>().value(
144 traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN))
151 .lanelet_pose_valid(
false);
158 const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE)
163 canonicalized_lanelet_pose);
168 const double matching_distance = 1.0,
const double speed = 0.0,
170 const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE)
173 const auto include_crosswalk =
174 (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == type ||
175 traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == type);
177 pose, bbox, include_crosswalk, matching_distance);
179 makeEntityStatus(pose, bbox, speed, name, type), canonicalized_lanelet_pose);
Definition: entity_status.hpp:32
Definition: lanelet_pose.hpp:28
auto convertEulerAngleToQuaternion(const T &v)
Definition: euler_to_quaternion.hpp:30
traffic_simulator_msgs::msg::ActionStatus constructActionStatus(double linear_vel=0, double angular_vel=0, double linear_accel=0, double angular_accel=0)
helper function for constructing action status
Definition: helper.cpp:25
LaneletPose constructLaneletPose(lanelet::Id lanelet_id, double s, double offset=0, double roll=0, double pitch=0, double yaw=0)
helper function for constructing lanelet pose
Definition: helper.cpp:40
auto activate(Ts &&... xs)
Definition: lanelet_map.hpp:33
geometry_msgs::msg::Pose Pose
Definition: lanelet_wrapper.hpp:66
traffic_simulator_msgs::msg::BoundingBox BoundingBox
Definition: lanelet_wrapper.hpp:62
geometry_msgs::msg::Point Point
Definition: lanelet_wrapper.hpp:65
auto toCanonicalizedLaneletPose(const LaneletPose &lanelet_pose) -> std::optional< CanonicalizedLaneletPose >
Definition: pose.cpp:104
auto toMapPose(const CanonicalizedLaneletPose &lanelet_pose) -> geometry_msgs::msg::Pose
Definition: pose.cpp:87
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:23
traffic_simulator_msgs::msg::EntityStatus EntityStatus
Definition: entity_status.hpp:25
std::string string
Definition: junit5.hpp:26
traffic_simulator::lanelet_pose::CanonicalizedLaneletPose CanonicalizedLaneletPose
Definition: test_lanelet_pose.cpp:21
auto makeSmallBoundingBox(const double center_y=0.0) -> traffic_simulator_msgs::msg::BoundingBox
Definition: helper_functions.hpp:54
auto makeCanonicalizedLaneletPose(const lanelet::Id id=120659, const double s=0.0, const double offset=0.0) -> traffic_simulator::lanelet_pose::CanonicalizedLaneletPose
Definition: helper_functions.hpp:107
auto makeHdMapUtilsSharedPointer() -> std::shared_ptr< hdmap_utils::HdMapUtils >
Definition: helper_functions.hpp:89
auto activateLaneletWrapper(const std::string map_name) -> void
Definition: helper_functions.hpp:100
auto makeBoundingBox(const double center_y=0.0) -> traffic_simulator_msgs::msg::BoundingBox
Definition: helper_functions.hpp:38
auto makePoint(const double x, const double y, const double z=0.0) -> geometry_msgs::msg::Point
Definition: helper_functions.hpp:33
auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion
Definition: helper_functions.hpp:61
auto makeEntityStatus(traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed=0.0, const std::string name="default_entity_name", const uint8_t type=traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::EntityStatus
Definition: helper_functions.hpp:115
auto makeCanonicalizedEntityStatus(traffic_simulator::lanelet_pose::CanonicalizedLaneletPose canonicalized_lanelet_pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed=0.0, const std::string name="default_entity_name", const uint8_t type=traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::entity_status::CanonicalizedEntityStatus
Definition: helper_functions.hpp:154
auto makeCustom2DBoundingBox(const double x, const double y, const double x_offset=0.0, const double y_offset=0.0) -> traffic_simulator_msgs::msg::BoundingBox
Definition: helper_functions.hpp:45
constexpr auto convertDegToRad(const double deg) -> double
Definition: helper_functions.hpp:30
auto makePose(const double x, const double y, const double z, const double yaw_deg) -> geometry_msgs::msg::Pose
Definition: helper_functions.hpp:67
constexpr auto convertRadToDeg(const double rad) -> double
Definition: helper_functions.hpp:31