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>
23 #include <traffic_simulator_msgs/msg/entity_subtype.hpp>
24 #include <traffic_simulator_msgs/msg/entity_type.hpp>
29 auto makePoint(
const double x,
const double y,
const double z = 0.0) -> geometry_msgs::msg::Point
31 return geometry_msgs::build<geometry_msgs::msg::Point>().x(x).y(y).z(z);
34 auto makeBoundingBox(
const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox
36 return traffic_simulator_msgs::build<traffic_simulator_msgs::msg::BoundingBox>()
38 .dimensions(geometry_msgs::build<geometry_msgs::msg::Vector3>().x(4.0).y(2.0).z(1.5));
43 return traffic_simulator_msgs::build<traffic_simulator_msgs::msg::BoundingBox>()
45 .dimensions(geometry_msgs::build<geometry_msgs::msg::Vector3>().x(1.0).y(1.0).z(1.0));
51 geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.0).y(0.0).z(yaw));
55 geometry_msgs::msg::Point position,
56 geometry_msgs::msg::Quaternion orientation = geometry_msgs::msg::Quaternion())
57 -> geometry_msgs::msg::Pose
59 return geometry_msgs::build<geometry_msgs::msg::Pose>().position(position).orientation(
65 return std::make_shared<hdmap_utils::HdMapUtils>(
66 ament_index_cpp::get_package_share_directory(
"traffic_simulator") +
"/map/lanelet2_map.osm",
67 geographic_msgs::build<geographic_msgs::msg::GeoPoint>()
68 .latitude(35.9037067912303)
69 .longitude(139.9337945139059)
74 std::shared_ptr<hdmap_utils::HdMapUtils>
hdmap_utils,
const lanelet::Id
id = 120659,
75 const double s = 0.0,
const double offset = 0.0)
83 std::shared_ptr<hdmap_utils::HdMapUtils>
hdmap_utils,
85 traffic_simulator_msgs::msg::BoundingBox bbox,
const double speed = 0.0,
87 const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE)
90 return traffic_simulator_msgs::build<traffic_simulator::EntityStatus>()
91 .type(traffic_simulator_msgs::build<traffic_simulator_msgs::msg::EntityType>().type(type))
92 .subtype(traffic_simulator_msgs::build<traffic_simulator_msgs::msg::EntitySubtype>().value(
93 traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN))
100 .lanelet_pose_valid(
true);
104 std::shared_ptr<hdmap_utils::HdMapUtils> , geometry_msgs::msg::Pose pose,
105 traffic_simulator_msgs::msg::BoundingBox bbox,
const double speed = 0.0,
107 const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE)
110 return traffic_simulator_msgs::build<traffic_simulator::EntityStatus>()
111 .type(traffic_simulator_msgs::build<traffic_simulator_msgs::msg::EntityType>().type(type))
112 .subtype(traffic_simulator_msgs::build<traffic_simulator_msgs::msg::EntitySubtype>().value(
113 traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN))
120 .lanelet_pose_valid(
false);
124 std::shared_ptr<hdmap_utils::HdMapUtils>
hdmap_utils,
126 traffic_simulator_msgs::msg::BoundingBox bbox,
const double speed = 0.0,
128 const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE)
136 std::shared_ptr<hdmap_utils::HdMapUtils>
hdmap_utils, geometry_msgs::msg::Pose pose,
137 traffic_simulator_msgs::msg::BoundingBox bbox,
const double speed = 0.0,
139 const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE)
Definition: entity_status.hpp:29
Definition: lanelet_pose.hpp:27
auto makeSmallBoundingBox(const double center_y=0.0) -> traffic_simulator_msgs::msg::BoundingBox
Definition: helper_functions.hpp:41
auto makeHdMapUtilsSharedPointer() -> std::shared_ptr< hdmap_utils::HdMapUtils >
Definition: helper_functions.hpp:63
auto makeEntityStatus(std::shared_ptr< hdmap_utils::HdMapUtils > hdmap_utils, 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:82
auto makeCanonicalizedEntityStatus(std::shared_ptr< hdmap_utils::HdMapUtils > hdmap_utils, 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::entity_status::CanonicalizedEntityStatus
Definition: helper_functions.hpp:123
auto makeBoundingBox(const double center_y=0.0) -> traffic_simulator_msgs::msg::BoundingBox
Definition: helper_functions.hpp:34
auto makePoint(const double x, const double y, const double z=0.0) -> geometry_msgs::msg::Point
Definition: helper_functions.hpp:29
auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion
Definition: helper_functions.hpp:48
auto makePose(geometry_msgs::msg::Point position, geometry_msgs::msg::Quaternion orientation=geometry_msgs::msg::Quaternion()) -> geometry_msgs::msg::Pose
Definition: helper_functions.hpp:54
auto makeCanonicalizedLaneletPose(std::shared_ptr< hdmap_utils::HdMapUtils > hdmap_utils, const lanelet::Id id=120659, const double s=0.0, const double offset=0.0) -> traffic_simulator::lanelet_pose::CanonicalizedLaneletPose
Definition: helper_functions.hpp:73
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:24
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:39
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:22
traffic_simulator_msgs::msg::EntityStatus EntityStatus
Definition: entity_status.hpp:24
std::string string
Definition: junit5.hpp:26