diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h
index 79750c9a..e47ac2f7 100644
--- a/mvsim_node_src/include/mvsim/mvsim_node_core.h
+++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h
@@ -9,13 +9,13 @@
 
 #pragma once
 
-#if PACKAGE_ROS_VERSION == 1
-#include <dynamic_reconfigure/server.h>
-#include <mvsim/mvsimNodeConfig.h>
-#endif
+#include <atomic>
+#include <thread>
 
 #include <mrpt/core/WorkerThreadsPool.h>
 #include <mrpt/obs/CObservation.h>
+#include <mrpt/obs/CObservation3DRangeScan.h>
+#include <mrpt/obs/CObservationIMU.h>
 #include <mrpt/obs/CObservationPointCloud.h>
 #include <mrpt/system/CTicTac.h>
 #include <mrpt/system/CTimeLogger.h>
@@ -26,31 +26,65 @@
 #include <tf2_ros/transform_broadcaster.h>
 
 #if PACKAGE_ROS_VERSION == 1
-#include <geometry_msgs/Twist.h>
 #include <ros/ros.h>
 #include <ros/time.h>
+
+#include <dynamic_reconfigure/server.h>
+#include <mvsim/mvsimNodeConfig.h>
+
+#include <geometry_msgs/Polygon.h>
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/MapMetaData.h>
 #include <rosgraph_msgs/Clock.h>
 #include <std_msgs/Bool.h>
 #include <visualization_msgs/MarkerArray.h>
+
+// usings:
+using Msg_Polygon = geometry_msgs::Polygon;
+using Msg_PoseArray = geometry_msgs::PoseArray;
+using Msg_PoseWithCovarianceStamped = geometry_msgs::PoseWithCovarianceStamped;
+using Msg_Twist = geometry_msgs::Twist;
+using Msg_Twist_CSPtr = geometry_msgs::Twist::ConstPtr;
+using Msg_OccupancyGrid = nav_msgs::OccupancyGrid;
+using Msg_Odometry = nav_msgs::Odometry;
+using Msg_MapMetaData = nav_msgs::MapMetaData;
+using Msg_Bool = std_msgs::Bool;
+using Msg_TFMessage = tf2_msgs::TFMessage;
+using Msg_MarkerArray = visualization_msgs::MarkerArray;
 #else
+#include <rclcpp/clock.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/time_source.hpp>
+
 #include <geometry_msgs/msg/polygon.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <nav_msgs/msg/map_meta_data.hpp>
+#include <geometry_msgs/msg/twist.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <nav_msgs/msg/map_meta_data.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
-#include "rclcpp/clock.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp/time_source.hpp"
-
 #include "wrapper/publisher_wrapper.h"
-#endif
 
