Implement pedestrian reg elem to the HD Map

[JStephenHuang]

Define enum for the different pedestrian reg elements state

wato_monorepo/src/world_modeling/hd_map/include/pedestrian_reg_elem.hpp

Lines 9 to 34 in b573a4f

	// TODO: Define the enum for the different pedestrian states (see traffic_light_reg_elem.hpp)
	class PedestrianRegElem : public lanelet::RegulatoryElement {
	public:
	// lanelet2 looks for this string when matching the subtype of a regulatory element to the
	// respective type
	static constexpr char RuleName[] = "pedestrian";
	static std::shared_ptr<PedestrianRegElem> make(const lanelet::BoundingBox3d& pedestrianBBox,
	uint64_t id);
	void updatePedestrian(const lanelet::BoundingBox3d& pedestrianBBox);
	uint64_t getId() const;
	private:
	uint64_t id;
	// The following lines are required so that the lanelet library can create the PedestrianRegElem
	// object Refer to :
	// https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_examples/src/02_regulatory_elements/main.cpp
	explicit PedestrianRegElem(const lanelet::RegulatoryElementDataPtr& data, uint64_t id = 0);
	friend class lanelet::RegisterRegulatoryElement<PedestrianRegElem>;
	};
	#endif

Finish and test pedestrian visualization

wato_monorepo/src/world_modeling/hd_map/src/lanelet_visualization.cpp

Lines 86 to 124 in b573a4f

	// TODO: finish and test pedestrian visualization
	visualization_msgs::msg::MarkerArray pedestrianAsMarkerArray(
	std::vector<std::shared_ptr<PedestrianRegElem>> pedestrianRegElems) {
	auto markerArray = visualization_msgs::msg::MarkerArray();
	if (pedestrianRegElems.empty()) {
	return markerArray;
	}
	std::shared_ptr<PedestrianRegElem> pedestrianRegElem = pedestrianRegElems.front();
	for (auto pedestrianRegElem = pedestrianRegElems.begin();
	pedestrianRegElem != pedestrianRegElems.end(); ++pedestrianRegElem) {
	auto pedestrian = pedestrianRegElem->get();
	auto id = pedestrian->getId();
	// TODO: get pedestrian state (crosswalks, sidewalks, etc)
	// Mock color (to change or remove)
	auto pedElemColor = std_msgs::msg::ColorRGBA();
	pedElemColor.r = 1;
	pedElemColor.a = 1;
	// Retrieving the polygon from parameters
	lanelet::ConstPolygon3d polygon = boost::get<lanelet::ConstPolygon3d>(
	pedestrian->getParameters().at(lanelet::RoleName::Refers).front());
	auto marker = polygonToMarker(polygon, &id, .1, 5, pedElemColor);
	markerArray.markers.push_back(marker);
	RCLCPP_INFO(rclcpp::get_logger("lanelet_visualization"),
	"Visualized pedestrian on hd map: ID = %i", id);
	}
	return markerArray;
	}

Refactor/Revamp updating/adding pedestrian reg elem in hd_map_router.cpp.

wato_monorepo/src/world_modeling/hd_map/src/hd_map_router.cpp

Lines 376 to 413 in b573a4f

	void HDMapRouter::add_pedestrian(
	const vision_msgs::msg::Detection3D::SharedPtr pedestrian_msg_ptr) {
	std::string pedestrian_class = get_detection3d_class(pedestrian_msg_ptr);
	if (pedestrian_class != "PEDESTRIAN") {
	RCLCPP_ERROR(rclcpp::get_logger("hd_map_router"),
	"Received non-pedestrian message in add_pedestrian function!");
	return;
	}
	uint64_t pedestrian_id = std::stoull(pedestrian_msg_ptr->id);
	auto bbox = pedestrian_msg_ptr->bbox;
	lanelet::BoundingBox3d new_pedestrian_bbox =
	lanelet::BoundingBox3d(lanelet::BasicPoint3d(bbox.center.position.x - bbox.size.x / 2,
	bbox.center.position.y - bbox.size.y / 2,
	bbox.center.position.z - bbox.size.z / 2),
	lanelet::BasicPoint3d(bbox.center.position.x + bbox.size.x / 2,
	bbox.center.position.y + bbox.size.y / 2,
	bbox.center.position.z + bbox.size.z / 2));
	lanelet::ConstLanelet nearest_lanelet = get_nearest_lanelet_to_xyz(
	bbox.center.position.x, bbox.center.position.y, bbox.center.position.z);
	auto pedestrian_reg_elem = PedestrianRegElem::make(new_pedestrian_bbox, pedestrian_id);
	// Add the regulatory element to the lanelet
	lanelet::Lanelet mutable_lanelet = lanelet_ptr_->laneletLayer.get(nearest_lanelet.id());
	mutable_lanelet.addRegulatoryElement(pedestrian_reg_elem);
	// Add the pedestrian to the map
	lanelet_ptr_->add(pedestrian_reg_elem);
	RCLCPP_INFO(rclcpp::get_logger("hd_map_router"),
	"Added pedestrian to the lanelet map: ID = %lu, Position = (%f, %f, %f)",
	pedestrian_id, bbox.center.position.x, bbox.center.position.y,
	bbox.center.position.z);
	}