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/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/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 de168f7b..a244a53e 100644 --- a/mvsim_node_src/include/mvsim/mvsim_node_core.h +++ b/mvsim_node_src/include/mvsim/mvsim_node_core.h @@ -9,14 +9,11 @@ #pragma once -#if PACKAGE_ROS_VERSION == 1 -#include -#include -#endif - #include #include -#include +#include +#include +#include #include #include #include @@ -25,38 +22,95 @@ #include #include +#include +#include + #if PACKAGE_ROS_VERSION == 1 +#include +#include +#include +#include #include +#include +#include +#include +#include #include #include #include #include #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; +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 "rclcpp/clock.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time_source.hpp" +#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; +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 -#include -#include - -namespace mrpt +namespace mvsim_node +{ +#if PACKAGE_ROS_VERSION == 1 +template +boost::shared_ptr make_shared(Args&&... args) { -namespace obs + return boost::make_shared(std::forward(args)...); +} + +template +using shared_ptr = boost::shared_ptr; +#else +template +std::shared_ptr make_shared(Args&&... args) { -class CObservationPointCloud; + return std::make_shared(std::forward(args)...); } -} // namespace mrpt + +template +using shared_ptr = std::shared_ptr; +#endif +} // namespace mvsim_node /** A class to wrap libmvsim as a ROS node */ @@ -86,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.) - 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 +164,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_; @@ -124,11 +176,11 @@ 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_; + rclcpp::Publisher::SharedPtr pub_map_ros_; + rclcpp::Publisher::SharedPtr pub_map_metadata_; rclcpp::TimeSource ts_{n_}; rclcpp::Clock::SharedPtr clock_; #endif @@ -144,56 +196,55 @@ 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 + 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: - ros::Publisher pub_amcl_pose, pub_particlecloud; + 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; - ros::Publisher pub_chassis_markers; //!< "/chassis_markers" - ros::Publisher pub_chassis_shape; //!< "/chassis_shape" - ros::Publisher 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" - visualization_msgs::MarkerArray chassis_shape_msg; + mvsim_node::shared_ptr pub_tf; //!< "/tf" + mvsim_node::shared_ptr pub_tf_static; //!< "/tf_static" - ros::Publisher pub_tf; - ros::Publisher pub_tf_static; + 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:: - 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; + 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 }; @@ -209,25 +260,15 @@ class MVSimNode // === End ROS Publishers ==== // === ROS Hooks ==== - void onROSMsgCmdVel( -#if PACKAGE_ROS_VERSION == 1 - const geometry_msgs::Twist::ConstPtr& cmd, -#else - const geometry_msgs::msg::Twist::ConstSharedPtr& cmd, -#endif - mvsim::VehicleBase* veh); + void onROSMsgCmdVel(Msg_Twist_CSPtr cmd, mvsim::VehicleBase* veh); // === End ROS Hooks==== #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(); @@ -277,42 +318,24 @@ 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, -#if PACKAGE_ROS_VERSION == 1 - const ros::Time& stamp -#else - const rclcpp::Time& stamp -#endif - ); + const std::string& frame_id, const std::string& child_frame_id, const tf2::Transform& tx, + 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"}; 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 new file mode 100644 index 00000000..012c90ed --- /dev/null +++ b/mvsim_node_src/include/wrapper/publisher_wrapper.h @@ -0,0 +1,42 @@ +#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 + { + 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 61a35d52..08e0f2ee 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -8,42 +8,52 @@ +-------------------------------------------------------------------------+ */ #include -#include -#include -#include -#include #include #include // kbhit() #include #include +#include + +#include "rapidxml_utils.hpp" + +#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 // =========================================== -#include -#include -#include #include #include #include #include #include #include -#include -#include -#include #include #include #include #include -#include #include + // usings: -using Msg_OccupancyGrid = nav_msgs::OccupancyGrid; -using Msg_MapMetaData = nav_msgs::MapMetaData; +using ros::ok; + +using Msg_Header = std_msgs::Header; + +using Msg_Pose = geometry_msgs::Pose; using Msg_TransformStamped = geometry_msgs::TransformStamped; -using Msg_Bool = std_msgs::Bool; + +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_Marker = visualization_msgs::Marker; #else // =========================================== // ROS 2 @@ -55,14 +65,10 @@ using Msg_Bool = std_msgs::Bool; #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) @@ -72,24 +78,20 @@ using Msg_Bool = std_msgs::Bool; #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; -#endif +using rclcpp::ok; -#if MRPT_VERSION >= 0x020b04 // >=2.11.4? -#define HAVE_POINTS_XYZIRT -#endif +using Msg_Header = std_msgs::msg::Header; -#if defined(HAVE_POINTS_XYZIRT) -#include -#endif +using Msg_Pose = geometry_msgs::msg::Pose; +using Msg_TransformStamped = geometry_msgs::msg::TransformStamped; -#include -#include +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; -#include "mvsim/mvsim_node_core.h" +using Msg_Marker = visualization_msgs::msg::Marker; +#endif #if PACKAGE_ROS_VERSION == 1 namespace mrpt2ros = mrpt::ros1bridge; @@ -99,9 +101,14 @@ 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(), *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 @@ -118,7 +125,25 @@ 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_); @@ -128,57 +153,53 @@ 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 - // 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( - 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 @@ -189,43 +210,16 @@ 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, - 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; - auto fut = ros_publisher_workers_.enqueue( + const auto fut = ros_publisher_workers_.enqueue( [this, vehPtr, obsCopy]() { try @@ -238,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()); } }); }); @@ -253,7 +245,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(); } @@ -303,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(); - ROS_INFO("MVSimNode::configCallback() called."); + 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 @@ -321,8 +311,6 @@ void MVSimNode::spin() { using namespace mvsim; using namespace std::string_literals; - using mrpt::DEG2RAD; - using mrpt::RAD2DEG; if (!mvsim_world_) return; @@ -331,8 +319,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; @@ -374,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_) { @@ -384,19 +371,16 @@ 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: { - ControllerBaseInterface* controller = - it_veh->second->getControllerInterface(); + auto* controller = it_veh->second->getControllerInterface(); ControllerBaseInterface::TeleopInput teleop_in; ControllerBaseInterface::TeleopOutput teleop_out; teleop_in.keycode = keyevent.keycode; @@ -414,19 +398,14 @@ 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 + // 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) { - mvsim::ControllerBaseInterface* controller = - veh->getControllerInterface(); + auto* controller = veh->getControllerInterface(); controller->setTwistCommand({0, 0, 0}); toRemove.insert(veh); @@ -445,38 +424,32 @@ 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); // 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(); @@ -516,22 +488,14 @@ 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(); -#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 @@ -548,25 +512,22 @@ 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: // ---------------------------------------------------- - auto& vehs = mvsim_world_->getListOfVehicles(); + const auto& vehs = mvsim_world_->getListOfVehicles(); pubsub_vehicles_.clear(); pubsub_vehicles_.resize(vehs.size()); 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); @@ -576,21 +537,27 @@ 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; tx.header.frame_id = frame_id; @@ -606,76 +573,69 @@ 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 = 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 - 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); }); + [this, veh](Msg_Twist_CSPtr msg) { return this->onROSMsgCmdVel(msg, veh); }); #endif #if PACKAGE_ROS_VERSION == 1 // pub: /odom - pubsubs.pub_odom = 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 = 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 = 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 = n_.advertise( - vehVarName("tf", *veh), publisher_history_len_); - pubsubs.pub_tf_static = 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( + 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 = - n_.advertise( - vehVarName("chassis_markers", *veh), 5, true /*latch*/); + 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); + 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); + 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; @@ -684,16 +644,10 @@ 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()); -#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"; @@ -719,15 +673,15 @@ 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; // 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; @@ -753,33 +707,26 @@ 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*/); - - geometry_msgs::Polygon poly_msg; + 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); - - geometry_msgs::msg::Polygon poly_msg; + n_->create_publisher(vehVarName("chassis_polygon", *veh), qosLatched1); #endif + 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++) { @@ -787,94 +734,59 @@ 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_) { #if PACKAGE_ROS_VERSION == 1 // pub: /amcl_pose - pubsubs.pub_amcl_pose = - n_.advertise( - vehVarName("amcl_pose", *veh), 1); + pubsubs.pub_amcl_pose = mvsim_node::make_shared( + n_.advertise(vehVarName("amcl_pose", *veh), 1)); // pub: /particlecloud - pubsubs.pub_particlecloud = 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); + n_->create_publisher(vehVarName("amcl_pose", *veh), 1); // pub: /particlecloud pubsubs.pub_particlecloud = - n_->create_publisher( - vehVarName("particlecloud", *veh), 1); + 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; - 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; - tfMsg.transforms.push_back(tx); - pubsubs.pub_tf_static.publish(tfMsg); -#else - tf2_msgs::msg::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); pubsubs.pub_tf_static->publish(tfMsg); -#endif } -void MVSimNode::onROSMsgCmdVel( -#if PACKAGE_ROS_VERSION == 1 - const geometry_msgs::Twist::ConstPtr& cmd, -#else - const geometry_msgs::msg::Twist::ConstSharedPtr& cmd, -#endif - mvsim::VehicleBase* veh) +void MVSimNode::onROSMsgCmdVel(Msg_Twist_CSPtr cmd, mvsim::VehicleBase* veh) { - mvsim::ControllerBaseInterface* controller = veh->getControllerInterface(); + auto* 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; + // 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) { -#if PACKAGE_ROS_VERSION == 1 - ROS_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!", + ROS12_WARN_THROTTLE( + 1.0, "*Warning* Vehicle's controller ['%s'] refuses Twist commands!", veh->getName().c_str()); -#endif } } @@ -883,24 +795,19 @@ void MVSimNode::spinNotifyROS() { if (!mvsim_world_) return; - using namespace mvsim; 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" // ---------------------------------------------------------------- #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(); + // sim_time_ = myNow(); // MRPT_TODO("Publish /clock for ROS2 too?"); #endif @@ -921,7 +828,7 @@ void MVSimNode::spinNotifyROS() for (auto it = vehs.begin(); it != vehs.end(); ++it, ++i) { - const VehicleBase::Ptr& veh = it->second; + const auto& veh = it->second; auto& pubs = pubsub_vehicles_[i]; const std::string sOdomName = vehVarName("odom", *veh); @@ -934,12 +841,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; @@ -951,20 +853,11 @@ 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 - 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 { @@ -972,22 +865,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 @@ -996,21 +881,14 @@ 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 tx.child_frame_id = "odom"; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; - tfMsg.transforms.push_back(tx); - pubs.pub_tf.publish(tfMsg); -#else - tf2_msgs::msg::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); pubs.pub_tf->publish(tfMsg); -#endif } } } @@ -1021,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 @@ -1030,7 +907,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()); @@ -1038,11 +915,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 @@ -1055,31 +928,20 @@ 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 tx.header.frame_id = "odom"; tx.child_frame_id = "base_link"; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; - tfMsg.transforms.push_back(tx); - pubs.pub_tf.publish(tfMsg); -#else - tf2_msgs::msg::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tx); pubs.pub_tf->publish(tfMsg); -#endif } // 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 @@ -1088,11 +950,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 } } @@ -1101,19 +959,11 @@ 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: -#if PACKAGE_ROS_VERSION == 1 - pubs.pub_collision.publish(colMsg); -#else pubs.pub_collision->publish(colMsg); -#endif } } // end for each vehicle @@ -1127,14 +977,8 @@ 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; -#else - if (!rclcpp::ok()) return; -#endif + if (!ok()) return; ASSERT_(obs); ASSERT_(!obs->sensorLabel.empty()); @@ -1146,64 +990,44 @@ 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); } 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 + << 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 { - using namespace std::string_literals; - if (mvsim_world_->getListOfVehicles().size() == 1) { return sVarName; @@ -1215,43 +1039,32 @@ 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_); - 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 = - 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 = 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 = n_->create_publisher( - vehVarName(obs.sensorLabel, veh), publisher_history_len_); + 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: - 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); @@ -1262,78 +1075,49 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = obs.sensorLabel; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); - pubs.pub_tf.publish(tfMsg); -#else - tf2_msgs::msg::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); -#endif // 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 - mrpt2ros::toROS(obs, msg_laser, msg_pose_laser); - + Msg_Pose msg_pose_laser; + Msg_LaserScan msg_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)); } } -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_); - 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 = - 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 = 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 = n_->create_publisher( - vehVarName(obs.sensorLabel, veh), publisher_history_len_); - ASSERT_(pub); + pub = mvsim_node::make_shared>( + n_, vehVarName(obs.sensorLabel, veh), publisher_history_len_); #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: - 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); @@ -1344,78 +1128,51 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = obs.sensorLabel; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); - pubs.pub_tf.publish(tfMsg); -#else - tf2_msgs::msg::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); -#endif // 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; - 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)); } } -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_); - 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 = - 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 = 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 = n_->create_publisher( - vehVarName(obs.sensorLabel, veh), publisher_history_len_); + 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: 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); @@ -1426,54 +1183,35 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = obs.sensorLabel; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); - pubs.pub_tf.publish(tfMsg); -#else - tf2_msgs::msg::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); -#endif // 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; - 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)); } } 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; 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; // 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]; @@ -1481,31 +1219,19 @@ 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 = 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 = n_->create_publisher( - vehVarName(lbImage, veh), publisher_history_len_); - pubPts = n_->create_publisher( - 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(); -#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); @@ -1516,10 +1242,8 @@ void MVSimNode::internalOn( if (obs.hasIntensityImage) { // Send TF: - mrpt::poses::CPose3D sensorPose = - obs.sensorPose + obs.relativePoseIntensityWRTDepth; - - tf2::Transform transform = mrpt2ros::toROS_tfTransform(sensorPose); + mrpt::poses::CPose3D sensorPose = obs.sensorPose + obs.relativePoseIntensityWRTDepth; + auto transform = mrpt2ros::toROS_tfTransform(sensorPose); Msg_TransformStamped tfStmp; tfStmp.transform = tf2::toMsg(transform); @@ -1530,36 +1254,19 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = lbImage; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); - pubs.pub_tf.publish(tfMsg); -#else - tf2_msgs::msg::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); -#endif // 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; - 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)); } } @@ -1569,8 +1276,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); @@ -1581,26 +1287,15 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = lbPoints; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); - pubs.pub_tf.publish(tfMsg); -#else - tf2_msgs::msg::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); -#endif // 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; @@ -1608,16 +1303,9 @@ 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); - -#if PACKAGE_ROS_VERSION == 1 - pubPts.publish(msg_pts); -#else - pubPoints->publish(msg_pts); -#endif + pubPts->publish(mvsim_node::make_shared(msg_pts)); } } } @@ -1628,35 +1316,27 @@ 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; // 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]; if (is_1st_pub) { #if PACKAGE_ROS_VERSION == 1 - pubPts = n_.advertise( - vehVarName(lbPoints, veh), publisher_history_len_); + pubPts = mvsim_node::make_shared( + n_.advertise(vehVarName(lbPoints, veh), publisher_history_len_)); #else - pubPts = n_->create_publisher( - vehVarName(lbPoints, veh), publisher_history_len_); + 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(); @@ -1666,8 +1346,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); @@ -1678,60 +1357,42 @@ void MVSimNode::internalOn( tfStmp.header.frame_id = "base_link"; tfStmp.child_frame_id = lbPoints; -#if PACKAGE_ROS_VERSION == 1 - tf2_msgs::TFMessage tfMsg; - tfMsg.transforms.push_back(tfStmp); - pubs.pub_tf.publish(tfMsg); -#else - tf2_msgs::msg::TFMessage tfMsg; + Msg_TFMessage tfMsg; tfMsg.transforms.push_back(tfStmp); pubs.pub_tf->publish(tfMsg); -#endif // 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; #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"); } -#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 fdc957df..8afb4638 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. @@ -68,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; - 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-> @@ -85,22 +84,20 @@ 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]() { if (rclcpp::ok()) node->spin(); }); 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); @@ -114,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; }