WIP: Publish costmaps to ROS for visualization too apart of the custo…

…m MRPT GUI

mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp

@@ -110,6 +110,7 @@ class TPS_Astar_Planner_Node : public rclcpp::Node
 	/// Publisher for waypoint sequence
 	rclcpp::Publisher<mrpt_msgs::msg::WaypointSequence>::SharedPtr pub_wp_seq_;
 	rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_wp_path_seq_;
+	std::vector<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr> pub_costmaps_;
 
 	// tf2 buffer and listener
 	std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
@@ -141,6 +142,9 @@ class TPS_Astar_Planner_Node : public rclcpp::Node
 	/// waypoint sequence topic publisher name
 	std::string topic_wp_seq_pub_;
 
+	/// costmaps topic publisher name prefix
+	std::string topic_costmaps_pub_ = "/costmap";
+
 	/// Parameter file for PTGs
 	std::string ptg_ini_file_ = "ptgs.ini";
 
@@ -782,11 +786,23 @@ TPS_Astar_Planner_Node::PlanResult TPS_Astar_Planner_Node::do_path_plan(
 	if (!plan.bestNodeId.has_value())
 	{
 		RCLCPP_ERROR_STREAM(this->get_logger(), "No bestNodeId in plan output.");
-
 		return {};
 	}
 
-	// if (plan.success) { activePlanOutput_ = plan; }
+	// Publish costmaps:
+	pub_costmaps_.resize(planner_->costEvaluators_.size());
+	for (size_t i = 0; i < planner_->costEvaluators_.size(); i++)
+	{
+		if (!pub_costmaps_[i])
+		{
+			// See: REP-2003: https://ros.org/reps/rep-2003.html
+			const auto mapQoS = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
+			pub_costmaps_[i] = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
+				topic_costmaps_pub_ + mrpt::format("_%zu", i), mapQoS);
+		}
+		const auto& cm = planner_->costEvaluators_.at(i);
+		// cm->get_visualization();
+	}
 
 	if (!plan.success)
 	{