Implement traffic sign to the HD Map

[JStephenHuang]

Refactor/Revamp updating and adding traffic sign reg elem in hd_map_router.cpp.

• done
• in progress
• not started

---

• Mainly get the TrafficSignSubType instead of the traffic_sign_name.
  

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

  Lines 358 to 369 in b573a4f

  	void HDMapRouter::add_traffic_sign(
  	const vision_msgs::msg::Detection3D::SharedPtr traffic_sign_msg_ptr) {
  	std::string traffic_sign_name = HDMapRouter::get_detection3d_class(traffic_sign_msg_ptr);
  	if (traffic_sign_name != "STOP SIGN") {
  	RCLCPP_ERROR(rclcpp::get_logger("hd_map_router"),
  	"Traffic Sign Type Does Not Exist in Vocabulary!");
  	return;
  	}
  	add_stop_sign(traffic_sign_msg_ptr);
  	}

  
  

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

  Lines 257 to 268 in b573a4f

  	void HDMapRouter::update_traffic_sign(
  	const vision_msgs::msg::Detection3D::SharedPtr traffic_sign_msg_ptr) {
  	std::string traffic_sign_name = HDMapRouter::get_detection3d_class(traffic_sign_msg_ptr);
  	if (traffic_sign_name == "STOP SIGN") {
  	update_stop_sign(traffic_sign_msg_ptr);
  	} else {
  	RCLCPP_ERROR(rclcpp::get_logger("hd_map_router"),
  	"Traffic Sign Type Does Not Exist in Vocabulary!");
  	}
  	// TODO: implement updating traffic sign in the HD Map
  	}

Add and test traffic signs visualization

• done
• in progress
• not started

---

• Convert the TrafficSignRegElems into markers and push it in an array and push them into the map marker array...
  

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

  Lines 119 to 133 in b573a4f

  	RCLCPP_INFO(rclcpp::get_logger("lanelet_visualization"),
  	"Visualized pedestrian on hd map: ID = %i", id);
  	}
  	return markerArray;
  	}
  	// TODO: add and test traffic signs visualization
  	visualization_msgs::msg::MarkerArray laneletAsMarkerArray(lanelet::ConstLanelet lanelet, int *id,
  	bool center, bool lanes,
  	std_msgs::msg::ColorRGBA centerColor,
  	std_msgs::msg::ColorRGBA laneColor,
  	float centerThickness,
  	float laneThickness) {

Convert the StopSignRegElem class to TrafficSignRegElem

• done
• in progress
• not started

---

wato_monorepo/src/world_modeling/hd_map/src/stop_sign_reg_elem.cpp

Lines 1 to 29 in b573a4f

	#include "stop_sign_reg_elem.hpp"
	#include "utils.hpp"
	// Static factory method to create a new instance
	std::shared_ptr<StopSignRegElem> StopSignRegElem::make(const lanelet::BoundingBox3d& stopSignBBox,
	uint64_t id) {
	auto stop_sign = utils::boundingBox3dToPolygon3d(stopSignBBox);
	lanelet::RuleParameterMap rpm = {
	{lanelet::RoleNameString::Refers, {stop_sign}}}; // Stop sign polygon in parameters
	lanelet::AttributeMap am;
	std::shared_ptr data =
	std::make_shared<lanelet::RegulatoryElementData>(lanelet::utils::getId(), std::move(rpm), am);
	data->attributes[lanelet::AttributeName::Type] = lanelet::AttributeValueString::RegulatoryElement;
	data->attributes[lanelet::AttributeName::Subtype] = RuleName;
	}
	// Constructor
	StopSignRegElem::StopSignRegElem(const lanelet::RegulatoryElementDataPtr& data, uint64_t id)
	: lanelet::RegulatoryElement(data), id{id} {}
	void StopSignRegElem::updateStopSign(const lanelet::BoundingBox3d& bbox) {
	auto stop_sign = utils::boundingBox3dToPolygon3d(bbox);
	parameters()[lanelet::RoleName::Refers].clear();
	parameters()[lanelet::RoleName::Refers].emplace_back(stop_sign); // Update the stop sign polygon
	}
	uint64_t StopSignRegElem::getId() const { return id; }

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

Lines 1 to 34 in b573a4f

	#ifndef WORLD_MODELING_STOP_SIGN_REG_ELEM_
	#define WORLD_MODELING_STOP_SIGN_REG_ELEM_
	#include <lanelet2_core/LaneletMap.h>
	#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
	#include <lanelet2_core/primitives/Lanelet.h>
	#include "rclcpp/rclcpp.hpp"
	// TODO: Define the enum for the different traffic sign states (see traffic_light_reg_elem.hpp)
	class StopSignRegElem : 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[] = "stop_sign";
	static std::shared_ptr<StopSignRegElem> make(const lanelet::BoundingBox3d& stopSignBBox,
	uint64_t id);
	void updateStopSign(const lanelet::BoundingBox3d& stopSignBBox);
	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
	friend class lanelet::RegisterRegulatoryElement<StopSignRegElem>;
	explicit StopSignRegElem(const lanelet::RegulatoryElementDataPtr& data, uint64_t id);
	};
	#endif