-#include <atomic>
-#include <thread>
+// usings:
+using Msg_Polygon = geometry_msgs::msg::Polygon;
+using Msg_PoseArray = geometry_msgs::msg::PoseArray;
+using Msg_PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+using Msg_Twist = geometry_msgs::msg::Twist;
+using Msg_Twist_CSPtr = geometry_msgs::msg::Twist::ConstSharedPtr;
+using Msg_OccupancyGrid = nav_msgs::msg::OccupancyGrid;
+using Msg_Odometry = nav_msgs::msg::Odometry;
+using Msg_MapMetaData = nav_msgs::msg::MapMetaData;
+using Msg_Bool = std_msgs::msg::Bool;
+using Msg_TFMessage = tf2_msgs::msg::TFMessage;
+using Msg_MarkerArray = visualization_msgs::msg::MarkerArray;
+#endif
 
 namespace mvsim_node {
 #if PACKAGE_ROS_VERSION == 1
@@ -141,8 +175,8 @@ class MVSimNode
 	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_;
+	rclcpp::Publisher<Msg_OccupancyGrid>::SharedPtr pub_map_ros_;
+	rclcpp::Publisher<Msg_MapMetaData>::SharedPtr pub_map_metadata_;
 	rclcpp::TimeSource ts_{n_};
 	rclcpp::Clock::SharedPtr clock_;
 #endif
@@ -176,38 +210,38 @@ class MVSimNode
 		mvsim_node::shared_ptr<ros::Publisher> pub_tf;	 //!< "<VEH>/tf"
 		mvsim_node::shared_ptr<ros::Publisher> pub_tf_static;	 //!< "<VEH>/tf_static"
 
-		visualization_msgs::MarkerArray chassis_shape_msg;
+		Msg_MarkerArray chassis_shape_msg;
 #else
 		/// Subscribers vehicle's "cmd_vel" topic
-		rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_vel;
+		rclcpp::Subscription<Msg_Twist>::SharedPtr sub_cmd_vel;
 		/// Publisher of "odom" topic
-		rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
+		rclcpp::Publisher<Msg_Odometry>::SharedPtr pub_odom;
 		/// "base_pose_ground_truth" topic
-		rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_ground_truth;
+		rclcpp::Publisher<Msg_Odometry>::SharedPtr pub_ground_truth;
 
 		/// "fake_localization" pubs:
-		rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::
+		rclcpp::Publisher<Msg_PoseWithCovarianceStamped>::
 			SharedPtr pub_amcl_pose;
-		rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr
+		rclcpp::Publisher<Msg_PoseArray>::SharedPtr
 			pub_particlecloud;
 
 		/// Map <sensor_label> => publisher
 		std::map<std::string, mvsim_node::shared_ptr<PublisherWrapperBase>> pub_sensors;
 
 		/// "<VEH>/chassis_markers"
-		rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+		rclcpp::Publisher<Msg_MarkerArray>::SharedPtr
 			pub_chassis_markers;
 		/// "<VEH>/chassis_shape"
-		rclcpp::Publisher<geometry_msgs::msg::Polygon>::SharedPtr
+		rclcpp::Publisher<Msg_Polygon>::SharedPtr
 			pub_chassis_shape;
 		/// "<VEH>/collision"
-		rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_collision;
+		rclcpp::Publisher<Msg_Bool>::SharedPtr pub_collision;
 
 		/// "<VEH>/tf", "<VEH>/tf_static"
-		rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf;
-		rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_static;
+		rclcpp::Publisher<Msg_TFMessage>::SharedPtr pub_tf;
+		rclcpp::Publisher<Msg_TFMessage>::SharedPtr pub_tf_static;
 
-		visualization_msgs::msg::MarkerArray chassis_shape_msg;
+		Msg_MarkerArray chassis_shape_msg;
 #endif
 	};
 
@@ -224,11 +258,7 @@ class MVSimNode
 
 	// === ROS Hooks ====
 	void onROSMsgCmdVel(
-#if PACKAGE_ROS_VERSION == 1
-		const geometry_msgs::Twist::ConstPtr& cmd,
-#else
-		const geometry_msgs::msg::Twist::ConstSharedPtr& cmd,
-#endif
+		Msg_Twist_CSPtr cmd,
 		mvsim::VehicleBase* veh);
 	// === End ROS Hooks====
 
diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp
index 41eb7106..b40f3b94 100644
--- a/mvsim_node_src/mvsim_node.cpp
+++ b/mvsim_node_src/mvsim_node.cpp
@@ -8,18 +8,13 @@
   +-------------------------------------------------------------------------+ */
 
 #include <mrpt/core/lock_helper.h>
-#include <mrpt/obs/CObservation3DRangeScan.h>
-#include <mrpt/obs/CObservationIMU.h>
-#include <mrpt/obs/CObservationPointCloud.h>
-#include <mrpt/system/CTicTac.h>
 #include <mrpt/system/filesystem.h>
 #include <mrpt/system/os.h>	 // kbhit()
 #include <mrpt/version.h>
 #include <mvsim/WorldElements/OccupancyGridMap.h>
-#include "mvsim/mvsim_node_core.h"
+#include <mvsim/mvsim_node_core.h>
 
-#include <iostream>
-#include <rapidxml_utils.hpp>
+#include "rapidxml_utils.hpp"
 
 #if MRPT_VERSION >= 0x020b04  // >=2.11.4?
 #define HAVE_POINTS_XYZIRT
@@ -40,44 +35,24 @@
 #include <mrpt/ros1bridge/point_cloud2.h>
 #include <mrpt/ros1bridge/pose.h>
 
-#include <nav_msgs/GetMap.h>
-#include <nav_msgs/MapMetaData.h>
-#include <nav_msgs/Odometry.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/PointCloud2.h>
-#include <std_msgs/Bool.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
-#include <geometry_msgs/Polygon.h>
-#include <geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-
 // usings:
