From a71169d27b507d5e10045ea21011874dc09fc90a Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 09:05:00 +0900 Subject: [PATCH 01/16] explicitly capture by reference --- mvsim_node_src/mvsim_node_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mvsim_node_src/mvsim_node_main.cpp b/mvsim_node_src/mvsim_node_main.cpp index fdc957df..92809edc 100644 --- a/mvsim_node_src/mvsim_node_main.cpp +++ b/mvsim_node_src/mvsim_node_main.cpp @@ -88,13 +88,13 @@ int main(int argc, char** argv) auto ros_clock = rclcpp::Clock::make_shared(); auto timer_ = rclcpp::create_timer( n, ros_clock, std::chrono::microseconds(periodMs), - [&]() + [&node]() { if (rclcpp::ok()) node->spin(); }); rclcpp::on_shutdown( - [&]() + [&node]() { std::cout << "[rclcpp::on_shutdown] Destroying MVSIM node..." << std::endl; From 43ec5f1b2d5eba38d0cb4760bcf28354ea355937 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 09:18:33 +0900 Subject: [PATCH 02/16] merge tf message type --- mvsim_node_src/mvsim_node.cpp | 84 ++++++++++++++--------------------- 1 file changed, 34 insertions(+), 50 deletions(-) diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index 61a35d52..c7f62b8f 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -16,20 +16,19 @@ #include // kbhit() #include #include +#include "mvsim/mvsim_node_core.h" #if PACKAGE_ROS_VERSION == 1 // =========================================== // ROS 1 // =========================================== -#include -#include -#include #include #include #include #include #include #include + #include #include #include @@ -39,11 +38,17 @@ #include #include #include + +#include +#include +#include + // usings: using Msg_OccupancyGrid = nav_msgs::OccupancyGrid; using Msg_MapMetaData = nav_msgs::MapMetaData; using Msg_TransformStamped = geometry_msgs::TransformStamped; using Msg_Bool = std_msgs::Bool; +using Msg_TFMessage = tf2_msgs::TFMessage; #else // =========================================== // ROS 2 @@ -76,6 +81,7 @@ using Msg_OccupancyGrid = nav_msgs::msg::OccupancyGrid; using Msg_MapMetaData = nav_msgs::msg::MapMetaData; using Msg_TransformStamped = geometry_msgs::msg::TransformStamped; using Msg_Bool = std_msgs::msg::Bool; +using Msg_TFMessage = tf2_msgs::msg::TFMessage; #endif #if MRPT_VERSION >= 0x020b04 // >=2.11.4? @@ -89,8 +95,6 @@ using Msg_Bool = std_msgs::msg::Bool; #include #include -#include "mvsim/mvsim_node_core.h" - #if PACKAGE_ROS_VERSION == 1 namespace mrpt2ros = mrpt::ros1bridge; #else @@ -632,9 +636,9 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) vehVarName("collision", *veh), publisher_history_len_); // pub: /tf, /tf_static - pubsubs.pub_tf = n_.advertise( + pubsubs.pub_tf = n_.advertise( vehVarName("tf", *veh), publisher_history_len_); - pubsubs.pub_tf_static = n_.advertise( + pubsubs.pub_tf_static = n_.advertise( vehVarName("tf_static", *veh), publisher_history_len_); #else // pub: /odom @@ -654,9 +658,9 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) qosLatched10.durability( rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - pubsubs.pub_tf = n_->create_publisher( + pubsubs.pub_tf = n_->create_publisher( vehVarName("tf", *veh), qosLatched10); - pubsubs.pub_tf_static = n_->create_publisher( + pubsubs.pub_tf_static = n_->create_publisher( vehVarName("tf_static", *veh), qosLatched10); #endif @@ -821,22 +825,18 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) vehVarName("base_link", *veh), vehVarName("base_footprint", *veh), tfIdentity_, myNow()); + // TF STATIC(namespace ): /base_link -> /base_footprint Msg_TransformStamped tx; - tx.header.frame_id = vehVarName("base_link", *veh); - tx.child_frame_id = vehVarName("base_footprint", *veh); + tx.header.frame_id = "base_link"; + tx.child_frame_id = "base_footprint"; tx.header.stamp = myNow(); tx.transform = tf2::toMsg(tfIdentity_); - // TF STATIC(namespace ): /base_link -> /base_footprint - tx.header.frame_id = "base_link"; - tx.child_frame_id = "base_footprint"; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); +#if PACKAGE_ROS_VERSION == 1 pubsubs.pub_tf_static.publish(tfMsg); #else - tf2_msgs::msg::TFMessage tfMsg; - tfMsg.transforms.push_back(tx); pubsubs.pub_tf_static->publish(tfMsg); #endif } @@ -1002,13 +1002,11 @@ void MVSimNode::spinNotifyROS() // TF(namespace ): /map -> /odom tx.child_frame_id = "odom"; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); +#if PACKAGE_ROS_VERSION == 1 pubs.pub_tf.publish(tfMsg); #else - tf2_msgs::msg::TFMessage tfMsg; - tfMsg.transforms.push_back(tx); pubs.pub_tf->publish(tfMsg); #endif } @@ -1062,13 +1060,11 @@ void MVSimNode::spinNotifyROS() // TF(namespace ): /odom -> /base_link tx.header.frame_id = "odom"; tx.child_frame_id = "base_link"; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); +#if PACKAGE_ROS_VERSION == 1 pubs.pub_tf.publish(tfMsg); #else - tf2_msgs::msg::TFMessage tfMsg; - tfMsg.transforms.push_back(tx); pubs.pub_tf->publish(tfMsg); #endif } @@ -1262,13 +1258,11 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = obs.sensorLabel; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); +#if PACKAGE_ROS_VERSION == 1 pubs.pub_tf.publish(tfMsg); #else - tf2_msgs::msg::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); #endif @@ -1344,13 +1338,11 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = obs.sensorLabel; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); +#if PACKAGE_ROS_VERSION == 1 pubs.pub_tf.publish(tfMsg); #else - tf2_msgs::msg::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); #endif @@ -1426,13 +1418,11 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = obs.sensorLabel; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); +#if PACKAGE_ROS_VERSION == 1 pubs.pub_tf.publish(tfMsg); #else - tf2_msgs::msg::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); #endif @@ -1530,13 +1520,11 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = lbImage; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); +#if PACKAGE_ROS_VERSION == 1 pubs.pub_tf.publish(tfMsg); #else - tf2_msgs::msg::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); #endif @@ -1581,13 +1569,11 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = lbPoints; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); +#if PACKAGE_ROS_VERSION == 1 pubs.pub_tf.publish(tfMsg); #else - tf2_msgs::msg::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); #endif @@ -1678,13 +1664,11 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = lbPoints; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); +#if PACKAGE_ROS_VERSION == 1 pubs.pub_tf.publish(tfMsg); #else - tf2_msgs::msg::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); #endif From b3febf83a5d82e7570f119b12562e1d6eb4044f0 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 09:21:47 +0900 Subject: [PATCH 03/16] merge tf pubs --- .../include/mvsim/mvsim_node_core.h | 4 +- mvsim_node_src/mvsim_node.cpp | 44 ++----------------- 2 files changed, 6 insertions(+), 42 deletions(-) diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h index de168f7b..93403c93 100644 --- a/mvsim_node_src/include/mvsim/mvsim_node_core.h +++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h @@ -160,8 +160,8 @@ class MVSimNode visualization_msgs::MarkerArray chassis_shape_msg; - ros::Publisher pub_tf; - ros::Publisher pub_tf_static; + std::shared_ptr pub_tf; + std::shared_ptr pub_tf_static; #else /// Subscribers vehicle's "cmd_vel" topic rclcpp::Subscription::SharedPtr sub_cmd_vel; diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index c7f62b8f..b2fbc2ab 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -636,10 +636,10 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) vehVarName("collision", *veh), publisher_history_len_); // pub: /tf, /tf_static - pubsubs.pub_tf = n_.advertise( - vehVarName("tf", *veh), publisher_history_len_); - pubsubs.pub_tf_static = n_.advertise( - vehVarName("tf_static", *veh), publisher_history_len_); + pubsubs.pub_tf = std::make_shared(n_.advertise( + vehVarName("tf", *veh), publisher_history_len_)); + pubsubs.pub_tf_static = std::make_shared(n_.advertise( + vehVarName("tf_static", *veh), publisher_history_len_)); #else // pub: /odom pubsubs.pub_odom = n_->create_publisher( @@ -834,11 +834,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); -#if PACKAGE_ROS_VERSION == 1 - pubsubs.pub_tf_static.publish(tfMsg); -#else pubsubs.pub_tf_static->publish(tfMsg); -#endif } void MVSimNode::onROSMsgCmdVel( @@ -1004,11 +1000,7 @@ void MVSimNode::spinNotifyROS() tx.child_frame_id = "odom"; Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_tf.publish(tfMsg); -#else pubs.pub_tf->publish(tfMsg); -#endif } } } @@ -1062,11 +1054,7 @@ void MVSimNode::spinNotifyROS() tx.child_frame_id = "base_link"; Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_tf.publish(tfMsg); -#else pubs.pub_tf->publish(tfMsg); -#endif } // Apart from TF, publish to the "odom" topic as well @@ -1260,11 +1248,7 @@ void MVSimNode::internalOn( tfStmp.child_frame_id = obs.sensorLabel; Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_tf.publish(tfMsg); -#else pubs.pub_tf->publish(tfMsg); -#endif // Send observation: { @@ -1340,11 +1324,7 @@ void MVSimNode::internalOn( tfStmp.child_frame_id = obs.sensorLabel; Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_tf.publish(tfMsg); -#else pubs.pub_tf->publish(tfMsg); -#endif // Send observation: { @@ -1420,11 +1400,7 @@ void MVSimNode::internalOn( tfStmp.child_frame_id = obs.sensorLabel; Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_tf.publish(tfMsg); -#else pubs.pub_tf->publish(tfMsg); -#endif // Send observation: { @@ -1522,11 +1498,7 @@ void MVSimNode::internalOn( tfStmp.child_frame_id = lbImage; Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_tf.publish(tfMsg); -#else pubs.pub_tf->publish(tfMsg); -#endif // Send observation: { @@ -1571,11 +1543,7 @@ void MVSimNode::internalOn( tfStmp.child_frame_id = lbPoints; Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_tf.publish(tfMsg); -#else pubs.pub_tf->publish(tfMsg); -#endif // Send observation: { @@ -1666,11 +1634,7 @@ void MVSimNode::internalOn( tfStmp.child_frame_id = lbPoints; Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_tf.publish(tfMsg); -#else pubs.pub_tf->publish(tfMsg); -#endif // Send observation: { From 0ff6e799a6ab37cd978faad9676eaaddbb0167c6 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 09:42:26 +0900 Subject: [PATCH 04/16] merge vehicle pubsubs --- .../include/mvsim/mvsim_node_core.h | 24 ++--- mvsim_node_src/mvsim_node.cpp | 100 ++++++------------ 2 files changed, 46 insertions(+), 78 deletions(-) diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h index 93403c93..09842f01 100644 --- a/mvsim_node_src/include/mvsim/mvsim_node_core.h +++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h @@ -144,24 +144,25 @@ class MVSimNode struct TPubSubPerVehicle { #if PACKAGE_ROS_VERSION == 1 - ros::Subscriber sub_cmd_vel; //!< Subscribers vehicle's "cmd_vel" topic - ros::Publisher pub_odom; //!< Publisher of "odom" topic - ros::Publisher pub_ground_truth; //!< "base_pose_ground_truth" topic + std::shared_ptr sub_cmd_vel; //!< Subscribers vehicle's "cmd_vel" topic + std::shared_ptr pub_odom; //!< Publisher of "odom" topic + std::shared_ptr pub_ground_truth; //!< "base_pose_ground_truth" topic /// "fake_localization" pubs: - ros::Publisher pub_amcl_pose, pub_particlecloud; + std::shared_ptr pub_amcl_pose; //!< Publisher of "amcl_pose" topic + std::shared_ptr pub_particlecloud; //!< Publisher of "particlecloud" topic /// Map => publisher - std::map pub_sensors; + std::map> pub_sensors; - ros::Publisher pub_chassis_markers; //!< "/chassis_markers" - ros::Publisher pub_chassis_shape; //!< "/chassis_shape" - ros::Publisher pub_collision; //!< "/collision" + std::shared_ptr pub_chassis_markers; //!< "/chassis_markers" + std::shared_ptr pub_chassis_shape; //!< "/chassis_shape" + std::shared_ptr pub_collision; //!< "/collision" - visualization_msgs::MarkerArray chassis_shape_msg; + std::shared_ptr pub_tf; //!< "/tf" + std::shared_ptr pub_tf_static; //!< "/tf_static" - std::shared_ptr pub_tf; - std::shared_ptr pub_tf_static; + visualization_msgs::MarkerArray chassis_shape_msg; #else /// Subscribers vehicle's "cmd_vel" topic rclcpp::Subscription::SharedPtr sub_cmd_vel; @@ -185,7 +186,6 @@ class MVSimNode /// "/chassis_shape" rclcpp::Publisher::SharedPtr pub_chassis_shape; - /// "/collision" rclcpp::Publisher::SharedPtr pub_collision; diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index b2fbc2ab..c4fdd003 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -610,9 +610,9 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) { // sub: /cmd_vel #if PACKAGE_ROS_VERSION == 1 - pubsubs.sub_cmd_vel = n_.subscribe( + pubsubs.sub_cmd_vel = std::make_shared(n_.subscribe( vehVarName("cmd_vel", *veh), 10, - boost::bind(&MVSimNode::onROSMsgCmdVel, this, _1, veh)); + boost::bind(&MVSimNode::onROSMsgCmdVel, this, _1, veh))); #else using std::placeholders::_1; @@ -624,16 +624,16 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #if PACKAGE_ROS_VERSION == 1 // pub: /odom - pubsubs.pub_odom = n_.advertise( - vehVarName("odom", *veh), publisher_history_len_); + pubsubs.pub_odom = std::make_shared(n_.advertise( + vehVarName("odom", *veh), publisher_history_len_)); // pub: /base_pose_ground_truth - pubsubs.pub_ground_truth = n_.advertise( - vehVarName("base_pose_ground_truth", *veh), publisher_history_len_); + pubsubs.pub_ground_truth = std::make_shared(n_.advertise( + vehVarName("base_pose_ground_truth", *veh), publisher_history_len_)); // pub: /collision - pubsubs.pub_collision = n_.advertise( - vehVarName("collision", *veh), publisher_history_len_); + pubsubs.pub_collision = std::make_shared(n_.advertise( + vehVarName("collision", *veh), publisher_history_len_)); // pub: /tf, /tf_static pubsubs.pub_tf = std::make_shared(n_.advertise( @@ -668,8 +668,8 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) { #if PACKAGE_ROS_VERSION == 1 pubsubs.pub_chassis_markers = - n_.advertise( - vehVarName("chassis_markers", *veh), 5, true /*latch*/); + std::make_shared(n_.advertise( + vehVarName("chassis_markers", *veh), 5, true /*latch*/)); #else rclcpp::QoS qosLatched5(rclcpp::KeepLast(5)); qosLatched5.durability(rmw_qos_durability_policy_t:: @@ -757,18 +757,14 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) } // end for each wheel // Publish Initial pose -#if PACKAGE_ROS_VERSION == 1 - pubsubs.pub_chassis_markers.publish(msg_shapes); -#else pubsubs.pub_chassis_markers->publish(msg_shapes); -#endif } // pub: /chassis_polygon { #if PACKAGE_ROS_VERSION == 1 - pubsubs.pub_chassis_shape = n_.advertise( - vehVarName("chassis_polygon", *veh), 1, true /*latch*/); + pubsubs.pub_chassis_shape = std::make_shared(n_.advertise( + vehVarName("chassis_polygon", *veh), 1, true /*latch*/)); geometry_msgs::Polygon poly_msg; #else @@ -791,11 +787,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) poly_msg.points[i].y = poly[i].y; poly_msg.points[i].z = 0; } -#if PACKAGE_ROS_VERSION == 1 - pubsubs.pub_chassis_shape.publish(poly_msg); -#else pubsubs.pub_chassis_shape->publish(poly_msg); -#endif } if (do_fake_localization_) @@ -803,11 +795,11 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #if PACKAGE_ROS_VERSION == 1 // pub: /amcl_pose pubsubs.pub_amcl_pose = - n_.advertise( - vehVarName("amcl_pose", *veh), 1); + std::make_shared(n_.advertise( + vehVarName("amcl_pose", *veh), 1)); // pub: /particlecloud - pubsubs.pub_particlecloud = n_.advertise( - vehVarName("particlecloud", *veh), 1); + pubsubs.pub_particlecloud = std::make_shared(n_.advertise( + vehVarName("particlecloud", *veh), 1)); #else // pub: /amcl_pose pubsubs.pub_amcl_pose = @@ -947,11 +939,7 @@ void MVSimNode::spinNotifyROS() gtOdoMsg.header.frame_id = sOdomName; gtOdoMsg.child_frame_id = sBaseLinkFrame; -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_ground_truth.publish(gtOdoMsg); -#else pubs.pub_ground_truth->publish(gtOdoMsg); -#endif if (do_fake_localization_) { #if PACKAGE_ROS_VERSION == 1 @@ -968,22 +956,14 @@ void MVSimNode::spinNotifyROS() particleCloud.header.frame_id = "map"; particleCloud.poses.resize(1); particleCloud.poses[0] = gtOdoMsg.pose.pose; -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_particlecloud.publish(particleCloud); -#else pubs.pub_particlecloud->publish(particleCloud); -#endif } // topic: /amcl_pose { currentPos.header = gtOdoMsg.header; currentPos.pose.pose = gtOdoMsg.pose.pose; -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_amcl_pose.publish(currentPos); -#else pubs.pub_amcl_pose->publish(currentPos); -#endif } // TF: /map -> /odom @@ -1028,11 +1008,7 @@ void MVSimNode::spinNotifyROS() } // end for each wheel // Publish Initial pose -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_chassis_markers.publish(msg_shapes); -#else pubs.pub_chassis_markers->publish(msg_shapes); -#endif } // 3) odometry transform @@ -1072,11 +1048,7 @@ void MVSimNode::spinNotifyROS() odoMsg.child_frame_id = sBaseLinkFrame; // publish: -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_odom.publish(odoMsg); -#else pubs.pub_odom->publish(odoMsg); -#endif } } @@ -1093,11 +1065,7 @@ void MVSimNode::spinNotifyROS() colMsg.data = col; // publish: -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_collision.publish(colMsg); -#else pubs.pub_collision->publish(colMsg); -#endif } } // end for each vehicle @@ -1213,8 +1181,8 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = n_.advertise( - vehVarName(obs.sensorLabel, veh), publisher_history_len_); + pub = std::make_shared(n_.advertise( + vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else pub = n_->create_publisher( vehVarName(obs.sensorLabel, veh), publisher_history_len_); @@ -1267,7 +1235,7 @@ void MVSimNode::internalOn( msg_laser.header.frame_id = sSensorFrameId; #if PACKAGE_ROS_VERSION == 1 - pub.publish(msg_laser); + pub->publish(msg_laser); #else pubLidar->publish(msg_laser); #endif @@ -1288,8 +1256,8 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = n_.advertise( - vehVarName(obs.sensorLabel, veh), publisher_history_len_); + pub = std::make_shared(n_.advertise( + vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else pub = n_->create_publisher( vehVarName(obs.sensorLabel, veh), publisher_history_len_); @@ -1344,7 +1312,7 @@ void MVSimNode::internalOn( mrpt2ros::toROS(obs, msg_header, msg_imu); #if PACKAGE_ROS_VERSION == 1 - pub.publish(msg_imu); + pub->publish(msg_imu); #else pubImu->publish(msg_imu); #endif @@ -1365,8 +1333,8 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = n_.advertise( - vehVarName(obs.sensorLabel, veh), publisher_history_len_); + pub = std::make_shared(n_.advertise( + vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else pub = n_->create_publisher( vehVarName(obs.sensorLabel, veh), publisher_history_len_); @@ -1418,7 +1386,7 @@ void MVSimNode::internalOn( msg_img = mrpt2ros::toROS(obs.image, msg_header); #if PACKAGE_ROS_VERSION == 1 - pub.publish(msg_img); + pub->publish(msg_img); #else pubImg->publish(msg_img); #endif @@ -1447,10 +1415,10 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pubPts = n_.advertise( - vehVarName(lbPoints, veh), publisher_history_len_); - pubImg = n_.advertise( - vehVarName(lbImage, veh), publisher_history_len_); + pubImg = std::make_shared(n_.advertise( + vehVarName(lbImage, veh), publisher_history_len_)); + pubPts = std::make_shared(n_.advertise( + vehVarName(lbPoints, veh), publisher_history_len_)); #else pubImg = n_->create_publisher( vehVarName(lbImage, veh), publisher_history_len_); @@ -1516,7 +1484,7 @@ void MVSimNode::internalOn( msg_img = mrpt2ros::toROS(obs.intensityImage, msg_header); #if PACKAGE_ROS_VERSION == 1 - pubImg.publish(msg_img); + pubImg->publish(msg_img); #else pubImage->publish(msg_img); #endif @@ -1568,7 +1536,7 @@ void MVSimNode::internalOn( mrpt2ros::toROS(pts, msg_header, msg_pts); #if PACKAGE_ROS_VERSION == 1 - pubPts.publish(msg_pts); + pubPts->publish(msg_pts); #else pubPoints->publish(msg_pts); #endif @@ -1595,8 +1563,8 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pubPts = n_.advertise( - vehVarName(lbPoints, veh), publisher_history_len_); + pubPts = std::make_shared(n_.advertise( + vehVarName(lbPoints, veh), publisher_history_len_)); #else pubPts = n_->create_publisher( vehVarName(lbPoints, veh), publisher_history_len_); @@ -1677,7 +1645,7 @@ void MVSimNode::internalOn( } #if PACKAGE_ROS_VERSION == 1 - pubPts.publish(msg_pts); + pubPts->publish(msg_pts); #else pubPoints->publish(msg_pts); #endif From 094e39a4921dc9af23e53ee83dc5dbe365970762 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 10:27:10 +0900 Subject: [PATCH 05/16] merge ros topics --- mvsim_node_src/mvsim_node.cpp | 235 ++++++++++++++++------------------ 1 file changed, 108 insertions(+), 127 deletions(-) diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index c4fdd003..926e6b86 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -44,11 +44,29 @@ #include // usings: -using Msg_OccupancyGrid = nav_msgs::OccupancyGrid; -using Msg_MapMetaData = nav_msgs::MapMetaData; -using Msg_TransformStamped = geometry_msgs::TransformStamped; 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 @@ -77,11 +95,29 @@ using Msg_TFMessage = tf2_msgs::TFMessage; #endif // usings: -using Msg_OccupancyGrid = nav_msgs::msg::OccupancyGrid; -using Msg_MapMetaData = nav_msgs::msg::MapMetaData; -using Msg_TransformStamped = geometry_msgs::msg::TransformStamped; 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 MRPT_VERSION >= 0x020b04 // >=2.11.4? @@ -170,18 +206,18 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) #if PACKAGE_ROS_VERSION == 1 // pub_clock_ = n_.advertise("/clock", 10); - pub_map_ros_ = n_.advertise( + pub_map_ros_ = n_.advertise( "simul_map", 1 /*queue len*/, true /*latch*/); - pub_map_metadata_ = n_.advertise( + pub_map_metadata_ = n_.advertise( "simul_map_metadata", 1 /*queue len*/, true /*latch*/); #else rclcpp::QoS qosLatched(rclcpp::KeepLast(10)); qosLatched.durability( rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - pub_map_ros_ = n_->create_publisher( + pub_map_ros_ = n_->create_publisher( "simul_map", qosLatched); - pub_map_metadata_ = n_->create_publisher( + pub_map_metadata_ = n_->create_publisher( "simul_map_metadata", qosLatched); #endif @@ -610,13 +646,13 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) { // sub: /cmd_vel #if PACKAGE_ROS_VERSION == 1 - pubsubs.sub_cmd_vel = std::make_shared(n_.subscribe( + pubsubs.sub_cmd_vel = std::make_shared(n_.subscribe( vehVarName("cmd_vel", *veh), 10, boost::bind(&MVSimNode::onROSMsgCmdVel, this, _1, veh))); #else using std::placeholders::_1; - pubsubs.sub_cmd_vel = n_->create_subscription( + pubsubs.sub_cmd_vel = n_->create_subscription( vehVarName("cmd_vel", *veh), 10, [this, veh](const geometry_msgs::msg::Twist::ConstSharedPtr& msg) { return this->onROSMsgCmdVel(msg, veh); }); @@ -624,15 +660,15 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #if PACKAGE_ROS_VERSION == 1 // pub: /odom - pubsubs.pub_odom = std::make_shared(n_.advertise( + pubsubs.pub_odom = std::make_shared(n_.advertise( vehVarName("odom", *veh), publisher_history_len_)); // pub: /base_pose_ground_truth - pubsubs.pub_ground_truth = std::make_shared(n_.advertise( + pubsubs.pub_ground_truth = std::make_shared(n_.advertise( vehVarName("base_pose_ground_truth", *veh), publisher_history_len_)); // pub: /collision - pubsubs.pub_collision = std::make_shared(n_.advertise( + pubsubs.pub_collision = std::make_shared(n_.advertise( vehVarName("collision", *veh), publisher_history_len_)); // pub: /tf, /tf_static @@ -642,15 +678,15 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) vehVarName("tf_static", *veh), publisher_history_len_)); #else // pub: /odom - pubsubs.pub_odom = n_->create_publisher( + pubsubs.pub_odom = n_->create_publisher( vehVarName("odom", *veh), publisher_history_len_); // pub: /base_pose_ground_truth - pubsubs.pub_ground_truth = n_->create_publisher( + pubsubs.pub_ground_truth = n_->create_publisher( vehVarName("base_pose_ground_truth", *veh), publisher_history_len_); // pub: /collision - pubsubs.pub_collision = n_->create_publisher( + pubsubs.pub_collision = n_->create_publisher( vehVarName("collision", *veh), publisher_history_len_); // "/tf", "/tf_static" @@ -668,7 +704,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) { #if PACKAGE_ROS_VERSION == 1 pubsubs.pub_chassis_markers = - std::make_shared(n_.advertise( + std::make_shared(n_.advertise( vehVarName("chassis_markers", *veh), 5, true /*latch*/)); #else rclcpp::QoS qosLatched5(rclcpp::KeepLast(5)); @@ -676,7 +712,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); pubsubs.pub_chassis_markers = - n_->create_publisher( + n_->create_publisher( vehVarName("chassis_markers", *veh), qosLatched5); #endif const mrpt::math::TPolygon2D& poly = veh->getChassisShape(); @@ -691,13 +727,8 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) chassis_shape_msg.pose = mrpt2ros::toROS_Pose(mrpt::poses::CPose3D::Identity()); -#if PACKAGE_ROS_VERSION == 1 - chassis_shape_msg.action = visualization_msgs::Marker::MODIFY; - chassis_shape_msg.type = visualization_msgs::Marker::LINE_STRIP; -#else - chassis_shape_msg.action = visualization_msgs::msg::Marker::MODIFY; - chassis_shape_msg.type = visualization_msgs::msg::Marker::LINE_STRIP; -#endif + chassis_shape_msg.action = Msg_Marker::MODIFY; + chassis_shape_msg.type = Msg_Marker::LINE_STRIP; chassis_shape_msg.header.frame_id = vehVarName("base_link", *veh); chassis_shape_msg.ns = "mvsim.chassis_shape"; @@ -763,21 +794,19 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) // pub: /chassis_polygon { #if PACKAGE_ROS_VERSION == 1 - pubsubs.pub_chassis_shape = std::make_shared(n_.advertise( + pubsubs.pub_chassis_shape = std::make_shared(n_.advertise( vehVarName("chassis_polygon", *veh), 1, true /*latch*/)); - - geometry_msgs::Polygon poly_msg; #else rclcpp::QoS qosLatched1(rclcpp::KeepLast(1)); qosLatched1.durability(rmw_qos_durability_policy_t:: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); pubsubs.pub_chassis_shape = - n_->create_publisher( + n_->create_publisher( vehVarName("chassis_polygon", *veh), qosLatched1); - - geometry_msgs::msg::Polygon poly_msg; #endif + Msg_Polygon poly_msg; + // Do the first (and unique) publish: const mrpt::math::TPolygon2D& poly = veh->getChassisShape(); poly_msg.points.resize(poly.size()); @@ -795,19 +824,19 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #if PACKAGE_ROS_VERSION == 1 // pub: /amcl_pose pubsubs.pub_amcl_pose = - std::make_shared(n_.advertise( + std::make_shared(n_.advertise( vehVarName("amcl_pose", *veh), 1)); // pub: /particlecloud - pubsubs.pub_particlecloud = std::make_shared(n_.advertise( + pubsubs.pub_particlecloud = std::make_shared(n_.advertise( vehVarName("particlecloud", *veh), 1)); #else // pub: /amcl_pose pubsubs.pub_amcl_pose = - n_->create_publisher( + n_->create_publisher( vehVarName("amcl_pose", *veh), 1); // pub: /particlecloud pubsubs.pub_particlecloud = - n_->create_publisher( + n_->create_publisher( vehVarName("particlecloud", *veh), 1); #endif } @@ -922,12 +951,7 @@ void MVSimNode::spinNotifyROS() const auto& gh_veh_vel = veh->getTwist(); { -#if PACKAGE_ROS_VERSION == 1 - nav_msgs::Odometry gtOdoMsg; -#else - nav_msgs::msg::Odometry gtOdoMsg; -#endif - + Msg_Odometry gtOdoMsg; gtOdoMsg.pose.pose = mrpt2ros::toROS_Pose(gh_veh_pose); gtOdoMsg.twist.twist.linear.x = gh_veh_vel.vx; @@ -942,13 +966,8 @@ void MVSimNode::spinNotifyROS() pubs.pub_ground_truth->publish(gtOdoMsg); if (do_fake_localization_) { -#if PACKAGE_ROS_VERSION == 1 - geometry_msgs::PoseWithCovarianceStamped currentPos; - geometry_msgs::PoseArray particleCloud; -#else - geometry_msgs::msg::PoseWithCovarianceStamped currentPos; - geometry_msgs::msg::PoseArray particleCloud; -#endif + Msg_PoseWithCovarianceStamped currentPos; + Msg_PoseArray particleCloud; // topic: /particlecloud { @@ -1035,11 +1054,7 @@ void MVSimNode::spinNotifyROS() // Apart from TF, publish to the "odom" topic as well { -#if PACKAGE_ROS_VERSION == 1 - nav_msgs::Odometry odoMsg; -#else - nav_msgs::msg::Odometry odoMsg; -#endif + Msg_Odometry odoMsg; odoMsg.pose.pose = mrpt2ros::toROS_Pose(odo_pose); // first, we'll populate the header for the odometry msg @@ -1057,11 +1072,7 @@ void MVSimNode::spinNotifyROS() const bool col = veh->hadCollision(); veh->resetCollisionFlag(); { -#if PACKAGE_ROS_VERSION == 1 - std_msgs::Bool colMsg; -#else - std_msgs::msg::Bool colMsg; -#endif + Msg_Bool colMsg; colMsg.data = col; // publish: @@ -1181,19 +1192,19 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = std::make_shared(n_.advertise( + pub = std::make_shared(n_.advertise( vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else - pub = n_->create_publisher( + pub = n_->create_publisher( vehVarName(obs.sensorLabel, veh), publisher_history_len_); #endif } lck.unlock(); #if PACKAGE_ROS_VERSION == 2 - rclcpp::Publisher::SharedPtr pubLidar = + rclcpp::Publisher::SharedPtr pubLidar = std::dynamic_pointer_cast< - rclcpp::Publisher>(pub); + rclcpp::Publisher>(pub); ASSERT_(pubLidar); #endif @@ -1221,13 +1232,8 @@ void MVSimNode::internalOn( // Send observation: { // Convert observation MRPT -> ROS -#if PACKAGE_ROS_VERSION == 1 - geometry_msgs::Pose msg_pose_laser; - sensor_msgs::LaserScan msg_laser; -#else - geometry_msgs::msg::Pose msg_pose_laser; - sensor_msgs::msg::LaserScan msg_laser; -#endif + Msg_Pose msg_pose_laser; + Msg_LaserScan msg_laser; mrpt2ros::toROS(obs, msg_laser, msg_pose_laser); // Force usage of simulation time: @@ -1256,10 +1262,10 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = std::make_shared(n_.advertise( + pub = std::make_shared(n_.advertise( vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else - pub = n_->create_publisher( + pub = n_->create_publisher( vehVarName(obs.sensorLabel, veh), publisher_history_len_); ASSERT_(pub); #endif @@ -1267,8 +1273,8 @@ void MVSimNode::internalOn( lck.unlock(); #if PACKAGE_ROS_VERSION == 2 - rclcpp::Publisher::SharedPtr pubImu = - std::dynamic_pointer_cast>( + rclcpp::Publisher::SharedPtr pubImu = + std::dynamic_pointer_cast>( pub); ASSERT_(pubImu); #endif @@ -1297,14 +1303,9 @@ void MVSimNode::internalOn( // Send observation: { // Convert observation MRPT -> ROS -#if PACKAGE_ROS_VERSION == 1 - sensor_msgs::Imu msg_imu; - geometry_msgs::Pose msg_pose_imu; - std_msgs::Header msg_header; -#else - sensor_msgs::msg::Imu msg_imu; - std_msgs::msg::Header msg_header; -#endif + Msg_Imu msg_imu; + Msg_Pose msg_pose_imu; + Msg_Header msg_header; // Force usage of simulation time: msg_header.stamp = myNow(); msg_header.frame_id = sSensorFrameId; @@ -1333,18 +1334,18 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = std::make_shared(n_.advertise( + pub = std::make_shared(n_.advertise( vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else - pub = n_->create_publisher( + pub = n_->create_publisher( vehVarName(obs.sensorLabel, veh), publisher_history_len_); #endif } lck.unlock(); #if PACKAGE_ROS_VERSION == 2 - rclcpp::Publisher::SharedPtr pubImg = - std::dynamic_pointer_cast>( + rclcpp::Publisher::SharedPtr pubImg = + std::dynamic_pointer_cast>( pub); ASSERT_(pubImg); #endif @@ -1373,13 +1374,8 @@ void MVSimNode::internalOn( // Send observation: { // Convert observation MRPT -> ROS -#if PACKAGE_ROS_VERSION == 1 - sensor_msgs::Image msg_img; - std_msgs::Header msg_header; -#else - sensor_msgs::msg::Image msg_img; - std_msgs::msg::Header msg_header; -#endif + Msg_Image msg_img; + Msg_Header msg_header; msg_header.stamp = myNow(); msg_header.frame_id = sSensorFrameId; @@ -1415,27 +1411,27 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pubImg = std::make_shared(n_.advertise( + pubImg = std::make_shared(n_.advertise( vehVarName(lbImage, veh), publisher_history_len_)); - pubPts = std::make_shared(n_.advertise( + pubPts = std::make_shared(n_.advertise( vehVarName(lbPoints, veh), publisher_history_len_)); #else - pubImg = n_->create_publisher( + pubImg = n_->create_publisher( vehVarName(lbImage, veh), publisher_history_len_); - pubPts = n_->create_publisher( + pubPts = n_->create_publisher( vehVarName(lbPoints, veh), publisher_history_len_); #endif } lck.unlock(); #if PACKAGE_ROS_VERSION == 2 - rclcpp::Publisher::SharedPtr pubPoints = + rclcpp::Publisher::SharedPtr pubPoints = std::dynamic_pointer_cast< - rclcpp::Publisher>(pubPts); + rclcpp::Publisher>(pubPts); ASSERT_(pubPoints); - rclcpp::Publisher::SharedPtr pubImage = - std::dynamic_pointer_cast>( + rclcpp::Publisher::SharedPtr pubImage = + std::dynamic_pointer_cast>( pubImg); ASSERT_(pubImage); #endif @@ -1471,13 +1467,8 @@ void MVSimNode::internalOn( // Send observation: { // Convert observation MRPT -> ROS -#if PACKAGE_ROS_VERSION == 1 - sensor_msgs::Image msg_img; - std_msgs::Header msg_header; -#else - sensor_msgs::msg::Image msg_img; - std_msgs::msg::Header msg_header; -#endif + Msg_Image msg_img; + Msg_Header msg_header; msg_header.stamp = now; msg_header.frame_id = sSensorFrameId_image; @@ -1516,13 +1507,8 @@ void MVSimNode::internalOn( // Send observation: { // Convert observation MRPT -> ROS -#if PACKAGE_ROS_VERSION == 1 - sensor_msgs::PointCloud2 msg_pts; - std_msgs::Header msg_header; -#else - sensor_msgs::msg::PointCloud2 msg_pts; - std_msgs::msg::Header msg_header; -#endif + Msg_PointCloud2 msg_pts; + Msg_Header msg_header; msg_header.stamp = now; msg_header.frame_id = sSensorFrameId_points; @@ -1563,19 +1549,19 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pubPts = std::make_shared(n_.advertise( + pubPts = std::make_shared(n_.advertise( vehVarName(lbPoints, veh), publisher_history_len_)); #else - pubPts = n_->create_publisher( + pubPts = n_->create_publisher( vehVarName(lbPoints, veh), publisher_history_len_); #endif } lck.unlock(); #if PACKAGE_ROS_VERSION == 2 - rclcpp::Publisher::SharedPtr pubPoints = + rclcpp::Publisher::SharedPtr pubPoints = std::dynamic_pointer_cast< - rclcpp::Publisher>(pubPts); + rclcpp::Publisher>(pubPts); ASSERT_(pubPoints); #endif @@ -1607,13 +1593,8 @@ void MVSimNode::internalOn( // Send observation: { // Convert observation MRPT -> ROS -#if PACKAGE_ROS_VERSION == 1 - sensor_msgs::PointCloud2 msg_pts; - std_msgs::Header msg_header; -#else - sensor_msgs::msg::PointCloud2 msg_pts; - std_msgs::msg::Header msg_header; -#endif + Msg_PointCloud2 msg_pts; + Msg_Header msg_header; msg_header.stamp = now; msg_header.frame_id = sSensorFrameId_points; From 97b6431ea8276ce6db60730071c9ea296d13e2e5 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 11:09:37 +0900 Subject: [PATCH 06/16] cleanup namespaces --- mvsim_node_src/mvsim_node.cpp | 39 +++++++++++++---------------------- 1 file changed, 14 insertions(+), 25 deletions(-) diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index 926e6b86..332ccf45 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -18,6 +18,17 @@ #include #include "mvsim/mvsim_node_core.h" +#include +#include + +#if MRPT_VERSION >= 0x020b04 // >=2.11.4? +#define HAVE_POINTS_XYZIRT +#endif + +#if defined(HAVE_POINTS_XYZIRT) +#include +#endif + #if PACKAGE_ROS_VERSION == 1 // =========================================== // ROS 1 @@ -120,17 +131,6 @@ using Msg_Marker = visualization_msgs::msg::Marker; using Msg_MarkerArray = visualization_msgs::msg::MarkerArray; #endif -#if MRPT_VERSION >= 0x020b04 // >=2.11.4? -#define HAVE_POINTS_XYZIRT -#endif - -#if defined(HAVE_POINTS_XYZIRT) -#include -#endif - -#include -#include - #if PACKAGE_ROS_VERSION == 1 namespace mrpt2ros = mrpt::ros1bridge; #else @@ -361,8 +361,6 @@ void MVSimNode::spin() { using namespace mvsim; using namespace std::string_literals; - using mrpt::DEG2RAD; - using mrpt::RAD2DEG; if (!mvsim_world_) return; @@ -485,15 +483,13 @@ void MVSimNode::thread_update_GUI(TThreadParams& thread_params) { try { - using namespace mvsim; - MVSimNode* obj = thread_params.obj; while (!thread_params.closing) { if (obj->world_init_ok_ && !obj->headless_) { - World::TUpdateGUIParams guiparams; + mvsim::World::TUpdateGUIParams guiparams; guiparams.msg_lines = obj->msg2gui_; obj->mvsim_world_->update_GUI(&guiparams); @@ -650,8 +646,6 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) vehVarName("cmd_vel", *veh), 10, boost::bind(&MVSimNode::onROSMsgCmdVel, this, _1, veh))); #else - using std::placeholders::_1; - pubsubs.sub_cmd_vel = n_->create_subscription( vehVarName("cmd_vel", *veh), 10, [this, veh](const geometry_msgs::msg::Twist::ConstSharedPtr& msg) @@ -900,7 +894,6 @@ void MVSimNode::spinNotifyROS() { if (!mvsim_world_) return; - using namespace mvsim; const auto& vehs = mvsim_world_->getListOfVehicles(); // skip if the node is already shutting down: @@ -938,7 +931,7 @@ void MVSimNode::spinNotifyROS() for (auto it = vehs.begin(); it != vehs.end(); ++it, ++i) { - const VehicleBase::Ptr& veh = it->second; + const mvsim::VehicleBase::Ptr& veh = it->second; auto& pubs = pubsub_vehicles_[i]; const std::string sOdomName = vehVarName("odom", *veh); @@ -1090,8 +1083,6 @@ void MVSimNode::onNewObservation( { mrpt::system::CTimeLoggerEntry tle(profiler_, "onNewObservation"); - using mrpt::obs::CObservation2DRangeScan; - // skip if the node is already shutting down: #if PACKAGE_ROS_VERSION == 1 if (!ros::ok()) return; @@ -1110,7 +1101,7 @@ void MVSimNode::onNewObservation( // Observation: 2d laser scans // ----------------------------- if (const auto* o2DLidar = - dynamic_cast(obs.get()); + dynamic_cast(obs.get()); o2DLidar) { internalOn(veh, *o2DLidar); @@ -1165,8 +1156,6 @@ void MVSimNode::onNewObservation( std::string MVSimNode::vehVarName( const std::string& sVarName, const mvsim::VehicleBase& veh) const { - using namespace std::string_literals; - if (mvsim_world_->getListOfVehicles().size() == 1) { return sVarName; From c90a662c9840816fad2cf09a3ea8094051f046ce Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 11:15:15 +0900 Subject: [PATCH 07/16] merge ros logs --- mvsim_node_src/mvsim_node.cpp | 25 +++++++------------------ 1 file changed, 7 insertions(+), 18 deletions(-) diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index 332ccf45..f9629f9d 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -139,9 +139,13 @@ namespace mrpt2ros = mrpt::ros2bridge; #if PACKAGE_ROS_VERSION == 1 #define ROS12_INFO(...) ROS_INFO(__VA_ARGS__) +#define ROS12_WARN_THROTTLE(...) ROS_WARN_THROTTLE(__VA_ARGS__) +#define ROS12_WARN_STREAM_THROTTLE(...) ROS_WARN_STREAM_THROTTLE(__VA_ARGS__) #define ROS12_ERROR(...) ROS_ERROR(__VA_ARGS__) #else #define ROS12_INFO(...) RCLCPP_INFO(n_->get_logger(), __VA_ARGS__) +#define ROS12_WARN_THROTTLE(...) RCLCPP_WARN_THROTTLE(n_->get_logger(), *n_->get_clock(), __VA_ARGS__) +#define ROS12_WARN_STREAM_THROTTLE(...) RCLCPP_WARN_STREAM_THROTTLE(n_->get_logger(), *n_->get_clock(), __VA_ARGS__) #define ROS12_ERROR(...) RCLCPP_ERROR(n_->get_logger(), __VA_ARGS__) #endif @@ -349,7 +353,7 @@ void MVSimNode::configCallback( // Set class variables to new values. They should match what is input at the // dynamic reconfigure GUI. // message = config.message.c_str(); - ROS_INFO("MVSimNode::configCallback() called."); + ROS12_INFO("MVSimNode::configCallback() called."); if (mvsim_world_->is_GUI_open() && !config.show_gui) mvsim_world_->close_GUI(); @@ -875,17 +879,10 @@ void MVSimNode::onROSMsgCmdVel( if (!ctrlAcceptTwist) { -#if PACKAGE_ROS_VERSION == 1 - ROS_WARN_THROTTLE( + ROS12_WARN_THROTTLE( 1.0, "*Warning* Vehicle's controller ['%s'] refuses Twist commands!", veh->getName().c_str()); -#else - RCLCPP_WARN_THROTTLE( - n_->get_logger(), *n_->get_clock(), 1.0, - "*Warning* Vehicle's controller ['%s'] refuses Twist commands!", - veh->getName().c_str()); -#endif } } @@ -1135,18 +1132,10 @@ void MVSimNode::onNewObservation( else { // Don't know how to emit this observation to ROS! -#if PACKAGE_ROS_VERSION == 1 - ROS_WARN_STREAM_THROTTLE( + ROS12_WARN_STREAM_THROTTLE( 1.0, "Do not know how to publish this observation to ROS: '" << obs->sensorLabel << "', class: " << obs->GetRuntimeClass()->className); -#else - RCLCPP_WARN_STREAM_THROTTLE( - n_->get_logger(), *n_->get_clock(), 1.0, - "Do not know how to publish this observation to ROS: '" - << obs->sensorLabel - << "', class: " << obs->GetRuntimeClass()->className); -#endif } } // end of onNewObservation() From 75c72d31369be8a37e24d202f91afe7bb535d858 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 22:06:39 +0900 Subject: [PATCH 08/16] add publisher wrapper class --- CMakeLists.txt | 13 +- .../include/mvsim/mvsim_node_core.h | 52 +++++-- .../include/wrapper/publisher_wrapper.h | 35 +++++ mvsim_node_src/mvsim_node.cpp | 135 ++++-------------- mvsim_node_src/mvsim_node_main.cpp | 2 +- 5 files changed, 110 insertions(+), 127 deletions(-) create mode 100644 mvsim_node_src/include/wrapper/publisher_wrapper.h diff --git a/CMakeLists.txt b/CMakeLists.txt index d33b5a63..83410e0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,7 @@ if("$ENV{ROS_VERSION}" STREQUAL "2") elseif("$ENV{ROS_VERSION}" STREQUAL "1") set(DETECTED_ROS1 TRUE) set(PACKAGE_ROS_VERSION 1) - set (CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 14) endif() if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) @@ -68,7 +68,7 @@ if(CMAKE_COMPILER_IS_GNUCXX) set(CMAKE_CXX_FLAGS_SANITIZEADDRESS "-fsanitize=address -fsanitize=leak -g") set(CMAKE_EXE_LINKER_FLAGS_SANITIZEADDRESS "-fsanitize=address -fsanitize=leak") set(CMAKE_SHARED_LINKER_FLAGS_SANITIZEADDRESS "-fsanitize=address -fsanitize=leak") - + # BUILD_TYPE: SanitizeThread set(CMAKE_CXX_FLAGS_SANITIZETHREAD "-fsanitize=thread -g") set(CMAKE_EXE_LINKER_FLAGS_SANITIZETHREAD "-fsanitize=thread") @@ -211,7 +211,7 @@ set(EMBEDDED_box2d_DIR "${EMBEDDED_box2d_INSTALL_DIR}/lib/cmake/box2d/") # Since box2d 2.4.1, the name changed "Box2D" -> "box2d". We now only support the version >=2.4.x # for compatibility with modern Ubuntu distros: # -# Update: Since MVSIM 0.7.0, we now always ship a custom build of box2d to +# Update: Since MVSIM 0.7.0, we now always ship a custom build of box2d to # increase its default maximum number of polygon vertices: ##find_package(box2d QUIET) # Defines: box2d::box2d @@ -290,8 +290,9 @@ if (BUILD_FOR_ROS) mvsim_node_src/mvsim_node.cpp mvsim_node_src/mvsim_node_main.cpp mvsim_node_src/include/mvsim/mvsim_node_core.h + mvsim_node_src/include/wrapper/publisher_wrapper.h ) - + mvsim_set_target_build_options(mvsim_node) target_include_directories(mvsim_node PRIVATE "mvsim_node_src/include") @@ -307,7 +308,7 @@ if (BUILD_FOR_ROS) mvsim_gencfg ) - target_link_libraries(mvsim_node + target_link_libraries(mvsim_node #PRIVATE # ros catkin already used the plain signature, we cannot use this here... ${catkin_LIBRARIES} mrpt::ros1bridge @@ -410,7 +411,7 @@ option(MVSIM_UNIT_TESTS "Build and run MVSIM unit tests (requires Python and ZMQ # Disable tests in armhf, since they fail due to (probably?) very slow rendering capabilities. # See: https://github.com/ros-infrastructure/ros_buildfarm_config/pull/220 -if (MVSIM_UNIT_TESTS AND +if (MVSIM_UNIT_TESTS AND ( ("armhf" STREQUAL "${CMAKE_SYSTEM_PROCESSOR}") OR ("arm64" STREQUAL "${CMAKE_SYSTEM_PROCESSOR}") OR diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h index 09842f01..4d802807 100644 --- a/mvsim_node_src/include/mvsim/mvsim_node_core.h +++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h @@ -45,11 +45,33 @@ #include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time_source.hpp" + +#include "wrapper/publisher_wrapper.h" #endif #include #include +namespace mvsim_node { +#if PACKAGE_ROS_VERSION == 1 + template + boost::shared_ptr make_shared(Args&&... args) { + return boost::make_shared(std::forward(args)...); + } + + template + using shared_ptr = boost::shared_ptr; +#else + template + std::shared_ptr make_shared(Args&&... args) { + return std::make_shared(std::forward(args)...); + } + + template + using shared_ptr = std::shared_ptr; +#endif +} + namespace mrpt { namespace obs @@ -91,8 +113,8 @@ class MVSimNode /// The mvsim library simulated world (includes everything: vehicles, /// obstacles, etc.) - std::shared_ptr mvsim_world_ = - std::make_shared(); + mvsim_node::shared_ptr mvsim_world_ = + mvsim_node::make_shared(); mrpt::WorkerThreadsPool ros_publisher_workers_{ 4 /*threads*/, mrpt::WorkerThreadsPool::POLICY_FIFO}; @@ -112,7 +134,7 @@ class MVSimNode double transform_tolerance_ = 0.1; protected: - std::shared_ptr mvsim_server_; + mvsim_node::shared_ptr mvsim_server_; #if PACKAGE_ROS_VERSION == 1 ros::NodeHandle& n_; @@ -144,23 +166,23 @@ class MVSimNode struct TPubSubPerVehicle { #if PACKAGE_ROS_VERSION == 1 - std::shared_ptr sub_cmd_vel; //!< Subscribers vehicle's "cmd_vel" topic - std::shared_ptr pub_odom; //!< Publisher of "odom" topic - std::shared_ptr pub_ground_truth; //!< "base_pose_ground_truth" topic + mvsim_node::shared_ptr sub_cmd_vel; //!< Subscribers vehicle's "cmd_vel" topic + mvsim_node::shared_ptr pub_odom; //!< Publisher of "odom" topic + mvsim_node::shared_ptr pub_ground_truth; //!< "base_pose_ground_truth" topic /// "fake_localization" pubs: - std::shared_ptr pub_amcl_pose; //!< Publisher of "amcl_pose" topic - std::shared_ptr pub_particlecloud; //!< Publisher of "particlecloud" topic + mvsim_node::shared_ptr pub_amcl_pose; //!< Publisher of "amcl_pose" topic + mvsim_node::shared_ptr pub_particlecloud; //!< Publisher of "particlecloud" topic /// Map => publisher - std::map> pub_sensors; + std::map> pub_sensors; - std::shared_ptr pub_chassis_markers; //!< "/chassis_markers" - std::shared_ptr pub_chassis_shape; //!< "/chassis_shape" - std::shared_ptr pub_collision; //!< "/collision" + mvsim_node::shared_ptr pub_chassis_markers; //!< "/chassis_markers" + mvsim_node::shared_ptr pub_chassis_shape; //!< "/chassis_shape" + mvsim_node::shared_ptr pub_collision; //!< "/collision" - std::shared_ptr pub_tf; //!< "/tf" - std::shared_ptr pub_tf_static; //!< "/tf_static" + mvsim_node::shared_ptr pub_tf; //!< "/tf" + mvsim_node::shared_ptr pub_tf_static; //!< "/tf_static" visualization_msgs::MarkerArray chassis_shape_msg; #else @@ -178,7 +200,7 @@ class MVSimNode pub_particlecloud; /// Map => publisher - std::map pub_sensors; + std::map> pub_sensors; /// "/chassis_markers" rclcpp::Publisher::SharedPtr diff --git a/mvsim_node_src/include/wrapper/publisher_wrapper.h b/mvsim_node_src/include/wrapper/publisher_wrapper.h new file mode 100644 index 00000000..ba68ce0a --- /dev/null +++ b/mvsim_node_src/include/wrapper/publisher_wrapper.h @@ -0,0 +1,35 @@ +#pragma once + +#if PACKAGE_ROS_VERSION == 1 +#else + +#include "rclcpp/rclcpp.hpp" + +class PublisherWrapperBase { + public: + PublisherWrapperBase() = default; + virtual ~PublisherWrapperBase() = default; + virtual void publish(std::shared_ptr message) = 0; +}; + + +template +class PublisherWrapper : public PublisherWrapperBase { + public: + PublisherWrapper(rclcpp::Node::SharedPtr node, const std::string& topic_name, size_t qos) + : publisher_(node->create_publisher(topic_name, qos)) {} + + void publish(std::shared_ptr msg) override { + auto typed_msg = std::static_pointer_cast(msg); + if (typed_msg) { + publisher_->publish(*typed_msg); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to cast message to correct type."); + } + } + + private: + typename rclcpp::Publisher::SharedPtr publisher_; +}; + +#endif diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index f9629f9d..ab1d2436 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -297,7 +297,7 @@ void MVSimNode::launch_mvsim_server() ASSERT_(!mvsim_server_); // Start network server: - mvsim_server_ = std::make_shared(); + mvsim_server_ = mvsim_node::make_shared(); mvsim_server_->start(); } @@ -646,7 +646,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) { // sub: /cmd_vel #if PACKAGE_ROS_VERSION == 1 - pubsubs.sub_cmd_vel = std::make_shared(n_.subscribe( + pubsubs.sub_cmd_vel = mvsim_node::make_shared(n_.subscribe( vehVarName("cmd_vel", *veh), 10, boost::bind(&MVSimNode::onROSMsgCmdVel, this, _1, veh))); #else @@ -658,21 +658,21 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #if PACKAGE_ROS_VERSION == 1 // pub: /odom - pubsubs.pub_odom = std::make_shared(n_.advertise( + pubsubs.pub_odom = mvsim_node::make_shared(n_.advertise( vehVarName("odom", *veh), publisher_history_len_)); // pub: /base_pose_ground_truth - pubsubs.pub_ground_truth = std::make_shared(n_.advertise( + pubsubs.pub_ground_truth = mvsim_node::make_shared(n_.advertise( vehVarName("base_pose_ground_truth", *veh), publisher_history_len_)); // pub: /collision - pubsubs.pub_collision = std::make_shared(n_.advertise( + pubsubs.pub_collision = mvsim_node::make_shared(n_.advertise( vehVarName("collision", *veh), publisher_history_len_)); // pub: /tf, /tf_static - pubsubs.pub_tf = std::make_shared(n_.advertise( + pubsubs.pub_tf = mvsim_node::make_shared(n_.advertise( vehVarName("tf", *veh), publisher_history_len_)); - pubsubs.pub_tf_static = std::make_shared(n_.advertise( + pubsubs.pub_tf_static = mvsim_node::make_shared(n_.advertise( vehVarName("tf_static", *veh), publisher_history_len_)); #else // pub: /odom @@ -702,7 +702,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) { #if PACKAGE_ROS_VERSION == 1 pubsubs.pub_chassis_markers = - std::make_shared(n_.advertise( + mvsim_node::make_shared(n_.advertise( vehVarName("chassis_markers", *veh), 5, true /*latch*/)); #else rclcpp::QoS qosLatched5(rclcpp::KeepLast(5)); @@ -792,7 +792,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) // pub: /chassis_polygon { #if PACKAGE_ROS_VERSION == 1 - pubsubs.pub_chassis_shape = std::make_shared(n_.advertise( + pubsubs.pub_chassis_shape = mvsim_node::make_shared(n_.advertise( vehVarName("chassis_polygon", *veh), 1, true /*latch*/)); #else rclcpp::QoS qosLatched1(rclcpp::KeepLast(1)); @@ -822,10 +822,10 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #if PACKAGE_ROS_VERSION == 1 // pub: /amcl_pose pubsubs.pub_amcl_pose = - std::make_shared(n_.advertise( + mvsim_node::make_shared(n_.advertise( vehVarName("amcl_pose", *veh), 1)); // pub: /particlecloud - pubsubs.pub_particlecloud = std::make_shared(n_.advertise( + pubsubs.pub_particlecloud = mvsim_node::make_shared(n_.advertise( vehVarName("particlecloud", *veh), 1)); #else // pub: /amcl_pose @@ -1170,22 +1170,15 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = std::make_shared(n_.advertise( + pub = mvsim_node::make_shared(n_.advertise( vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else - pub = n_->create_publisher( + pub = mvsim_node::make_shared>(n_, vehVarName(obs.sensorLabel, veh), publisher_history_len_); #endif } lck.unlock(); -#if PACKAGE_ROS_VERSION == 2 - rclcpp::Publisher::SharedPtr pubLidar = - std::dynamic_pointer_cast< - rclcpp::Publisher>(pub); - ASSERT_(pubLidar); -#endif - const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh); // Send TF: @@ -1212,17 +1205,11 @@ void MVSimNode::internalOn( // Convert observation MRPT -> ROS Msg_Pose msg_pose_laser; Msg_LaserScan msg_laser; - mrpt2ros::toROS(obs, msg_laser, msg_pose_laser); - // Force usage of simulation time: msg_laser.header.stamp = myNow(); msg_laser.header.frame_id = sSensorFrameId; - -#if PACKAGE_ROS_VERSION == 1 - pub->publish(msg_laser); -#else - pubLidar->publish(msg_laser); -#endif + mrpt2ros::toROS(obs, msg_laser, msg_pose_laser); + pub->publish(mvsim_node::make_shared(msg_laser)); } } @@ -1240,23 +1227,15 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = std::make_shared(n_.advertise( + pub = mvsim_node::make_shared(n_.advertise( vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else - pub = n_->create_publisher( + pub = mvsim_node::make_shared>(n_, vehVarName(obs.sensorLabel, veh), publisher_history_len_); - ASSERT_(pub); #endif } lck.unlock(); -#if PACKAGE_ROS_VERSION == 2 - rclcpp::Publisher::SharedPtr pubImu = - std::dynamic_pointer_cast>( - pub); - ASSERT_(pubImu); -#endif - const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh); // Send TF: @@ -1287,14 +1266,8 @@ void MVSimNode::internalOn( // Force usage of simulation time: msg_header.stamp = myNow(); msg_header.frame_id = sSensorFrameId; - mrpt2ros::toROS(obs, msg_header, msg_imu); - -#if PACKAGE_ROS_VERSION == 1 - pub->publish(msg_imu); -#else - pubImu->publish(msg_imu); -#endif + pub->publish(mvsim_node::make_shared(msg_imu)); } } @@ -1312,22 +1285,15 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = std::make_shared(n_.advertise( + pub = mvsim_node::make_shared(n_.advertise( vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else - pub = n_->create_publisher( + pub = mvsim_node::make_shared>(n_, vehVarName(obs.sensorLabel, veh), publisher_history_len_); #endif } lck.unlock(); -#if PACKAGE_ROS_VERSION == 2 - rclcpp::Publisher::SharedPtr pubImg = - std::dynamic_pointer_cast>( - pub); - ASSERT_(pubImg); -#endif - const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh); // Send TF: @@ -1356,14 +1322,8 @@ void MVSimNode::internalOn( Msg_Header msg_header; msg_header.stamp = myNow(); msg_header.frame_id = sSensorFrameId; - msg_img = mrpt2ros::toROS(obs.image, msg_header); - -#if PACKAGE_ROS_VERSION == 1 - pub->publish(msg_img); -#else - pubImg->publish(msg_img); -#endif + pub->publish(mvsim_node::make_shared(msg_img)); } } @@ -1389,31 +1349,19 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pubImg = std::make_shared(n_.advertise( + pubImg = mvsim_node::make_shared(n_.advertise( vehVarName(lbImage, veh), publisher_history_len_)); - pubPts = std::make_shared(n_.advertise( + pubPts = mvsim_node::make_shared(n_.advertise( vehVarName(lbPoints, veh), publisher_history_len_)); #else - pubImg = n_->create_publisher( + pubImg = mvsim_node::make_shared>(n_, vehVarName(lbImage, veh), publisher_history_len_); - pubPts = n_->create_publisher( + pubPts = mvsim_node::make_shared>(n_, vehVarName(lbPoints, veh), publisher_history_len_); #endif } lck.unlock(); -#if PACKAGE_ROS_VERSION == 2 - rclcpp::Publisher::SharedPtr pubPoints = - std::dynamic_pointer_cast< - rclcpp::Publisher>(pubPts); - ASSERT_(pubPoints); - - rclcpp::Publisher::SharedPtr pubImage = - std::dynamic_pointer_cast>( - pubImg); - ASSERT_(pubImage); -#endif - const std::string sSensorFrameId_image = vehVarName(lbImage, veh); const std::string sSensorFrameId_points = vehVarName(lbPoints, veh); @@ -1449,14 +1397,8 @@ void MVSimNode::internalOn( Msg_Header msg_header; msg_header.stamp = now; msg_header.frame_id = sSensorFrameId_image; - msg_img = mrpt2ros::toROS(obs.intensityImage, msg_header); - -#if PACKAGE_ROS_VERSION == 1 - pubImg->publish(msg_img); -#else - pubImage->publish(msg_img); -#endif + pubImg->publish(mvsim_node::make_shared(msg_img)); } } @@ -1496,14 +1438,8 @@ void MVSimNode::internalOn( mrpt::maps::CSimplePointsMap pts; const_cast(obs).unprojectInto( pts, pp); - mrpt2ros::toROS(pts, msg_header, msg_pts); - -#if PACKAGE_ROS_VERSION == 1 - pubPts->publish(msg_pts); -#else - pubPoints->publish(msg_pts); -#endif + pubPts->publish(mvsim_node::make_shared(msg_pts)); } } } @@ -1527,22 +1463,15 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pubPts = std::make_shared(n_.advertise( + pubPts = mvsim_node::make_shared(n_.advertise( vehVarName(lbPoints, veh), publisher_history_len_)); #else - pubPts = n_->create_publisher( + pubPts = mvsim_node::make_shared>(n_, vehVarName(lbPoints, veh), publisher_history_len_); #endif } lck.unlock(); -#if PACKAGE_ROS_VERSION == 2 - rclcpp::Publisher::SharedPtr pubPoints = - std::dynamic_pointer_cast< - rclcpp::Publisher>(pubPts); - ASSERT_(pubPoints); -#endif - const std::string sSensorFrameId_points = vehVarName(lbPoints, veh); const auto now = myNow(); @@ -1603,10 +1532,6 @@ void MVSimNode::internalOn( "Do not know how to handle this variant of CPointsMap"); } -#if PACKAGE_ROS_VERSION == 1 - pubPts->publish(msg_pts); -#else - pubPoints->publish(msg_pts); -#endif + pubPts->publish(mvsim_node::make_shared(msg_pts)); } } diff --git a/mvsim_node_src/mvsim_node_main.cpp b/mvsim_node_src/mvsim_node_main.cpp index 92809edc..1108d9ba 100644 --- a/mvsim_node_src/mvsim_node_main.cpp +++ b/mvsim_node_src/mvsim_node_main.cpp @@ -30,7 +30,7 @@ int main(int argc, char** argv) try { // Create a "Node" object. - std::shared_ptr node = std::make_shared(n); + mvsim_node::shared_ptr node = mvsim_node::make_shared(n); // Declare variables that can be modified by launch file or command // line. From c26b1f7efcad84d8577036ddce1b2009a7f41f6d Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 14:43:43 +0900 Subject: [PATCH 09/16] cleanup namespace --- mvsim_node_src/include/mvsim/mvsim_node_core.h | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h index 4d802807..aa008ac4 100644 --- a/mvsim_node_src/include/mvsim/mvsim_node_core.h +++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include #include @@ -72,14 +72,6 @@ namespace mvsim_node { #endif } -namespace mrpt -{ -namespace obs -{ -class CObservationPointCloud; -} -} // namespace mrpt - /** A class to wrap libmvsim as a ROS node */ class MVSimNode 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 10/16] 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 pub_map_ros_, pub_map_metadata_; + // mvsim_node::shared_ptr pub_clock_; #else rclcpp::Publisher::SharedPtr pub_map_ros_; rclcpp::Publisher::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("/clock", 10); + // pub_clock_ = mvsim_node::make_shared(n_.advertise("/clock", 10)); - pub_map_ros_ = n_.advertise( - "simul_map", 1 /*queue len*/, true /*latch*/); - pub_map_metadata_ = n_.advertise( - "simul_map_metadata", 1 /*queue len*/, true /*latch*/); + pub_map_ros_ = mvsim_node::make_shared(n_.advertise( + "simul_map", 1 /*queue len*/, true /*latch*/)); + pub_map_metadata_ = mvsim_node::make_shared(n_.advertise( + "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?"); From 045ab21bdcff2e8570b7123894f5c96944e33472 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 22:14:44 +0900 Subject: [PATCH 11/16] move & merge usings to header --- .../include/mvsim/mvsim_node_core.h | 94 ++++++++++++------- mvsim_node_src/mvsim_node.cpp | 53 +---------- 2 files changed, 66 insertions(+), 81 deletions(-) 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 -#include -#endif +#include +#include #include #include +#include +#include #include #include #include @@ -26,31 +26,65 @@ #include #if PACKAGE_ROS_VERSION == 1 -#include #include #include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include #include #include #include + +// 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 +#include +#include + #include #include #include -#include +#include #include #include +#include #include #include -#include "rclcpp/clock.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time_source.hpp" - #include "wrapper/publisher_wrapper.h" -#endif -#include -#include +// 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 pub_map_ros_, pub_map_metadata_; // mvsim_node::shared_ptr pub_clock_; #else - rclcpp::Publisher::SharedPtr pub_map_ros_; - rclcpp::Publisher::SharedPtr pub_map_metadata_; + rclcpp::Publisher::SharedPtr pub_map_ros_; + rclcpp::Publisher::SharedPtr pub_map_metadata_; rclcpp::TimeSource ts_{n_}; rclcpp::Clock::SharedPtr clock_; #endif @@ -176,38 +210,38 @@ class MVSimNode mvsim_node::shared_ptr pub_tf; //!< "/tf" mvsim_node::shared_ptr pub_tf_static; //!< "/tf_static" - visualization_msgs::MarkerArray chassis_shape_msg; + Msg_MarkerArray chassis_shape_msg; #else /// Subscribers vehicle's "cmd_vel" topic - rclcpp::Subscription::SharedPtr sub_cmd_vel; + rclcpp::Subscription::SharedPtr sub_cmd_vel; /// Publisher of "odom" topic - rclcpp::Publisher::SharedPtr pub_odom; + rclcpp::Publisher::SharedPtr pub_odom; /// "base_pose_ground_truth" topic - rclcpp::Publisher::SharedPtr pub_ground_truth; + rclcpp::Publisher::SharedPtr pub_ground_truth; /// "fake_localization" pubs: - rclcpp::Publisher:: + rclcpp::Publisher:: SharedPtr pub_amcl_pose; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_particlecloud; /// Map => publisher std::map> pub_sensors; /// "/chassis_markers" - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_chassis_markers; /// "/chassis_shape" - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_chassis_shape; /// "/collision" - rclcpp::Publisher::SharedPtr pub_collision; + rclcpp::Publisher::SharedPtr pub_collision; /// "/tf", "/tf_static" - rclcpp::Publisher::SharedPtr pub_tf; - rclcpp::Publisher::SharedPtr pub_tf_static; + rclcpp::Publisher::SharedPtr pub_tf; + rclcpp::Publisher::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 -#include -#include -#include -#include #include #include // kbhit() #include #include -#include "mvsim/mvsim_node_core.h" +#include -#include -#include +#include "rapidxml_utils.hpp" #if MRPT_VERSION >= 0x020b04 // >=2.11.4? #define HAVE_POINTS_XYZIRT @@ -40,44 +35,24 @@ #include #include -#include -#include -#include #include #include #include #include -#include #include -#include -#include -#include - // 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 #include -#include -#include -#include #include #include #include #include -#include // 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( 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(); From c56239a70d25bdcfbaef3883c30b39d0757dd877 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 22:29:25 +0900 Subject: [PATCH 12/16] merge ros/rclcpp --- .../include/mvsim/mvsim_node_core.h | 29 ++--- mvsim_node_src/mvsim_node.cpp | 106 ++++++++---------- 2 files changed, 60 insertions(+), 75 deletions(-) diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h index e47ac2f7..a96e9e72 100644 --- a/mvsim_node_src/include/mvsim/mvsim_node_core.h +++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h @@ -44,6 +44,9 @@ #include // usings: +using ros_Time = ros::Time; +using ros_Duration = ros::Duration; + using Msg_Polygon = geometry_msgs::Polygon; using Msg_PoseArray = geometry_msgs::PoseArray; using Msg_PoseWithCovarianceStamped = geometry_msgs::PoseWithCovarianceStamped; @@ -73,6 +76,9 @@ using Msg_MarkerArray = visualization_msgs::MarkerArray; #include "wrapper/publisher_wrapper.h" // usings: +using ros_Time = rclcpp::Time; +using ros_Duration = rclcpp::Duration; + using Msg_Polygon = geometry_msgs::msg::Polygon; using Msg_PoseArray = geometry_msgs::msg::PoseArray; using Msg_PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; @@ -264,14 +270,10 @@ class MVSimNode #if PACKAGE_ROS_VERSION == 1 // rosgraph_msgs::Clock clockMsg_; - // ros::Time sim_time_; //!< Current simulation time - ros::Time base_last_cmd_; //!< received a vel_cmd (for watchdog) - ros::Duration base_watchdog_timeout_; -#else - // rclcpp::Time sim_time_; //!< Current simulation time - rclcpp::Time base_last_cmd_; //!< received a vel_cmd (for watchdog) - rclcpp::Duration base_watchdog_timeout_ = std::chrono::seconds(1); #endif + // ros_Time sim_time_; //!< Current simulation time + ros_Time base_last_cmd_; //!< received a vel_cmd (for watchdog) + ros_Duration base_watchdog_timeout_ = ros_Duration(1, 0); /// Unit transform (const, once) const tf2::Transform tfIdentity_ = tf2::Transform::getIdentity(); @@ -327,18 +329,11 @@ class MVSimNode void sendStaticTF( const std::string& frame_id, const std::string& child_frame_id, const tf2::Transform& tx, -#if PACKAGE_ROS_VERSION == 1 - const ros::Time& stamp -#else - const rclcpp::Time& stamp -#endif + const ros_Time& stamp ); -#if PACKAGE_ROS_VERSION == 1 - ros::Time myNow() const; -#else - rclcpp::Time myNow() const; -#endif + ros_Time myNow() const; + double myNowSec() const; mrpt::system::CTimeLogger profiler_{true /*enabled*/, "mvsim_node"}; diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index b40f3b94..80b347ce 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -42,6 +42,8 @@ #include // usings: +using ros::ok; + using Msg_Header = std_msgs::Header; using Msg_Pose = geometry_msgs::Pose; @@ -77,6 +79,8 @@ using Msg_Marker = visualization_msgs::Marker; #endif // usings: +using rclcpp::ok; + using Msg_Header = std_msgs::msg::Header; using Msg_Pose = geometry_msgs::msg::Pose; @@ -103,8 +107,8 @@ namespace mrpt2ros = mrpt::ros2bridge; #define ROS12_ERROR(...) ROS_ERROR(__VA_ARGS__) #else #define ROS12_INFO(...) RCLCPP_INFO(n_->get_logger(), __VA_ARGS__) -#define ROS12_WARN_THROTTLE(...) RCLCPP_WARN_THROTTLE(n_->get_logger(), *n_->get_clock(), __VA_ARGS__) -#define ROS12_WARN_STREAM_THROTTLE(...) RCLCPP_WARN_STREAM_THROTTLE(n_->get_logger(), *n_->get_clock(), __VA_ARGS__) +#define ROS12_WARN_THROTTLE(...) RCLCPP_WARN_THROTTLE(n_->get_logger(), *clock_, __VA_ARGS__) +#define ROS12_WARN_STREAM_THROTTLE(...) RCLCPP_WARN_STREAM_THROTTLE(n_->get_logger(), *clock_, __VA_ARGS__) #define ROS12_ERROR(...) RCLCPP_ERROR(n_->get_logger(), __VA_ARGS__) #endif @@ -121,7 +125,29 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) #endif : n_(n) { -#if PACKAGE_ROS_VERSION == 2 + // Node parameters: +#if PACKAGE_ROS_VERSION == 1 + double t; + if (!localn_.getParam("base_watchdog_timeout", t)) t = 0.2; + base_watchdog_timeout_.fromSec(t); + localn_.param("realtime_factor", realtime_factor_, 1.0); + localn_.param( + "gui_refresh_period", gui_refresh_period_ms_, gui_refresh_period_ms_); + localn_.param("headless", headless_, headless_); + localn_.param( + "period_ms_publish_tf", period_ms_publish_tf_, period_ms_publish_tf_); + localn_.param( + "do_fake_localization", do_fake_localization_, do_fake_localization_); + + // JLBC: At present, mvsim does not use sim_time for neither ROS 1 nor + // ROS 2. + // n_.setParam("use_sim_time", false); + if (true == localn_.param("use_sim_time", false)) + { + THROW_EXCEPTION( + "At present, MVSIM can only work with use_sim_time=false"); + } +#else clock_ = n_->get_clock(); ts_.attachClock(clock_); @@ -192,30 +218,6 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) base_last_cmd_ = rclcpp::Time(0); #endif - // Node parameters: -#if PACKAGE_ROS_VERSION == 1 - double t; - if (!localn_.getParam("base_watchdog_timeout", t)) t = 0.2; - base_watchdog_timeout_.fromSec(t); - localn_.param("realtime_factor", realtime_factor_, 1.0); - localn_.param( - "gui_refresh_period", gui_refresh_period_ms_, gui_refresh_period_ms_); - localn_.param("headless", headless_, headless_); - localn_.param( - "period_ms_publish_tf", period_ms_publish_tf_, period_ms_publish_tf_); - localn_.param( - "do_fake_localization", do_fake_localization_, do_fake_localization_); - - // JLBC: At present, mvsim does not use sim_time for neither ROS 1 nor - // ROS 2. - // n_.setParam("use_sim_time", false); - if (true == localn_.param("use_sim_time", false)) - { - THROW_EXCEPTION( - "At present, MVSIM can only work with use_sim_time=false"); - } -#endif - mvsim_world_->registerCallbackOnObservation( [this]( const mvsim::Simulable& veh, @@ -416,11 +418,7 @@ void MVSimNode::spin() } // end refresh teleop stuff // Check cmd_vel timeout: -#if PACKAGE_ROS_VERSION == 1 - const double rosNow = ros::Time::now().toSec(); -#else - const double rosNow = n_->get_clock()->now().nanoseconds() * 1e-9; -#endif + const double rosNow = myNowSec(); std::set toRemove; for (const auto& [veh, cmdVelTimestamp] : lastCmdVelTimestamp_) { @@ -515,12 +513,11 @@ void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj) #if PACKAGE_ROS_VERSION == 1 static size_t loop_count = 0; - ros_map.header.stamp = ros::Time::now(); ros_map.header.seq = loop_count++; #else - ros_map.header.stamp = clock_->now(); ros_map.header.frame_id = "map"; #endif + ros_map.header.stamp = myNow(); pub_map_ros_->publish(ros_map); pub_map_metadata_->publish(ros_map.info); @@ -568,20 +565,26 @@ void MVSimNode::notifyROSWorldIsUpdated() // sendStaticTF("world", "map", tfIdentity_, myNow()); } +ros_Time MVSimNode::myNow() const { #if PACKAGE_ROS_VERSION == 1 -ros::Time MVSimNode::myNow() const { return ros::Time::now(); } + return ros::Time::now(); #else -rclcpp::Time MVSimNode::myNow() const { return n_->get_clock()->now(); } + return clock_->now(); #endif +} -void MVSimNode::sendStaticTF( - const std::string& frame_id, const std::string& child_frame_id, - const tf2::Transform& txf, +double MVSimNode::myNowSec() const { #if PACKAGE_ROS_VERSION == 1 - const ros::Time& stamp + return ros::Time::now().toSec(); #else - const rclcpp::Time& stamp + return clock_->now().nanoseconds() * 1e-9; #endif +} + +void MVSimNode::sendStaticTF( + const std::string& frame_id, const std::string& child_frame_id, + const tf2::Transform& txf, + const ros_Time& stamp ) { Msg_TransformStamped tx; @@ -815,12 +818,7 @@ void MVSimNode::onROSMsgCmdVel( mvsim::ControllerBaseInterface* controller = veh->getControllerInterface(); // Update cmd_vel timestamp: -#if PACKAGE_ROS_VERSION == 1 - const double rosNow = ros::Time::now().toSec(); -#else - const double rosNow = n_->get_clock()->now().nanoseconds() * 1e-9; -#endif - lastCmdVelTimestamp_[veh] = rosNow; + lastCmdVelTimestamp_[veh] = myNowSec(); const bool ctrlAcceptTwist = controller->setTwistCommand( {cmd->linear.x, cmd->linear.y, cmd->angular.z}); @@ -842,11 +840,7 @@ void MVSimNode::spinNotifyROS() const auto& vehs = mvsim_world_->getListOfVehicles(); // skip if the node is already shutting down: -#if PACKAGE_ROS_VERSION == 1 - if (!ros::ok()) return; -#else - if (!rclcpp::ok()) return; -#endif + if (!ok()) return; // Get current simulation time (for messages) and publish "/clock" // ---------------------------------------------------------------- @@ -855,7 +849,7 @@ void MVSimNode::spinNotifyROS() // clockMsg_.clock = sim_time_; // pub_clock_->publish(clockMsg_); #else - // sim_time_ = n_->get_clock()->now(); + // sim_time_ = myNow(); // MRPT_TODO("Publish /clock for ROS2 too?"); #endif @@ -1029,11 +1023,7 @@ void MVSimNode::onNewObservation( mrpt::system::CTimeLoggerEntry tle(profiler_, "onNewObservation"); // skip if the node is already shutting down: -#if PACKAGE_ROS_VERSION == 1 - if (!ros::ok()) return; -#else - if (!rclcpp::ok()) return; -#endif + if (!ok()) return; ASSERT_(obs); ASSERT_(!obs->sensorLabel.empty()); From 2f29b14d8e407ba54ad143efe64e91a11acbbf06 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Wed, 7 Aug 2024 16:55:09 +0900 Subject: [PATCH 13/16] use lambda instead of bind (c++14) --- mvsim_node_src/mvsim_node.cpp | 3 ++- mvsim_node_src/mvsim_node_main.cpp | 8 +++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index 80b347ce..d810e4c1 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -603,7 +603,8 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #if PACKAGE_ROS_VERSION == 1 pubsubs.sub_cmd_vel = mvsim_node::make_shared(n_.subscribe( vehVarName("cmd_vel", *veh), 10, - boost::bind(&MVSimNode::onROSMsgCmdVel, this, _1, veh))); + [this, veh](Msg_Twist_CSPtr msg) + { return this->onROSMsgCmdVel(msg, veh); })); #else pubsubs.sub_cmd_vel = n_->create_subscription( vehVarName("cmd_vel", *veh), 10, diff --git a/mvsim_node_src/mvsim_node_main.cpp b/mvsim_node_src/mvsim_node_main.cpp index 1108d9ba..4567dd91 100644 --- a/mvsim_node_src/mvsim_node_main.cpp +++ b/mvsim_node_src/mvsim_node_main.cpp @@ -68,9 +68,11 @@ int main(int argc, char** argv) // Do this before parameter server, else some of the parameter server // values can be overwritten. dynamic_reconfigure::Server dr_srv; - dynamic_reconfigure::Server::CallbackType cb; - cb = boost::bind(&MVSimNode::configCallback, node.get(), _1, _2); - dr_srv.setCallback(cb); + dr_srv.setCallback( + [&node](mvsim::mvsimNodeConfig& config, uint32_t level) + { + return node->configCallback(config, level); + }); #endif // Tell ROS how fast to run this node-> From 8003fa7d80264255e3e1e9c85e154d5f7dc2fa6b Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Thu, 8 Aug 2024 14:07:56 +0900 Subject: [PATCH 14/16] set const & auto --- .../include/wrapper/publisher_wrapper.h | 2 +- mvsim_node_src/mvsim_node.cpp | 58 ++++++++----------- mvsim_node_src/mvsim_node_main.cpp | 4 +- 3 files changed, 28 insertions(+), 36 deletions(-) diff --git a/mvsim_node_src/include/wrapper/publisher_wrapper.h b/mvsim_node_src/include/wrapper/publisher_wrapper.h index ba68ce0a..c7471f74 100644 --- a/mvsim_node_src/include/wrapper/publisher_wrapper.h +++ b/mvsim_node_src/include/wrapper/publisher_wrapper.h @@ -20,7 +20,7 @@ class PublisherWrapper : public PublisherWrapperBase { : publisher_(node->create_publisher(topic_name, qos)) {} void publish(std::shared_ptr msg) override { - auto typed_msg = std::static_pointer_cast(msg); + const auto typed_msg = std::static_pointer_cast(msg); if (typed_msg) { publisher_->publish(*typed_msg); } else { diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index d810e4c1..b5ece193 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -230,7 +230,7 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) const mvsim::Simulable* vehPtr = &veh; const mrpt::obs::CObservation::Ptr obsCopy = obs; - auto fut = ros_publisher_workers_.enqueue( + const auto fut = ros_publisher_workers_.enqueue( [this, vehPtr, obsCopy]() { try @@ -334,8 +334,8 @@ void MVSimNode::spin() // Handle 1st iter: if (t_old_ < 0) t_old_ = realtime_tictac_.Tac(); // Compute how much time has passed to simulate in real-time: - double t_new = realtime_tictac_.Tac(); - double incr_time = realtime_factor_ * (t_new - t_old_); + const double t_new = realtime_tictac_.Tac(); + const double incr_time = realtime_factor_ * (t_new - t_old_); // Just in case the computer is *really fast*... if (incr_time < mvsim_world_->get_simul_timestep()) return; @@ -398,7 +398,7 @@ void MVSimNode::spin() // Generic teleoperation interface for any controller that // supports it: { - ControllerBaseInterface* controller = + auto* controller = it_veh->second->getControllerInterface(); ControllerBaseInterface::TeleopInput teleop_in; ControllerBaseInterface::TeleopOutput teleop_out; @@ -424,7 +424,7 @@ void MVSimNode::spin() { if (rosNow - cmdVelTimestamp > MAX_CMD_VEL_AGE_SECONDS) { - mvsim::ControllerBaseInterface* controller = + auto* controller = veh->getControllerInterface(); controller->setTwistCommand({0, 0, 0}); @@ -548,7 +548,7 @@ void MVSimNode::notifyROSWorldIsUpdated() // Create subscribers & publishers for each vehicle's stuff: // ---------------------------------------------------- - auto& vehs = mvsim_world_->getListOfVehicles(); + const auto& vehs = mvsim_world_->getListOfVehicles(); pubsub_vehicles_.clear(); pubsub_vehicles_.resize(vehs.size()); size_t idx = 0; @@ -669,7 +669,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) n_->create_publisher( vehVarName("chassis_markers", *veh), qosLatched5); #endif - const mrpt::math::TPolygon2D& poly = veh->getChassisShape(); + const auto& poly = veh->getChassisShape(); // Create one "ROS marker" for each wheel + 1 for the chassis: auto& msg_shapes = pubsubs.chassis_shape_msg; @@ -708,7 +708,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) for (size_t i = 0; i < veh->getNumWheels(); i++) { auto& wheel_shape_msg = msg_shapes.markers[1 + i]; - const mvsim::Wheel& w = veh->getWheelInfo(i); + const auto& w = veh->getWheelInfo(i); const double lx = w.diameter * 0.5, ly = w.width * 0.5; @@ -762,7 +762,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) Msg_Polygon poly_msg; // Do the first (and unique) publish: - const mrpt::math::TPolygon2D& poly = veh->getChassisShape(); + const auto& poly = veh->getChassisShape(); poly_msg.points.resize(poly.size()); for (size_t i = 0; i < poly.size(); i++) { @@ -816,7 +816,7 @@ void MVSimNode::onROSMsgCmdVel( Msg_Twist_CSPtr cmd, mvsim::VehicleBase* veh) { - mvsim::ControllerBaseInterface* controller = veh->getControllerInterface(); + auto* controller = veh->getControllerInterface(); // Update cmd_vel timestamp: lastCmdVelTimestamp_[veh] = myNowSec(); @@ -871,7 +871,7 @@ void MVSimNode::spinNotifyROS() for (auto it = vehs.begin(); it != vehs.end(); ++it, ++i) { - const mvsim::VehicleBase::Ptr& veh = it->second; + const auto& veh = it->second; auto& pubs = pubsub_vehicles_[i]; const std::string sOdomName = vehVarName("odom", *veh); @@ -952,7 +952,7 @@ void MVSimNode::spinNotifyROS() { // visualization_msgs::Marker auto& wheel_shape_msg = msg_shapes.markers[1 + j]; - const mvsim::Wheel& w = veh->getWheelInfo(j); + const auto& w = veh->getWheelInfo(j); // Set local pose of the wheel wrt the vehicle: wheel_shape_msg.pose = mrpt2ros::toROS_Pose(w.pose()); @@ -1099,7 +1099,7 @@ void MVSimNode::internalOn( const mrpt::obs::CObservation2DRangeScan& obs) { auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_); - TPubSubPerVehicle& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; + auto& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; // Create the publisher the first time an observation arrives: const bool is_1st_pub = @@ -1121,10 +1121,8 @@ void MVSimNode::internalOn( const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh); // Send TF: - mrpt::poses::CPose3D sensorPose; - obs.getSensorPose(sensorPose); - - tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose); + mrpt::poses::CPose3D sensorPose = obs.sensorPose; + auto transform = mrpt2ros::toROS_tfTransform(sensorPose); Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); @@ -1156,7 +1154,7 @@ void MVSimNode::internalOn( const mvsim::VehicleBase& veh, const mrpt::obs::CObservationIMU& obs) { auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_); - TPubSubPerVehicle& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; + auto& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; // Create the publisher the first time an observation arrives: const bool is_1st_pub = @@ -1178,10 +1176,8 @@ void MVSimNode::internalOn( const std::string sSensorFrameId = vehVarName(obs.sensorLabel, veh); // Send TF: - mrpt::poses::CPose3D sensorPose; - obs.getSensorPose(sensorPose); - - tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose); + mrpt::poses::CPose3D sensorPose = obs.sensorPose; + auto transform = mrpt2ros::toROS_tfTransform(sensorPose); Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); @@ -1214,7 +1210,7 @@ void MVSimNode::internalOn( const mvsim::VehicleBase& veh, const mrpt::obs::CObservationImage& obs) { auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_); - TPubSubPerVehicle& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; + auto& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; // Create the publisher the first time an observation arrives: const bool is_1st_pub = @@ -1238,8 +1234,7 @@ void MVSimNode::internalOn( // Send TF: mrpt::poses::CPose3D sensorPose; obs.getSensorPose(sensorPose); - - tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose); + auto transform = mrpt2ros::toROS_tfTransform(sensorPose); Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); @@ -1273,7 +1268,7 @@ void MVSimNode::internalOn( using namespace std::string_literals; auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_); - TPubSubPerVehicle& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; + auto& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; const auto lbPoints = obs.sensorLabel + "_points"s; const auto lbImage = obs.sensorLabel + "_image"s; @@ -1313,8 +1308,7 @@ void MVSimNode::internalOn( // Send TF: mrpt::poses::CPose3D sensorPose = obs.sensorPose + obs.relativePoseIntensityWRTDepth; - - tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose); + auto transform = mrpt2ros::toROS_tfTransform(sensorPose); Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); @@ -1347,8 +1341,7 @@ void MVSimNode::internalOn( { // Send TF: mrpt::poses::CPose3D sensorPose = obs.sensorPose; - - tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose); + auto transform = mrpt2ros::toROS_tfTransform(sensorPose); Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); @@ -1389,7 +1382,7 @@ void MVSimNode::internalOn( using namespace std::string_literals; auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_); - TPubSubPerVehicle& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; + auto& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; const auto lbPoints = obs.sensorLabel + "_points"s; @@ -1420,8 +1413,7 @@ void MVSimNode::internalOn( // Send TF: mrpt::poses::CPose3D sensorPose = obs.sensorPose; - - tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose); + auto transform = mrpt2ros::toROS_tfTransform(sensorPose); Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); diff --git a/mvsim_node_src/mvsim_node_main.cpp b/mvsim_node_src/mvsim_node_main.cpp index 4567dd91..96e681f5 100644 --- a/mvsim_node_src/mvsim_node_main.cpp +++ b/mvsim_node_src/mvsim_node_main.cpp @@ -87,8 +87,8 @@ int main(int argc, char** argv) r.sleep(); } #else - auto ros_clock = rclcpp::Clock::make_shared(); - auto timer_ = rclcpp::create_timer( + const auto ros_clock = rclcpp::Clock::make_shared(); + const auto timer_ = rclcpp::create_timer( n, ros_clock, std::chrono::microseconds(periodMs), [&node]() { From 9a29dda16bb00472bbcc0ebec72acf4b45757e87 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Thu, 8 Aug 2024 14:12:39 +0900 Subject: [PATCH 15/16] clang-format-14 --- .../include/mvsim/mvsim_node_core.h | 115 ++++++------ .../include/wrapper/publisher_wrapper.h | 54 +++--- mvsim_node_src/mvsim_node.cpp | 166 ++++++++++-------- mvsim_node_src/mvsim_node_main.cpp | 7 +- 4 files changed, 183 insertions(+), 159 deletions(-) diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h index a96e9e72..3d8e93cf 100644 --- a/mvsim_node_src/include/mvsim/mvsim_node_core.h +++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h @@ -9,9 +9,6 @@ #pragma once -#include -#include - #include #include #include @@ -25,20 +22,21 @@ #include #include -#if PACKAGE_ROS_VERSION == 1 -#include -#include +#include +#include +#if PACKAGE_ROS_VERSION == 1 #include -#include - #include #include #include #include +#include +#include #include #include -#include +#include +#include #include #include #include @@ -59,17 +57,16 @@ using Msg_Bool = std_msgs::Bool; using Msg_TFMessage = tf2_msgs::TFMessage; using Msg_MarkerArray = visualization_msgs::MarkerArray; #else -#include -#include -#include - #include #include #include #include +#include #include #include -#include +#include +#include +#include #include #include @@ -81,7 +78,8 @@ using ros_Duration = rclcpp::Duration; using Msg_Polygon = geometry_msgs::msg::Polygon; using Msg_PoseArray = geometry_msgs::msg::PoseArray; -using Msg_PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; +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; @@ -92,25 +90,28 @@ using Msg_TFMessage = tf2_msgs::msg::TFMessage; using Msg_MarkerArray = visualization_msgs::msg::MarkerArray; #endif -namespace mvsim_node { +namespace mvsim_node +{ #if PACKAGE_ROS_VERSION == 1 - template - boost::shared_ptr make_shared(Args&&... args) { - return boost::make_shared(std::forward(args)...); - } +template +boost::shared_ptr make_shared(Args&&... args) +{ + return boost::make_shared(std::forward(args)...); +} - template - using shared_ptr = boost::shared_ptr; +template +using shared_ptr = boost::shared_ptr; #else - template - std::shared_ptr make_shared(Args&&... args) { - return std::make_shared(std::forward(args)...); - } +template +std::shared_ptr make_shared(Args&&... args) +{ + return std::make_shared(std::forward(args)...); +} - template - using shared_ptr = std::shared_ptr; +template +using shared_ptr = std::shared_ptr; #endif -} +} // namespace mvsim_node /** A class to wrap libmvsim as a ROS node */ @@ -198,23 +199,33 @@ class MVSimNode struct TPubSubPerVehicle { #if PACKAGE_ROS_VERSION == 1 - mvsim_node::shared_ptr sub_cmd_vel; //!< Subscribers vehicle's "cmd_vel" topic - mvsim_node::shared_ptr pub_odom; //!< Publisher of "odom" topic - mvsim_node::shared_ptr pub_ground_truth; //!< "base_pose_ground_truth" topic + mvsim_node::shared_ptr + sub_cmd_vel; //!< Subscribers vehicle's "cmd_vel" topic + mvsim_node::shared_ptr + pub_odom; //!< Publisher of "odom" topic + mvsim_node::shared_ptr + pub_ground_truth; //!< "base_pose_ground_truth" topic /// "fake_localization" pubs: - mvsim_node::shared_ptr pub_amcl_pose; //!< Publisher of "amcl_pose" topic - mvsim_node::shared_ptr pub_particlecloud; //!< Publisher of "particlecloud" topic + mvsim_node::shared_ptr + pub_amcl_pose; //!< Publisher of "amcl_pose" topic + mvsim_node::shared_ptr + pub_particlecloud; //!< Publisher of "particlecloud" topic /// Map => publisher - std::map> pub_sensors; + std::map> + pub_sensors; - mvsim_node::shared_ptr pub_chassis_markers; //!< "/chassis_markers" - mvsim_node::shared_ptr pub_chassis_shape; //!< "/chassis_shape" - mvsim_node::shared_ptr pub_collision; //!< "/collision" + mvsim_node::shared_ptr + pub_chassis_markers; //!< "/chassis_markers" + mvsim_node::shared_ptr + pub_chassis_shape; //!< "/chassis_shape" + mvsim_node::shared_ptr + pub_collision; //!< "/collision" - mvsim_node::shared_ptr pub_tf; //!< "/tf" - mvsim_node::shared_ptr pub_tf_static; //!< "/tf_static" + mvsim_node::shared_ptr pub_tf; //!< "/tf" + mvsim_node::shared_ptr + pub_tf_static; //!< "/tf_static" Msg_MarkerArray chassis_shape_msg; #else @@ -226,20 +237,18 @@ class MVSimNode rclcpp::Publisher::SharedPtr pub_ground_truth; /// "fake_localization" pubs: - rclcpp::Publisher:: - SharedPtr pub_amcl_pose; - rclcpp::Publisher::SharedPtr - pub_particlecloud; + rclcpp::Publisher::SharedPtr + pub_amcl_pose; + rclcpp::Publisher::SharedPtr pub_particlecloud; /// Map => publisher - std::map> pub_sensors; + std::map> + pub_sensors; /// "/chassis_markers" - rclcpp::Publisher::SharedPtr - pub_chassis_markers; + rclcpp::Publisher::SharedPtr pub_chassis_markers; /// "/chassis_shape" - rclcpp::Publisher::SharedPtr - pub_chassis_shape; + rclcpp::Publisher::SharedPtr pub_chassis_shape; /// "/collision" rclcpp::Publisher::SharedPtr pub_collision; @@ -263,9 +272,7 @@ class MVSimNode // === End ROS Publishers ==== // === ROS Hooks ==== - void onROSMsgCmdVel( - Msg_Twist_CSPtr cmd, - mvsim::VehicleBase* veh); + void onROSMsgCmdVel(Msg_Twist_CSPtr cmd, mvsim::VehicleBase* veh); // === End ROS Hooks==== #if PACKAGE_ROS_VERSION == 1 @@ -328,9 +335,7 @@ class MVSimNode void sendStaticTF( const std::string& frame_id, const std::string& child_frame_id, - const tf2::Transform& tx, - const ros_Time& stamp - ); + const tf2::Transform& tx, const ros_Time& stamp); ros_Time myNow() const; double myNowSec() const; diff --git a/mvsim_node_src/include/wrapper/publisher_wrapper.h b/mvsim_node_src/include/wrapper/publisher_wrapper.h index c7471f74..fa850da6 100644 --- a/mvsim_node_src/include/wrapper/publisher_wrapper.h +++ b/mvsim_node_src/include/wrapper/publisher_wrapper.h @@ -5,31 +5,41 @@ #include "rclcpp/rclcpp.hpp" -class PublisherWrapperBase { - public: - PublisherWrapperBase() = default; - virtual ~PublisherWrapperBase() = default; - virtual void publish(std::shared_ptr message) = 0; +class PublisherWrapperBase +{ + public: + PublisherWrapperBase() = default; + virtual ~PublisherWrapperBase() = default; + virtual void publish(std::shared_ptr message) = 0; }; - template -class PublisherWrapper : public PublisherWrapperBase { - public: - PublisherWrapper(rclcpp::Node::SharedPtr node, const std::string& topic_name, size_t qos) - : publisher_(node->create_publisher(topic_name, qos)) {} - - void publish(std::shared_ptr msg) override { - const auto typed_msg = std::static_pointer_cast(msg); - if (typed_msg) { - publisher_->publish(*typed_msg); - } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to cast message to correct type."); - } - } - - private: - typename rclcpp::Publisher::SharedPtr publisher_; +class PublisherWrapper : public PublisherWrapperBase +{ + public: + PublisherWrapper( + rclcpp::Node::SharedPtr node, const std::string& topic_name, size_t qos) + : publisher_(node->create_publisher(topic_name, qos)) + { + } + + void publish(std::shared_ptr msg) override + { + const auto typed_msg = std::static_pointer_cast(msg); + if (typed_msg) + { + publisher_->publish(*typed_msg); + } + else + { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to cast message to correct type."); + } + } + + private: + typename rclcpp::Publisher::SharedPtr publisher_; }; #endif diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index b5ece193..1cff1a80 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -34,7 +34,6 @@ #include #include #include - #include #include #include @@ -107,8 +106,10 @@ namespace mrpt2ros = mrpt::ros2bridge; #define ROS12_ERROR(...) ROS_ERROR(__VA_ARGS__) #else #define ROS12_INFO(...) RCLCPP_INFO(n_->get_logger(), __VA_ARGS__) -#define ROS12_WARN_THROTTLE(...) RCLCPP_WARN_THROTTLE(n_->get_logger(), *clock_, __VA_ARGS__) -#define ROS12_WARN_STREAM_THROTTLE(...) RCLCPP_WARN_STREAM_THROTTLE(n_->get_logger(), *clock_, __VA_ARGS__) +#define ROS12_WARN_THROTTLE(...) \ + RCLCPP_WARN_THROTTLE(n_->get_logger(), *clock_, __VA_ARGS__) +#define ROS12_WARN_STREAM_THROTTLE(...) \ + RCLCPP_WARN_STREAM_THROTTLE(n_->get_logger(), *clock_, __VA_ARGS__) #define ROS12_ERROR(...) RCLCPP_ERROR(n_->get_logger(), __VA_ARGS__) #endif @@ -193,21 +194,25 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) // Init ROS publishers: #if PACKAGE_ROS_VERSION == 1 - // pub_clock_ = mvsim_node::make_shared(n_.advertise("/clock", 10)); - - pub_map_ros_ = mvsim_node::make_shared(n_.advertise( - "simul_map", 1 /*queue len*/, true /*latch*/)); - pub_map_metadata_ = mvsim_node::make_shared(n_.advertise( - "simul_map_metadata", 1 /*queue len*/, true /*latch*/)); + // pub_clock_ = + // mvsim_node::make_shared(n_.advertise("/clock", + // 10)); + + pub_map_ros_ = + mvsim_node::make_shared(n_.advertise( + "simul_map", 1 /*queue len*/, true /*latch*/)); + pub_map_metadata_ = + mvsim_node::make_shared(n_.advertise( + "simul_map_metadata", 1 /*queue len*/, true /*latch*/)); #else rclcpp::QoS qosLatched(rclcpp::KeepLast(10)); qosLatched.durability( rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - pub_map_ros_ = n_->create_publisher( - "simul_map", qosLatched); - pub_map_metadata_ = n_->create_publisher( - "simul_map_metadata", qosLatched); + pub_map_ros_ = + n_->create_publisher("simul_map", qosLatched); + pub_map_metadata_ = + n_->create_publisher("simul_map_metadata", qosLatched); #endif #if PACKAGE_ROS_VERSION == 1 @@ -398,8 +403,7 @@ void MVSimNode::spin() // Generic teleoperation interface for any controller that // supports it: { - auto* controller = - it_veh->second->getControllerInterface(); + auto* controller = it_veh->second->getControllerInterface(); ControllerBaseInterface::TeleopInput teleop_in; ControllerBaseInterface::TeleopOutput teleop_out; teleop_in.keycode = keyevent.keycode; @@ -417,15 +421,14 @@ void MVSimNode::spin() } // end refresh teleop stuff -// Check cmd_vel timeout: + // Check cmd_vel timeout: const double rosNow = myNowSec(); std::set toRemove; for (const auto& [veh, cmdVelTimestamp] : lastCmdVelTimestamp_) { if (rosNow - cmdVelTimestamp > MAX_CMD_VEL_AGE_SECONDS) { - auto* controller = - veh->getControllerInterface(); + auto* controller = veh->getControllerInterface(); controller->setTwistCommand({0, 0, 0}); toRemove.insert(veh); @@ -565,7 +568,8 @@ void MVSimNode::notifyROSWorldIsUpdated() // sendStaticTF("world", "map", tfIdentity_, myNow()); } -ros_Time MVSimNode::myNow() const { +ros_Time MVSimNode::myNow() const +{ #if PACKAGE_ROS_VERSION == 1 return ros::Time::now(); #else @@ -573,7 +577,8 @@ ros_Time MVSimNode::myNow() const { #endif } -double MVSimNode::myNowSec() const { +double MVSimNode::myNowSec() const +{ #if PACKAGE_ROS_VERSION == 1 return ros::Time::now().toSec(); #else @@ -583,9 +588,7 @@ double MVSimNode::myNowSec() const { void MVSimNode::sendStaticTF( const std::string& frame_id, const std::string& child_frame_id, - const tf2::Transform& txf, - const ros_Time& stamp -) + const tf2::Transform& txf, const ros_Time& stamp) { Msg_TransformStamped tx; tx.header.frame_id = frame_id; @@ -601,10 +604,11 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) { // sub: /cmd_vel #if PACKAGE_ROS_VERSION == 1 - pubsubs.sub_cmd_vel = mvsim_node::make_shared(n_.subscribe( - vehVarName("cmd_vel", *veh), 10, - [this, veh](Msg_Twist_CSPtr msg) - { return this->onROSMsgCmdVel(msg, veh); })); + pubsubs.sub_cmd_vel = + mvsim_node::make_shared(n_.subscribe( + vehVarName("cmd_vel", *veh), 10, + [this, veh](Msg_Twist_CSPtr msg) + { return this->onROSMsgCmdVel(msg, veh); })); #else pubsubs.sub_cmd_vel = n_->create_subscription( vehVarName("cmd_vel", *veh), 10, @@ -614,22 +618,28 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #if PACKAGE_ROS_VERSION == 1 // pub: /odom - pubsubs.pub_odom = mvsim_node::make_shared(n_.advertise( - vehVarName("odom", *veh), publisher_history_len_)); + pubsubs.pub_odom = + mvsim_node::make_shared(n_.advertise( + vehVarName("odom", *veh), publisher_history_len_)); // pub: /base_pose_ground_truth - pubsubs.pub_ground_truth = mvsim_node::make_shared(n_.advertise( - vehVarName("base_pose_ground_truth", *veh), publisher_history_len_)); + pubsubs.pub_ground_truth = + mvsim_node::make_shared(n_.advertise( + vehVarName("base_pose_ground_truth", *veh), + publisher_history_len_)); // pub: /collision - pubsubs.pub_collision = mvsim_node::make_shared(n_.advertise( - vehVarName("collision", *veh), publisher_history_len_)); + pubsubs.pub_collision = + mvsim_node::make_shared(n_.advertise( + vehVarName("collision", *veh), publisher_history_len_)); // pub: /tf, /tf_static - pubsubs.pub_tf = mvsim_node::make_shared(n_.advertise( - vehVarName("tf", *veh), publisher_history_len_)); - pubsubs.pub_tf_static = mvsim_node::make_shared(n_.advertise( - vehVarName("tf_static", *veh), publisher_history_len_)); + pubsubs.pub_tf = + mvsim_node::make_shared(n_.advertise( + vehVarName("tf", *veh), publisher_history_len_)); + pubsubs.pub_tf_static = + mvsim_node::make_shared(n_.advertise( + vehVarName("tf_static", *veh), publisher_history_len_)); #else // pub: /odom pubsubs.pub_odom = n_->create_publisher( @@ -657,17 +667,16 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) // pub: /chassis_markers { #if PACKAGE_ROS_VERSION == 1 - pubsubs.pub_chassis_markers = - mvsim_node::make_shared(n_.advertise( + pubsubs.pub_chassis_markers = mvsim_node::make_shared( + n_.advertise( vehVarName("chassis_markers", *veh), 5, true /*latch*/)); #else rclcpp::QoS qosLatched5(rclcpp::KeepLast(5)); qosLatched5.durability(rmw_qos_durability_policy_t:: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - pubsubs.pub_chassis_markers = - n_->create_publisher( - vehVarName("chassis_markers", *veh), qosLatched5); + pubsubs.pub_chassis_markers = n_->create_publisher( + vehVarName("chassis_markers", *veh), qosLatched5); #endif const auto& poly = veh->getChassisShape(); @@ -748,16 +757,16 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) // pub: /chassis_polygon { #if PACKAGE_ROS_VERSION == 1 - pubsubs.pub_chassis_shape = mvsim_node::make_shared(n_.advertise( - vehVarName("chassis_polygon", *veh), 1, true /*latch*/)); + pubsubs.pub_chassis_shape = + mvsim_node::make_shared(n_.advertise( + vehVarName("chassis_polygon", *veh), 1, true /*latch*/)); #else rclcpp::QoS qosLatched1(rclcpp::KeepLast(1)); qosLatched1.durability(rmw_qos_durability_policy_t:: RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - pubsubs.pub_chassis_shape = - n_->create_publisher( - vehVarName("chassis_polygon", *veh), qosLatched1); + pubsubs.pub_chassis_shape = n_->create_publisher( + vehVarName("chassis_polygon", *veh), qosLatched1); #endif Msg_Polygon poly_msg; @@ -777,21 +786,20 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) { #if PACKAGE_ROS_VERSION == 1 // pub: /amcl_pose - pubsubs.pub_amcl_pose = - mvsim_node::make_shared(n_.advertise( + pubsubs.pub_amcl_pose = mvsim_node::make_shared( + n_.advertise( vehVarName("amcl_pose", *veh), 1)); // pub: /particlecloud - pubsubs.pub_particlecloud = mvsim_node::make_shared(n_.advertise( - vehVarName("particlecloud", *veh), 1)); + pubsubs.pub_particlecloud = mvsim_node::make_shared( + n_.advertise(vehVarName("particlecloud", *veh), 1)); #else // pub: /amcl_pose pubsubs.pub_amcl_pose = n_->create_publisher( vehVarName("amcl_pose", *veh), 1); // pub: /particlecloud - pubsubs.pub_particlecloud = - n_->create_publisher( - vehVarName("particlecloud", *veh), 1); + pubsubs.pub_particlecloud = n_->create_publisher( + vehVarName("particlecloud", *veh), 1); #endif } @@ -812,13 +820,11 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) pubsubs.pub_tf_static->publish(tfMsg); } -void MVSimNode::onROSMsgCmdVel( - Msg_Twist_CSPtr cmd, - mvsim::VehicleBase* veh) +void MVSimNode::onROSMsgCmdVel(Msg_Twist_CSPtr cmd, mvsim::VehicleBase* veh) { auto* controller = veh->getControllerInterface(); -// Update cmd_vel timestamp: + // Update cmd_vel timestamp: lastCmdVelTimestamp_[veh] = myNowSec(); const bool ctrlAcceptTwist = controller->setTwistCommand( @@ -1109,11 +1115,12 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = mvsim_node::make_shared(n_.advertise( - vehVarName(obs.sensorLabel, veh), publisher_history_len_)); + pub = + mvsim_node::make_shared(n_.advertise( + vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else - pub = mvsim_node::make_shared>(n_, - vehVarName(obs.sensorLabel, veh), publisher_history_len_); + pub = mvsim_node::make_shared>( + n_, vehVarName(obs.sensorLabel, veh), publisher_history_len_); #endif } lck.unlock(); @@ -1167,8 +1174,8 @@ void MVSimNode::internalOn( pub = mvsim_node::make_shared(n_.advertise( vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else - pub = mvsim_node::make_shared>(n_, - vehVarName(obs.sensorLabel, veh), publisher_history_len_); + pub = mvsim_node::make_shared>( + n_, vehVarName(obs.sensorLabel, veh), publisher_history_len_); #endif } lck.unlock(); @@ -1223,8 +1230,8 @@ void MVSimNode::internalOn( pub = mvsim_node::make_shared(n_.advertise( vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else - pub = mvsim_node::make_shared>(n_, - vehVarName(obs.sensorLabel, veh), publisher_history_len_); + pub = mvsim_node::make_shared>( + n_, vehVarName(obs.sensorLabel, veh), publisher_history_len_); #endif } lck.unlock(); @@ -1283,15 +1290,17 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pubImg = mvsim_node::make_shared(n_.advertise( - vehVarName(lbImage, veh), publisher_history_len_)); - pubPts = mvsim_node::make_shared(n_.advertise( - vehVarName(lbPoints, veh), publisher_history_len_)); + pubImg = + mvsim_node::make_shared(n_.advertise( + vehVarName(lbImage, veh), publisher_history_len_)); + pubPts = mvsim_node::make_shared( + n_.advertise( + vehVarName(lbPoints, veh), publisher_history_len_)); #else - pubImg = mvsim_node::make_shared>(n_, - vehVarName(lbImage, veh), publisher_history_len_); - pubPts = mvsim_node::make_shared>(n_, - vehVarName(lbPoints, veh), publisher_history_len_); + pubImg = mvsim_node::make_shared>( + n_, vehVarName(lbImage, veh), publisher_history_len_); + pubPts = mvsim_node::make_shared>( + n_, vehVarName(lbPoints, veh), publisher_history_len_); #endif } lck.unlock(); @@ -1395,11 +1404,12 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pubPts = mvsim_node::make_shared(n_.advertise( - vehVarName(lbPoints, veh), publisher_history_len_)); + pubPts = mvsim_node::make_shared( + n_.advertise( + vehVarName(lbPoints, veh), publisher_history_len_)); #else - pubPts = mvsim_node::make_shared>(n_, - vehVarName(lbPoints, veh), publisher_history_len_); + pubPts = mvsim_node::make_shared>( + n_, vehVarName(lbPoints, veh), publisher_history_len_); #endif } lck.unlock(); diff --git a/mvsim_node_src/mvsim_node_main.cpp b/mvsim_node_src/mvsim_node_main.cpp index 96e681f5..ae12a36e 100644 --- a/mvsim_node_src/mvsim_node_main.cpp +++ b/mvsim_node_src/mvsim_node_main.cpp @@ -30,7 +30,8 @@ int main(int argc, char** argv) try { // Create a "Node" object. - mvsim_node::shared_ptr node = mvsim_node::make_shared(n); + mvsim_node::shared_ptr node = + mvsim_node::make_shared(n); // Declare variables that can be modified by launch file or command // line. @@ -70,9 +71,7 @@ int main(int argc, char** argv) dynamic_reconfigure::Server dr_srv; dr_srv.setCallback( [&node](mvsim::mvsimNodeConfig& config, uint32_t level) - { - return node->configCallback(config, level); - }); + { return node->configCallback(config, level); }); #endif // Tell ROS how fast to run this node-> From 12646e1ff8cf1f04ed0ac2ef2ac462157e464392 Mon Sep 17 00:00:00 2001 From: finani <10ezenfinani@gmail.com> Date: Fri, 9 Aug 2024 10:30:27 +0900 Subject: [PATCH 16/16] Change ColumnLimit to 100 (.clang-format) --- .clang-format | 2 +- formatter.sh | 2 + .../include/mvsim/mvsim_node_core.h | 61 ++-- .../include/wrapper/publisher_wrapper.h | 7 +- mvsim_node_src/mvsim_node.cpp | 330 +++++++----------- mvsim_node_src/mvsim_node_main.cpp | 18 +- 6 files changed, 156 insertions(+), 264 deletions(-) create mode 100755 formatter.sh diff --git a/.clang-format b/.clang-format index 847445c4..41ae2bbb 100644 --- a/.clang-format +++ b/.clang-format @@ -55,7 +55,7 @@ AlignTrailingComments: false # Should be off, causes many dummy problems!! BreakBeforeBraces: Allman #BreakBeforeTernaryOperators: true #BreakConstructorInitializersBeforeComma: false -ColumnLimit: 80 +ColumnLimit: 100 #CommentPragmas: '' #ConstructorInitializerAllOnOneLineOrOnePerLine: true #ConstructorInitializerIndentWidth: 4 diff --git a/formatter.sh b/formatter.sh new file mode 100755 index 00000000..06a0718b --- /dev/null +++ b/formatter.sh @@ -0,0 +1,2 @@ +# formatter.sh +find mvsim_node_src/ -iname *.h -o -iname *.hpp -o -iname *.cpp -o -iname *.c | xargs clang-format-14 -i diff --git a/mvsim_node_src/include/mvsim/mvsim_node_core.h b/mvsim_node_src/include/mvsim/mvsim_node_core.h index 3d8e93cf..a244a53e 100644 --- a/mvsim_node_src/include/mvsim/mvsim_node_core.h +++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h @@ -78,8 +78,7 @@ using ros_Duration = rclcpp::Duration; using Msg_Polygon = geometry_msgs::msg::Polygon; using Msg_PoseArray = geometry_msgs::msg::PoseArray; -using Msg_PoseWithCovarianceStamped = - geometry_msgs::msg::PoseWithCovarianceStamped; +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; @@ -141,13 +140,11 @@ class MVSimNode void configCallback(mvsim::mvsimNodeConfig& config, uint32_t level); #endif - void onNewObservation( - const mvsim::Simulable& veh, const mrpt::obs::CObservation::Ptr& obs); + void onNewObservation(const mvsim::Simulable& veh, const mrpt::obs::CObservation::Ptr& obs); /// The mvsim library simulated world (includes everything: vehicles, /// obstacles, etc.) - mvsim_node::shared_ptr mvsim_world_ = - mvsim_node::make_shared(); + mvsim_node::shared_ptr mvsim_world_ = mvsim_node::make_shared(); mrpt::WorkerThreadsPool ros_publisher_workers_{ 4 /*threads*/, mrpt::WorkerThreadsPool::POLICY_FIFO}; @@ -201,31 +198,24 @@ class MVSimNode #if PACKAGE_ROS_VERSION == 1 mvsim_node::shared_ptr sub_cmd_vel; //!< Subscribers vehicle's "cmd_vel" topic - mvsim_node::shared_ptr - pub_odom; //!< Publisher of "odom" topic + mvsim_node::shared_ptr pub_odom; //!< Publisher of "odom" topic mvsim_node::shared_ptr pub_ground_truth; //!< "base_pose_ground_truth" topic /// "fake_localization" pubs: - mvsim_node::shared_ptr - pub_amcl_pose; //!< Publisher of "amcl_pose" topic + mvsim_node::shared_ptr pub_amcl_pose; //!< Publisher of "amcl_pose" topic mvsim_node::shared_ptr pub_particlecloud; //!< Publisher of "particlecloud" topic /// Map => publisher - std::map> - pub_sensors; + std::map> pub_sensors; - mvsim_node::shared_ptr - pub_chassis_markers; //!< "/chassis_markers" - mvsim_node::shared_ptr - pub_chassis_shape; //!< "/chassis_shape" - mvsim_node::shared_ptr - pub_collision; //!< "/collision" + mvsim_node::shared_ptr pub_chassis_markers; //!< "/chassis_markers" + mvsim_node::shared_ptr pub_chassis_shape; //!< "/chassis_shape" + mvsim_node::shared_ptr pub_collision; //!< "/collision" mvsim_node::shared_ptr pub_tf; //!< "/tf" - mvsim_node::shared_ptr - pub_tf_static; //!< "/tf_static" + mvsim_node::shared_ptr pub_tf_static; //!< "/tf_static" Msg_MarkerArray chassis_shape_msg; #else @@ -237,13 +227,11 @@ class MVSimNode rclcpp::Publisher::SharedPtr pub_ground_truth; /// "fake_localization" pubs: - rclcpp::Publisher::SharedPtr - pub_amcl_pose; + rclcpp::Publisher::SharedPtr pub_amcl_pose; rclcpp::Publisher::SharedPtr pub_particlecloud; /// Map => publisher - std::map> - pub_sensors; + std::map> pub_sensors; /// "/chassis_markers" rclcpp::Publisher::SharedPtr pub_chassis_markers; @@ -330,12 +318,11 @@ class MVSimNode /** Creates the string "//" if there're more than * one vehicle in the World, or "/" otherwise. */ - std::string vehVarName( - const std::string& sVarName, const mvsim::VehicleBase& veh) const; + std::string vehVarName(const std::string& sVarName, const mvsim::VehicleBase& veh) const; void sendStaticTF( - const std::string& frame_id, const std::string& child_frame_id, - const tf2::Transform& tx, const ros_Time& stamp); + const std::string& frame_id, const std::string& child_frame_id, const tf2::Transform& tx, + const ros_Time& stamp); ros_Time myNow() const; double myNowSec() const; @@ -345,18 +332,10 @@ class MVSimNode void publishWorldElements(mvsim::WorldElementBase& obj); void publishVehicles(mvsim::VehicleBase& veh); - void internalOn( - const mvsim::VehicleBase& veh, - const mrpt::obs::CObservation2DRangeScan& obs); - void internalOn( - const mvsim::VehicleBase& veh, const mrpt::obs::CObservationImage& obs); - void internalOn( - const mvsim::VehicleBase& veh, - const mrpt::obs::CObservation3DRangeScan& obs); - void internalOn( - const mvsim::VehicleBase& veh, - const mrpt::obs::CObservationPointCloud& obs); - void internalOn( - const mvsim::VehicleBase& veh, const mrpt::obs::CObservationIMU& obs); + void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservation2DRangeScan& obs); + void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservationImage& obs); + void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservation3DRangeScan& obs); + void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservationPointCloud& obs); + void internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservationIMU& obs); }; // end class diff --git a/mvsim_node_src/include/wrapper/publisher_wrapper.h b/mvsim_node_src/include/wrapper/publisher_wrapper.h index fa850da6..012c90ed 100644 --- a/mvsim_node_src/include/wrapper/publisher_wrapper.h +++ b/mvsim_node_src/include/wrapper/publisher_wrapper.h @@ -17,8 +17,7 @@ template class PublisherWrapper : public PublisherWrapperBase { public: - PublisherWrapper( - rclcpp::Node::SharedPtr node, const std::string& topic_name, size_t qos) + PublisherWrapper(rclcpp::Node::SharedPtr node, const std::string& topic_name, size_t qos) : publisher_(node->create_publisher(topic_name, qos)) { } @@ -32,9 +31,7 @@ class PublisherWrapper : public PublisherWrapperBase } else { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "Failed to cast message to correct type."); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to cast message to correct type."); } } diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index 1cff1a80..08e0f2ee 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -106,8 +106,7 @@ namespace mrpt2ros = mrpt::ros2bridge; #define ROS12_ERROR(...) ROS_ERROR(__VA_ARGS__) #else #define ROS12_INFO(...) RCLCPP_INFO(n_->get_logger(), __VA_ARGS__) -#define ROS12_WARN_THROTTLE(...) \ - RCLCPP_WARN_THROTTLE(n_->get_logger(), *clock_, __VA_ARGS__) +#define ROS12_WARN_THROTTLE(...) RCLCPP_WARN_THROTTLE(n_->get_logger(), *clock_, __VA_ARGS__) #define ROS12_WARN_STREAM_THROTTLE(...) \ RCLCPP_WARN_STREAM_THROTTLE(n_->get_logger(), *clock_, __VA_ARGS__) #define ROS12_ERROR(...) RCLCPP_ERROR(n_->get_logger(), __VA_ARGS__) @@ -132,21 +131,17 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) if (!localn_.getParam("base_watchdog_timeout", t)) t = 0.2; base_watchdog_timeout_.fromSec(t); localn_.param("realtime_factor", realtime_factor_, 1.0); - localn_.param( - "gui_refresh_period", gui_refresh_period_ms_, gui_refresh_period_ms_); + localn_.param("gui_refresh_period", gui_refresh_period_ms_, gui_refresh_period_ms_); localn_.param("headless", headless_, headless_); - localn_.param( - "period_ms_publish_tf", period_ms_publish_tf_, period_ms_publish_tf_); - localn_.param( - "do_fake_localization", do_fake_localization_, do_fake_localization_); + localn_.param("period_ms_publish_tf", period_ms_publish_tf_, period_ms_publish_tf_); + localn_.param("do_fake_localization", do_fake_localization_, do_fake_localization_); // JLBC: At present, mvsim does not use sim_time for neither ROS 1 nor // ROS 2. // n_.setParam("use_sim_time", false); if (true == localn_.param("use_sim_time", false)) { - THROW_EXCEPTION( - "At present, MVSIM can only work with use_sim_time=false"); + THROW_EXCEPTION("At present, MVSIM can only work with use_sim_time=false"); } #else clock_ = n_->get_clock(); @@ -158,39 +153,36 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) n_->declare_parameter("base_watchdog_timeout", 0.2); { double t; - base_watchdog_timeout_ = std::chrono::milliseconds( - 1000 * n_->get_parameter_or("base_watchdog_timeout", t, 0.2)); + base_watchdog_timeout_ = + std::chrono::milliseconds(1000 * n_->get_parameter_or("base_watchdog_timeout", t, 0.2)); } - realtime_factor_ = - n_->declare_parameter("realtime_factor", realtime_factor_); + realtime_factor_ = n_->declare_parameter("realtime_factor", realtime_factor_); - gui_refresh_period_ms_ = n_->declare_parameter( - "gui_refresh_period", gui_refresh_period_ms_); + gui_refresh_period_ms_ = + n_->declare_parameter("gui_refresh_period", gui_refresh_period_ms_); headless_ = n_->declare_parameter("headless", headless_); - period_ms_publish_tf_ = n_->declare_parameter( - "period_ms_publish_tf", period_ms_publish_tf_); + period_ms_publish_tf_ = + n_->declare_parameter("period_ms_publish_tf", period_ms_publish_tf_); - do_fake_localization_ = n_->declare_parameter( - "do_fake_localization", do_fake_localization_); + do_fake_localization_ = + n_->declare_parameter("do_fake_localization", do_fake_localization_); - publisher_history_len_ = n_->declare_parameter( - "publisher_history_len", publisher_history_len_); + publisher_history_len_ = + n_->declare_parameter("publisher_history_len", publisher_history_len_); // n_->declare_parameter("use_sim_time"); // already declared error? if (true == n_->get_parameter_or("use_sim_time", false)) { - THROW_EXCEPTION( - "At present, MVSIM can only work with use_sim_time=false"); + THROW_EXCEPTION("At present, MVSIM can only work with use_sim_time=false"); } #endif // Launch GUI thread: thread_params_.obj = this; - thGUI_ = - std::thread(&MVSimNode::thread_update_GUI, std::ref(thread_params_)); + thGUI_ = std::thread(&MVSimNode::thread_update_GUI, std::ref(thread_params_)); // Init ROS publishers: #if PACKAGE_ROS_VERSION == 1 @@ -198,21 +190,16 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) // mvsim_node::make_shared(n_.advertise("/clock", // 10)); - pub_map_ros_ = - mvsim_node::make_shared(n_.advertise( - "simul_map", 1 /*queue len*/, true /*latch*/)); - pub_map_metadata_ = - mvsim_node::make_shared(n_.advertise( - "simul_map_metadata", 1 /*queue len*/, true /*latch*/)); + pub_map_ros_ = mvsim_node::make_shared( + n_.advertise("simul_map", 1 /*queue len*/, true /*latch*/)); + pub_map_metadata_ = mvsim_node::make_shared( + n_.advertise("simul_map_metadata", 1 /*queue len*/, true /*latch*/)); #else rclcpp::QoS qosLatched(rclcpp::KeepLast(10)); - qosLatched.durability( - rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + qosLatched.durability(rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - pub_map_ros_ = - n_->create_publisher("simul_map", qosLatched); - pub_map_metadata_ = - n_->create_publisher("simul_map_metadata", qosLatched); + pub_map_ros_ = n_->create_publisher("simul_map", qosLatched); + pub_map_metadata_ = n_->create_publisher("simul_map_metadata", qosLatched); #endif #if PACKAGE_ROS_VERSION == 1 @@ -224,14 +211,11 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) #endif mvsim_world_->registerCallbackOnObservation( - [this]( - const mvsim::Simulable& veh, - const mrpt::obs::CObservation::Ptr& obs) + [this](const mvsim::Simulable& veh, const mrpt::obs::CObservation::Ptr& obs) { if (!obs) return; - mrpt::system::CTimeLoggerEntry tle( - profiler_, "lambda_onNewObservation"); + mrpt::system::CTimeLoggerEntry tle(profiler_, "lambda_onNewObservation"); const mvsim::Simulable* vehPtr = &veh; const mrpt::obs::CObservation::Ptr obsCopy = obs; @@ -248,9 +232,7 @@ MVSimNode::MVSimNode(rclcpp::Node::SharedPtr& n) "[MVSimNode] Error processing observation with " "label " "'%s':\n%s", - obsCopy ? obsCopy->sensorLabel.c_str() - : "(nullptr)", - e.what()); + obsCopy ? obsCopy->sensorLabel.c_str() : "(nullptr)", e.what()); } }); }); @@ -313,16 +295,14 @@ void MVSimNode::terminateSimulation() * configCallback() * Callback function for dynamic reconfigure server. *----------------------------------------------------------------------------*/ -void MVSimNode::configCallback( - mvsim::mvsimNodeConfig& config, [[maybe_unused]] uint32_t level) +void MVSimNode::configCallback(mvsim::mvsimNodeConfig& config, [[maybe_unused]] uint32_t level) { // Set class variables to new values. They should match what is input at the // dynamic reconfigure GUI. // message = config.message.c_str(); ROS12_INFO("MVSimNode::configCallback() called."); - if (mvsim_world_->is_GUI_open() && !config.show_gui) - mvsim_world_->close_GUI(); + if (mvsim_world_->is_GUI_open() && !config.show_gui) mvsim_world_->close_GUI(); } #endif @@ -382,8 +362,7 @@ void MVSimNode::spin() { // Test: Differential drive: Control raw forces txt2gui_tmp += mrpt::format( - "Selected vehicle: %u/%u\n", - static_cast(teleop_idx_veh_ + 1), + "Selected vehicle: %u/%u\n", static_cast(teleop_idx_veh_ + 1), static_cast(vehs.size())); if (vehs.size() > teleop_idx_veh_) { @@ -392,13 +371,11 @@ void MVSimNode::spin() std::advance(it_veh, teleop_idx_veh_); // Get speed: ground truth - txt2gui_tmp += "gt. vel: "s + - it_veh->second->getVelocityLocal().asString(); + txt2gui_tmp += "gt. vel: "s + it_veh->second->getVelocityLocal().asString(); // Get speed: ground truth txt2gui_tmp += - "\nodo vel: "s + - it_veh->second->getVelocityLocalOdoEstimate().asString(); + "\nodo vel: "s + it_veh->second->getVelocityLocalOdoEstimate().asString(); // Generic teleoperation interface for any controller that // supports it: @@ -459,24 +436,20 @@ void MVSimNode::thread_update_GUI(TThreadParams& thread_params) obj->mvsim_world_->update_GUI(&guiparams); // Send key-strokes to the main thread: - if (guiparams.keyevent.keycode != 0) - obj->gui_key_events_ = guiparams.keyevent; + if (guiparams.keyevent.keycode != 0) obj->gui_key_events_ = guiparams.keyevent; - std::this_thread::sleep_for( - std::chrono::milliseconds(obj->gui_refresh_period_ms_)); + std::this_thread::sleep_for(std::chrono::milliseconds(obj->gui_refresh_period_ms_)); } else if (obj->world_init_ok_ && obj->headless_) { obj->mvsim_world_->internalGraphicsLoopTasksForSimulation(); - std::this_thread::sleep_for( - std::chrono::microseconds(static_cast( - obj->mvsim_world_->get_simul_timestep() * 1000000))); + std::this_thread::sleep_for(std::chrono::microseconds( + static_cast(obj->mvsim_world_->get_simul_timestep() * 1000000))); } else { - std::this_thread::sleep_for( - std::chrono::milliseconds(obj->gui_refresh_period_ms_)); + std::this_thread::sleep_for(std::chrono::milliseconds(obj->gui_refresh_period_ms_)); } } } @@ -499,8 +472,7 @@ void MVSimNode::publishWorldElements(mvsim::WorldElementBase& obj) { // GridMaps -------------- static mrpt::system::CTicTac lastMapPublished; - if (mvsim::OccupancyGridMap* grid = - dynamic_cast(&obj); + if (mvsim::OccupancyGridMap* grid = dynamic_cast(&obj); grid && lastMapPublished.Tac() > 2.0) { lastMapPublished.Tic(); @@ -540,14 +512,12 @@ void MVSimNode::notifyROSWorldIsUpdated() { lastMapPublished.Tic(); - mvsim_world_->runVisitorOnWorldElements( - [this](mvsim::WorldElementBase& obj) - { publishWorldElements(obj); }); + mvsim_world_->runVisitorOnWorldElements([this](mvsim::WorldElementBase& obj) + { publishWorldElements(obj); }); } #endif - mvsim_world_->runVisitorOnVehicles([this](mvsim::VehicleBase& v) - { publishVehicles(v); }); + mvsim_world_->runVisitorOnVehicles([this](mvsim::VehicleBase& v) { publishVehicles(v); }); // Create subscribers & publishers for each vehicle's stuff: // ---------------------------------------------------- @@ -557,8 +527,7 @@ void MVSimNode::notifyROSWorldIsUpdated() size_t idx = 0; for (auto it = vehs.begin(); it != vehs.end(); ++it, ++idx) { - mvsim::VehicleBase* veh = - dynamic_cast(it->second.get()); + mvsim::VehicleBase* veh = dynamic_cast(it->second.get()); if (!veh) continue; initPubSubs(pubsub_vehicles_[idx], veh); @@ -587,8 +556,8 @@ double MVSimNode::myNowSec() const } void MVSimNode::sendStaticTF( - const std::string& frame_id, const std::string& child_frame_id, - const tf2::Transform& txf, const ros_Time& stamp) + const std::string& frame_id, const std::string& child_frame_id, const tf2::Transform& txf, + const ros_Time& stamp) { Msg_TransformStamped tx; tx.header.frame_id = frame_id; @@ -604,79 +573,67 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) { // sub: /cmd_vel #if PACKAGE_ROS_VERSION == 1 - pubsubs.sub_cmd_vel = - mvsim_node::make_shared(n_.subscribe( - vehVarName("cmd_vel", *veh), 10, - [this, veh](Msg_Twist_CSPtr msg) - { return this->onROSMsgCmdVel(msg, veh); })); + pubsubs.sub_cmd_vel = mvsim_node::make_shared(n_.subscribe( + vehVarName("cmd_vel", *veh), 10, + [this, veh](Msg_Twist_CSPtr msg) { return this->onROSMsgCmdVel(msg, veh); })); #else pubsubs.sub_cmd_vel = n_->create_subscription( vehVarName("cmd_vel", *veh), 10, - [this, veh](Msg_Twist_CSPtr msg) - { return this->onROSMsgCmdVel(msg, veh); }); + [this, veh](Msg_Twist_CSPtr msg) { return this->onROSMsgCmdVel(msg, veh); }); #endif #if PACKAGE_ROS_VERSION == 1 // pub: /odom - pubsubs.pub_odom = - mvsim_node::make_shared(n_.advertise( - vehVarName("odom", *veh), publisher_history_len_)); + pubsubs.pub_odom = mvsim_node::make_shared( + n_.advertise(vehVarName("odom", *veh), publisher_history_len_)); // pub: /base_pose_ground_truth - pubsubs.pub_ground_truth = - mvsim_node::make_shared(n_.advertise( - vehVarName("base_pose_ground_truth", *veh), - publisher_history_len_)); + pubsubs.pub_ground_truth = mvsim_node::make_shared(n_.advertise( + vehVarName("base_pose_ground_truth", *veh), publisher_history_len_)); // pub: /collision - pubsubs.pub_collision = - mvsim_node::make_shared(n_.advertise( - vehVarName("collision", *veh), publisher_history_len_)); + pubsubs.pub_collision = mvsim_node::make_shared( + n_.advertise(vehVarName("collision", *veh), publisher_history_len_)); // pub: /tf, /tf_static - pubsubs.pub_tf = - mvsim_node::make_shared(n_.advertise( - vehVarName("tf", *veh), publisher_history_len_)); - pubsubs.pub_tf_static = - mvsim_node::make_shared(n_.advertise( - vehVarName("tf_static", *veh), publisher_history_len_)); + pubsubs.pub_tf = mvsim_node::make_shared( + n_.advertise(vehVarName("tf", *veh), publisher_history_len_)); + pubsubs.pub_tf_static = mvsim_node::make_shared( + n_.advertise(vehVarName("tf_static", *veh), publisher_history_len_)); #else // pub: /odom - pubsubs.pub_odom = n_->create_publisher( - vehVarName("odom", *veh), publisher_history_len_); + pubsubs.pub_odom = + n_->create_publisher(vehVarName("odom", *veh), publisher_history_len_); // pub: /base_pose_ground_truth pubsubs.pub_ground_truth = n_->create_publisher( vehVarName("base_pose_ground_truth", *veh), publisher_history_len_); // pub: /collision - pubsubs.pub_collision = n_->create_publisher( - vehVarName("collision", *veh), publisher_history_len_); + pubsubs.pub_collision = + n_->create_publisher(vehVarName("collision", *veh), publisher_history_len_); // "/tf", "/tf_static" rclcpp::QoS qosLatched10(rclcpp::KeepLast(10)); - qosLatched10.durability( - rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + qosLatched10.durability(rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - pubsubs.pub_tf = n_->create_publisher( - vehVarName("tf", *veh), qosLatched10); - pubsubs.pub_tf_static = n_->create_publisher( - vehVarName("tf_static", *veh), qosLatched10); + pubsubs.pub_tf = n_->create_publisher(vehVarName("tf", *veh), qosLatched10); + pubsubs.pub_tf_static = + n_->create_publisher(vehVarName("tf_static", *veh), qosLatched10); #endif // pub: /chassis_markers { #if PACKAGE_ROS_VERSION == 1 pubsubs.pub_chassis_markers = mvsim_node::make_shared( - n_.advertise( - vehVarName("chassis_markers", *veh), 5, true /*latch*/)); + n_.advertise(vehVarName("chassis_markers", *veh), 5, true /*latch*/)); #else rclcpp::QoS qosLatched5(rclcpp::KeepLast(5)); - qosLatched5.durability(rmw_qos_durability_policy_t:: - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + qosLatched5.durability( + rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - pubsubs.pub_chassis_markers = n_->create_publisher( - vehVarName("chassis_markers", *veh), qosLatched5); + pubsubs.pub_chassis_markers = + n_->create_publisher(vehVarName("chassis_markers", *veh), qosLatched5); #endif const auto& poly = veh->getChassisShape(); @@ -687,8 +644,7 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) // [0] Chassis shape: auto& chassis_shape_msg = msg_shapes.markers[0]; - chassis_shape_msg.pose = - mrpt2ros::toROS_Pose(mrpt::poses::CPose3D::Identity()); + chassis_shape_msg.pose = mrpt2ros::toROS_Pose(mrpt::poses::CPose3D::Identity()); chassis_shape_msg.action = Msg_Marker::MODIFY; chassis_shape_msg.type = Msg_Marker::LINE_STRIP; @@ -724,8 +680,8 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) // Init values. Copy the contents from the chassis msg wheel_shape_msg = msg_shapes.markers[0]; - wheel_shape_msg.ns = mrpt::format( - "mvsim.chassis_shape.wheel%u", static_cast(i)); + wheel_shape_msg.ns = + mrpt::format("mvsim.chassis_shape.wheel%u", static_cast(i)); wheel_shape_msg.points.resize(5); wheel_shape_msg.points[0].x = lx; wheel_shape_msg.points[0].y = ly; @@ -757,16 +713,15 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) // pub: /chassis_polygon { #if PACKAGE_ROS_VERSION == 1 - pubsubs.pub_chassis_shape = - mvsim_node::make_shared(n_.advertise( - vehVarName("chassis_polygon", *veh), 1, true /*latch*/)); + pubsubs.pub_chassis_shape = mvsim_node::make_shared( + n_.advertise(vehVarName("chassis_polygon", *veh), 1, true /*latch*/)); #else rclcpp::QoS qosLatched1(rclcpp::KeepLast(1)); - qosLatched1.durability(rmw_qos_durability_policy_t:: - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + qosLatched1.durability( + rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - pubsubs.pub_chassis_shape = n_->create_publisher( - vehVarName("chassis_polygon", *veh), qosLatched1); + pubsubs.pub_chassis_shape = + n_->create_publisher(vehVarName("chassis_polygon", *veh), qosLatched1); #endif Msg_Polygon poly_msg; @@ -787,26 +742,23 @@ void MVSimNode::initPubSubs(TPubSubPerVehicle& pubsubs, mvsim::VehicleBase* veh) #if PACKAGE_ROS_VERSION == 1 // pub: /amcl_pose pubsubs.pub_amcl_pose = mvsim_node::make_shared( - n_.advertise( - vehVarName("amcl_pose", *veh), 1)); + n_.advertise(vehVarName("amcl_pose", *veh), 1)); // pub: /particlecloud pubsubs.pub_particlecloud = mvsim_node::make_shared( n_.advertise(vehVarName("particlecloud", *veh), 1)); #else // pub: /amcl_pose pubsubs.pub_amcl_pose = - n_->create_publisher( - vehVarName("amcl_pose", *veh), 1); + n_->create_publisher(vehVarName("amcl_pose", *veh), 1); // pub: /particlecloud - pubsubs.pub_particlecloud = n_->create_publisher( - vehVarName("particlecloud", *veh), 1); + pubsubs.pub_particlecloud = + n_->create_publisher(vehVarName("particlecloud", *veh), 1); #endif } // STATIC Identity transform /base_link -> /base_footprint sendStaticTF( - vehVarName("base_link", *veh), vehVarName("base_footprint", *veh), - tfIdentity_, myNow()); + vehVarName("base_link", *veh), vehVarName("base_footprint", *veh), tfIdentity_, myNow()); // TF STATIC(namespace ): /base_link -> /base_footprint Msg_TransformStamped tx; @@ -827,14 +779,13 @@ void MVSimNode::onROSMsgCmdVel(Msg_Twist_CSPtr cmd, mvsim::VehicleBase* veh) // Update cmd_vel timestamp: lastCmdVelTimestamp_[veh] = myNowSec(); - const bool ctrlAcceptTwist = controller->setTwistCommand( - {cmd->linear.x, cmd->linear.y, cmd->angular.z}); + const bool ctrlAcceptTwist = + controller->setTwistCommand({cmd->linear.x, cmd->linear.y, cmd->angular.z}); if (!ctrlAcceptTwist) { ROS12_WARN_THROTTLE( - 1.0, - "*Warning* Vehicle's controller ['%s'] refuses Twist commands!", + 1.0, "*Warning* Vehicle's controller ['%s'] refuses Twist commands!", veh->getName().c_str()); } } @@ -930,8 +881,7 @@ void MVSimNode::spinNotifyROS() tx.header.frame_id = "map"; tx.child_frame_id = sOdomName; tx.header.stamp = myNow(); - tx.transform = - tf2::toMsg(tf2::Transform::getIdentity()); + tx.transform = tf2::toMsg(tf2::Transform::getIdentity()); tf_br_.sendTransform(tx); // TF(namespace ): /map -> /odom @@ -949,8 +899,7 @@ void MVSimNode::spinNotifyROS() { // visualization_msgs::MarkerArray auto& msg_shapes = pubs.chassis_shape_msg; - ASSERT_EQUAL_( - msg_shapes.markers.size(), (1 + veh->getNumWheels())); + ASSERT_EQUAL_(msg_shapes.markers.size(), (1 + veh->getNumWheels())); // [0] Chassis shape: static no need to update. // [1:N] Wheel shapes: may move @@ -979,8 +928,7 @@ void MVSimNode::spinNotifyROS() tx.header.frame_id = sOdomName; tx.child_frame_id = sBaseLinkFrame; tx.header.stamp = myNow(); - tx.transform = - tf2::toMsg(mrpt2ros::toROS_tfTransform(odo_pose)); + tx.transform = tf2::toMsg(mrpt2ros::toROS_tfTransform(odo_pose)); tf_br_.sendTransform(tx); // TF(namespace ): /odom -> /base_link @@ -1042,35 +990,27 @@ void MVSimNode::onNewObservation( // ----------------------------- // Observation: 2d laser scans // ----------------------------- - if (const auto* o2DLidar = - dynamic_cast(obs.get()); + if (const auto* o2DLidar = dynamic_cast(obs.get()); o2DLidar) { internalOn(veh, *o2DLidar); } - else if (const auto* oImage = - dynamic_cast(obs.get()); + else if (const auto* oImage = dynamic_cast(obs.get()); oImage) { internalOn(veh, *oImage); } - else if (const auto* oRGBD = - dynamic_cast( - obs.get()); + else if (const auto* oRGBD = dynamic_cast(obs.get()); oRGBD) { internalOn(veh, *oRGBD); } - else if (const auto* oPC = - dynamic_cast( - obs.get()); + else if (const auto* oPC = dynamic_cast(obs.get()); oPC) { internalOn(veh, *oPC); } - else if (const auto* oIMU = - dynamic_cast(obs.get()); - oIMU) + else if (const auto* oIMU = dynamic_cast(obs.get()); oIMU) { internalOn(veh, *oIMU); } @@ -1079,16 +1019,14 @@ void MVSimNode::onNewObservation( // Don't know how to emit this observation to ROS! ROS12_WARN_STREAM_THROTTLE( 1.0, "Do not know how to publish this observation to ROS: '" - << obs->sensorLabel - << "', class: " << obs->GetRuntimeClass()->className); + << obs->sensorLabel << "', class: " << obs->GetRuntimeClass()->className); } } // end of onNewObservation() /** Creates the string "//" if there're more than one * vehicle in the World, or "/" otherwise. */ -std::string MVSimNode::vehVarName( - const std::string& sVarName, const mvsim::VehicleBase& veh) const +std::string MVSimNode::vehVarName(const std::string& sVarName, const mvsim::VehicleBase& veh) const { if (mvsim_world_->getListOfVehicles().size() == 1) { @@ -1101,23 +1039,20 @@ std::string MVSimNode::vehVarName( } void MVSimNode::internalOn( - const mvsim::VehicleBase& veh, - const mrpt::obs::CObservation2DRangeScan& obs) + const mvsim::VehicleBase& veh, const mrpt::obs::CObservation2DRangeScan& obs) { auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_); auto& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; // Create the publisher the first time an observation arrives: - const bool is_1st_pub = - pubs.pub_sensors.find(obs.sensorLabel) == pubs.pub_sensors.end(); + const bool is_1st_pub = pubs.pub_sensors.find(obs.sensorLabel) == pubs.pub_sensors.end(); auto& pub = pubs.pub_sensors[obs.sensorLabel]; if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = - mvsim_node::make_shared(n_.advertise( - vehVarName(obs.sensorLabel, veh), publisher_history_len_)); + pub = mvsim_node::make_shared( + n_.advertise(vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else pub = mvsim_node::make_shared>( n_, vehVarName(obs.sensorLabel, veh), publisher_history_len_); @@ -1157,22 +1092,20 @@ void MVSimNode::internalOn( } } -void MVSimNode::internalOn( - const mvsim::VehicleBase& veh, const mrpt::obs::CObservationIMU& obs) +void MVSimNode::internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservationIMU& obs) { auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_); auto& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; // Create the publisher the first time an observation arrives: - const bool is_1st_pub = - pubs.pub_sensors.find(obs.sensorLabel) == pubs.pub_sensors.end(); + const bool is_1st_pub = pubs.pub_sensors.find(obs.sensorLabel) == pubs.pub_sensors.end(); auto& pub = pubs.pub_sensors[obs.sensorLabel]; if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = mvsim_node::make_shared(n_.advertise( - vehVarName(obs.sensorLabel, veh), publisher_history_len_)); + pub = mvsim_node::make_shared( + n_.advertise(vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else pub = mvsim_node::make_shared>( n_, vehVarName(obs.sensorLabel, veh), publisher_history_len_); @@ -1213,22 +1146,20 @@ void MVSimNode::internalOn( } } -void MVSimNode::internalOn( - const mvsim::VehicleBase& veh, const mrpt::obs::CObservationImage& obs) +void MVSimNode::internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObservationImage& obs) { auto lck = mrpt::lockHelper(pubsub_vehicles_mtx_); auto& pubs = pubsub_vehicles_[veh.getVehicleIndex()]; // Create the publisher the first time an observation arrives: - const bool is_1st_pub = - pubs.pub_sensors.find(obs.sensorLabel) == pubs.pub_sensors.end(); + const bool is_1st_pub = pubs.pub_sensors.find(obs.sensorLabel) == pubs.pub_sensors.end(); auto& pub = pubs.pub_sensors[obs.sensorLabel]; if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pub = mvsim_node::make_shared(n_.advertise( - vehVarName(obs.sensorLabel, veh), publisher_history_len_)); + pub = mvsim_node::make_shared( + n_.advertise(vehVarName(obs.sensorLabel, veh), publisher_history_len_)); #else pub = mvsim_node::make_shared>( n_, vehVarName(obs.sensorLabel, veh), publisher_history_len_); @@ -1269,8 +1200,7 @@ void MVSimNode::internalOn( } void MVSimNode::internalOn( - const mvsim::VehicleBase& veh, - const mrpt::obs::CObservation3DRangeScan& obs) + const mvsim::VehicleBase& veh, const mrpt::obs::CObservation3DRangeScan& obs) { using namespace std::string_literals; @@ -1281,8 +1211,7 @@ void MVSimNode::internalOn( const auto lbImage = obs.sensorLabel + "_image"s; // Create the publisher the first time an observation arrives: - const bool is_1st_pub = - pubs.pub_sensors.find(lbPoints) == pubs.pub_sensors.end(); + const bool is_1st_pub = pubs.pub_sensors.find(lbPoints) == pubs.pub_sensors.end(); auto& pubPts = pubs.pub_sensors[lbPoints]; auto& pubImg = pubs.pub_sensors[lbImage]; @@ -1290,12 +1219,10 @@ void MVSimNode::internalOn( if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pubImg = - mvsim_node::make_shared(n_.advertise( - vehVarName(lbImage, veh), publisher_history_len_)); + pubImg = mvsim_node::make_shared( + n_.advertise(vehVarName(lbImage, veh), publisher_history_len_)); pubPts = mvsim_node::make_shared( - n_.advertise( - vehVarName(lbPoints, veh), publisher_history_len_)); + n_.advertise(vehVarName(lbPoints, veh), publisher_history_len_)); #else pubImg = mvsim_node::make_shared>( n_, vehVarName(lbImage, veh), publisher_history_len_); @@ -1315,8 +1242,7 @@ void MVSimNode::internalOn( if (obs.hasIntensityImage) { // Send TF: - mrpt::poses::CPose3D sensorPose = - obs.sensorPose + obs.relativePoseIntensityWRTDepth; + mrpt::poses::CPose3D sensorPose = obs.sensorPose + obs.relativePoseIntensityWRTDepth; auto transform = mrpt2ros::toROS_tfTransform(sensorPose); Msg_TransformStamped tfStmp; @@ -1377,8 +1303,7 @@ void MVSimNode::internalOn( pp.takeIntoAccountSensorPoseOnRobot = false; mrpt::maps::CSimplePointsMap pts; - const_cast(obs).unprojectInto( - pts, pp); + const_cast(obs).unprojectInto(pts, pp); mrpt2ros::toROS(pts, msg_header, msg_pts); pubPts->publish(mvsim_node::make_shared(msg_pts)); } @@ -1396,8 +1321,7 @@ void MVSimNode::internalOn( const auto lbPoints = obs.sensorLabel + "_points"s; // Create the publisher the first time an observation arrives: - const bool is_1st_pub = - pubs.pub_sensors.find(lbPoints) == pubs.pub_sensors.end(); + const bool is_1st_pub = pubs.pub_sensors.find(lbPoints) == pubs.pub_sensors.end(); auto& pubPts = pubs.pub_sensors[lbPoints]; @@ -1405,8 +1329,7 @@ void MVSimNode::internalOn( { #if PACKAGE_ROS_VERSION == 1 pubPts = mvsim_node::make_shared( - n_.advertise( - vehVarName(lbPoints, veh), publisher_history_len_)); + n_.advertise(vehVarName(lbPoints, veh), publisher_history_len_)); #else pubPts = mvsim_node::make_shared>( n_, vehVarName(lbPoints, veh), publisher_history_len_); @@ -1447,30 +1370,27 @@ void MVSimNode::internalOn( msg_header.frame_id = sSensorFrameId_points; #if defined(HAVE_POINTS_XYZIRT) - if (auto* xyzirt = dynamic_cast( - obs.pointcloud.get()); + if (auto* xyzirt = dynamic_cast(obs.pointcloud.get()); xyzirt) { mrpt2ros::toROS(*xyzirt, msg_header, msg_pts); } else #endif - if (auto* xyzi = dynamic_cast( - obs.pointcloud.get()); + if (auto* xyzi = dynamic_cast(obs.pointcloud.get()); xyzi) { mrpt2ros::toROS(*xyzi, msg_header, msg_pts); } - else if (auto* sPts = dynamic_cast( - obs.pointcloud.get()); + else if (auto* sPts = + dynamic_cast(obs.pointcloud.get()); sPts) { mrpt2ros::toROS(*sPts, msg_header, msg_pts); } else { - THROW_EXCEPTION( - "Do not know how to handle this variant of CPointsMap"); + THROW_EXCEPTION("Do not know how to handle this variant of CPointsMap"); } pubPts->publish(mvsim_node::make_shared(msg_pts)); diff --git a/mvsim_node_src/mvsim_node_main.cpp b/mvsim_node_src/mvsim_node_main.cpp index ae12a36e..8afb4638 100644 --- a/mvsim_node_src/mvsim_node_main.cpp +++ b/mvsim_node_src/mvsim_node_main.cpp @@ -30,8 +30,7 @@ int main(int argc, char** argv) try { // Create a "Node" object. - mvsim_node::shared_ptr node = - mvsim_node::make_shared(n); + mvsim_node::shared_ptr node = mvsim_node::make_shared(n); // Declare variables that can be modified by launch file or command // line. @@ -69,9 +68,8 @@ int main(int argc, char** argv) // Do this before parameter server, else some of the parameter server // values can be overwritten. dynamic_reconfigure::Server dr_srv; - dr_srv.setCallback( - [&node](mvsim::mvsimNodeConfig& config, uint32_t level) - { return node->configCallback(config, level); }); + dr_srv.setCallback([&node](mvsim::mvsimNodeConfig& config, uint32_t level) + { return node->configCallback(config, level); }); #endif // Tell ROS how fast to run this node-> @@ -97,11 +95,9 @@ int main(int argc, char** argv) rclcpp::on_shutdown( [&node]() { - std::cout << "[rclcpp::on_shutdown] Destroying MVSIM node..." - << std::endl; + std::cout << "[rclcpp::on_shutdown] Destroying MVSIM node..." << std::endl; node->terminateSimulation(); - std::cout << "[rclcpp::on_shutdown] MVSIM node destroyed." - << std::endl; + std::cout << "[rclcpp::on_shutdown] MVSIM node destroyed." << std::endl; }); rclcpp::spin(n); @@ -115,9 +111,7 @@ int main(int argc, char** argv) #if PACKAGE_ROS_VERSION == 1 std::cerr << e.what() << std::endl; #else - RCLCPP_ERROR_STREAM( - n->get_logger(), "Exception in main node body:\n" - << e.what()); + RCLCPP_ERROR_STREAM(n->get_logger(), "Exception in main node body:\n" << e.what()); #endif return 1; }