From a537aae213c0e5d6cd441da09d6573cef2f36148 Mon Sep 17 00:00:00 2001
From: finani <10ezenfinani@gmail.com>
Date: Wed, 7 Aug 2024 22:09:39 +0900
Subject: [PATCH] merge map pubs

---
 .../include/mvsim/mvsim_node_core.h           |  4 ++--
 mvsim_node_src/mvsim_node.cpp                 | 23 +++++++------------
 2 files changed, 10 insertions(+), 17 deletions(-)

diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h
index aa008ac4..79750c9a 100644
--- a/mvsim_node_src/include/mvsim/mvsim_node_core.h
+++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h
@@ -138,8 +138,8 @@ class MVSimNode
 	// === ROS Publishers ====
 	/// used for simul_map publication
 #if PACKAGE_ROS_VERSION == 1
-	ros::Publisher pub_map_ros_, pub_map_metadata_;
-	// ros::Publisher pub_clock_;
+	mvsim_node::shared_ptr<ros::Publisher> pub_map_ros_, pub_map_metadata_;
+	// mvsim_node::shared_ptr<ros::Publisher> pub_clock_;
 #else
 	rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_map_ros_;
 	rclcpp::Publisher<nav_msgs::msg::MapMetaData>::SharedPtr pub_map_metadata_;
diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp
index ab1d2436..41eb7106 100644
--- a/mvsim_node_src/mvsim_node.cpp
+++ b/mvsim_node_src/mvsim_node.cpp
@@ -208,12 +208,12 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n)
 
 	// Init ROS publishers:
 #if PACKAGE_ROS_VERSION == 1
-	// pub_clock_ = n_.advertise<rosgraph_msgs::Clock>("/clock", 10);
+	// pub_clock_ = mvsim_node::make_shared<ros::Publisher>(n_.advertise<rosgraph_msgs::Clock>("/clock", 10));
 
-	pub_map_ros_ = n_.advertise<Msg_OccupancyGrid>(
-		"simul_map", 1 /*queue len*/, true /*latch*/);
-	pub_map_metadata_ = n_.advertise<Msg_MapMetaData>(
-		"simul_map_metadata", 1 /*queue len*/, true /*latch*/);
+	pub_map_ros_ = mvsim_node::make_shared<ros::Publisher>(n_.advertise<Msg_OccupancyGrid>(
+		"simul_map", 1 /*queue len*/, true /*latch*/));
+	pub_map_metadata_ = mvsim_node::make_shared<ros::Publisher>(n_.advertise<Msg_MapMetaData>(
+		"simul_map_metadata", 1 /*queue len*/, true /*latch*/));
 #else
 	rclcpp::QoS qosLatched(rclcpp::KeepLast(10));
 	qosLatched.durability(
@@ -563,15 +563,8 @@ void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj)
 		ros_map.header.frame_id = "map";
 #endif
 
-#if PACKAGE_ROS_VERSION == 1
-		auto& pubMap = pub_map_ros_;
-		auto& pubMapMeta = pub_map_metadata_;
-#else
-		auto& pubMap = *pub_map_ros_;
-		auto& pubMapMeta = *pub_map_metadata_;
-#endif
-		pubMap.publish(ros_map);
-		pubMapMeta.publish(ros_map.info);
+		pub_map_ros_->publish(ros_map);
+		pub_map_metadata_->publish(ros_map.info);
 
 	}  // end gridmap
 
@@ -905,7 +898,7 @@ void MVSimNode::spinNotifyROS()
 #if PACKAGE_ROS_VERSION == 1
 		// sim_time_.fromSec(mvsim_world_->get_simul_time());
 		// clockMsg_.clock = sim_time_;
-		// pub_clock_.publish(clockMsg_);
+		// pub_clock_->publish(clockMsg_);
 #else
 		// sim_time_ = n_->get_clock()->now();
 		// MRPT_TODO("Publish /clock for ROS2 too?");