-using Msg_Bool = std_msgs::Bool;
 using Msg_Header = std_msgs::Header;
 
-using Msg_Polygon = geometry_msgs::Polygon;
 using Msg_Pose = geometry_msgs::Pose;
-using Msg_PoseArray = geometry_msgs::PoseArray;
-using Msg_PoseWithCovarianceStamped = geometry_msgs::PoseWithCovarianceStamped;
 using Msg_TransformStamped = geometry_msgs::TransformStamped;
-using Msg_Twist = geometry_msgs::Twist;
-
-using Msg_MapMetaData = nav_msgs::MapMetaData;
-using Msg_OccupancyGrid = nav_msgs::OccupancyGrid;
-using Msg_Odometry = nav_msgs::Odometry;
 
 using Msg_Image = sensor_msgs::Image;
 using Msg_Imu = sensor_msgs::Imu;
 using Msg_LaserScan = sensor_msgs::LaserScan;
 using Msg_PointCloud2 = sensor_msgs::PointCloud2;
 
-using Msg_TFMessage = tf2_msgs::TFMessage;
-
 using Msg_Marker = visualization_msgs::Marker;
-using Msg_MarkerArray = visualization_msgs::MarkerArray;
 #else
 // ===========================================
 //                    ROS 2
@@ -89,14 +64,10 @@ using Msg_MarkerArray = visualization_msgs::MarkerArray;
 #include <mrpt/ros2bridge/point_cloud2.h>
 #include <mrpt/ros2bridge/pose.h>
 
-#include <nav_msgs/msg/map_meta_data.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-#include <nav_msgs/srv/get_map.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <std_msgs/msg/bool.hpp>
 
 // see: https://github.com/ros2/geometry2/pull/416
 #if defined(MVSIM_HAS_TF2_GEOMETRY_MSGS_HPP)
@@ -106,29 +77,17 @@ using Msg_MarkerArray = visualization_msgs::MarkerArray;
 #endif
 
 // usings:
-using Msg_Bool = std_msgs::msg::Bool;
 using Msg_Header = std_msgs::msg::Header;
 
-using Msg_Polygon = geometry_msgs::msg::Polygon;
 using Msg_Pose = geometry_msgs::msg::Pose;
-using Msg_PoseArray = geometry_msgs::msg::PoseArray;
-using Msg_PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
 using Msg_TransformStamped = geometry_msgs::msg::TransformStamped;
-using Msg_Twist = geometry_msgs::msg::Twist;
-
-using Msg_MapMetaData = nav_msgs::msg::MapMetaData;
-using Msg_OccupancyGrid = nav_msgs::msg::OccupancyGrid;
-using Msg_Odometry = nav_msgs::msg::Odometry;
 
 using Msg_Image = sensor_msgs::msg::Image;
 using Msg_Imu = sensor_msgs::msg::Imu;
 using Msg_LaserScan = sensor_msgs::msg::LaserScan;
 using Msg_PointCloud2 = sensor_msgs::msg::PointCloud2;
 
-using Msg_TFMessage = tf2_msgs::msg::TFMessage;
-
 using Msg_Marker = visualization_msgs::msg::Marker;
-using Msg_MarkerArray = visualization_msgs::msg::MarkerArray;
 #endif
 
 #if PACKAGE_ROS_VERSION == 1
@@ -645,7 +604,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh)
 #else
 	pubsubs.sub_cmd_vel = n_->create_subscription<Msg_Twist>(
 		vehVarName("cmd_vel", *veh), 10,
-		[this, veh](const geometry_msgs::msg::Twist::ConstSharedPtr& msg)
+		[this, veh](Msg_Twist_CSPtr msg)
 		{ return this->onROSMsgCmdVel(msg, veh); });
 #endif
 
@@ -850,11 +809,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh)
 }
 
 void MVSimNode::onROSMsgCmdVel(
-#if PACKAGE_ROS_VERSION == 1
-	const geometry_msgs::Twist::ConstPtr& cmd,
-#else
-	const geometry_msgs::msg::Twist::ConstSharedPtr& cmd,
-#endif
+	Msg_Twist_CSPtr cmd,
 	mvsim::VehicleBase* veh)
 {
 	mvsim::ControllerBaseInterface* controller = veh->getControllerInterface();