From d94e7c7731b22bb5e0e2532312a881e0b49181cc Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 8 Jul 2022 14:36:59 -0500 Subject: [PATCH] [rolling] Port test improvements from #272 (#273) Port test improvements from #272 on galactic to humble+. This should clear up some of the CI issues that we have been facing. Signed-off-by: Michael Carroll Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel --- .../test/publishers/ign_publisher.cpp | 4 +- .../test/publishers/ros_publisher.cpp | 90 +++++++++---------- .../ros_subscriber/ros_subscriber.cpp | 11 ++- .../ros_subscriber/ros_subscriber.hpp | 4 +- ros_ign_bridge/test/utils/test_utils.hpp | 6 +- 5 files changed, 59 insertions(+), 56 deletions(-) diff --git a/ros_ign_bridge/test/publishers/ign_publisher.cpp b/ros_ign_bridge/test/publishers/ign_publisher.cpp index 1de96af0..28aa36d6 100644 --- a/ros_ign_bridge/test/publishers/ign_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ign_publisher.cpp @@ -275,7 +275,7 @@ int main(int /*argc*/, char **/*argv*/) ignition::msgs::VideoRecord video_record_msg; ros_ign_bridge::testing::createTestMsg(video_record_msg); - // Publish messages at 1Hz. + // Publish messages at 100Hz. while (!g_terminatePub) { color_pub.Publish(color_msg); light_pub.Publish(light_msg); @@ -322,7 +322,7 @@ int main(int /*argc*/, char **/*argv*/) track_visual_pub.Publish(track_visual_msg); video_record_pub.Publish(video_record_msg); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } return 0; diff --git a/ros_ign_bridge/test/publishers/ros_publisher.cpp b/ros_ign_bridge/test/publishers/ros_publisher.cpp index 0bef8a57..52f72a76 100644 --- a/ros_ign_bridge/test/publishers/ros_publisher.cpp +++ b/ros_ign_bridge/test/publishers/ros_publisher.cpp @@ -22,259 +22,259 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("ros_string_publisher"); - rclcpp::Rate loop_rate(1); + rclcpp::Rate loop_rate(100); // builtin_interfaces::msg::Time. - auto time_pub = node->create_publisher("time", 1000); + auto time_pub = node->create_publisher("time", 1); builtin_interfaces::msg::Time time_msg; ros_ign_bridge::testing::createTestMsg(time_msg); // std_msgs::msg::Color. - auto color_pub = node->create_publisher("color", 1000); + auto color_pub = node->create_publisher("color", 1); std_msgs::msg::ColorRGBA color_msg; ros_ign_bridge::testing::createTestMsg(color_msg); // std_msgs::msg::Bool. - auto bool_pub = node->create_publisher("bool", 1000); + auto bool_pub = node->create_publisher("bool", 1); std_msgs::msg::Bool bool_msg; ros_ign_bridge::testing::createTestMsg(bool_msg); // std_msgs::msg::Empty. - auto empty_pub = node->create_publisher("empty", 1000); + auto empty_pub = node->create_publisher("empty", 1); std_msgs::msg::Empty empty_msg; // std_msgs::msg::Float32. - auto float_pub = node->create_publisher("float", 1000); + auto float_pub = node->create_publisher("float", 1); std_msgs::msg::Float32 float_msg; ros_ign_bridge::testing::createTestMsg(float_msg); // std_msgs::Float64. - auto double_pub = node->create_publisher("double", 1000); + auto double_pub = node->create_publisher("double", 1); std_msgs::msg::Float64 double_msg; ros_ign_bridge::testing::createTestMsg(double_msg); // std_msgs::UInt32. - auto uint32_pub = node->create_publisher("uint32", 1000); + auto uint32_pub = node->create_publisher("uint32", 1); std_msgs::msg::UInt32 uint32_msg; ros_ign_bridge::testing::createTestMsg(uint32_msg); // std_msgs::msg::Header. - auto header_pub = node->create_publisher("header", 1000); + auto header_pub = node->create_publisher("header", 1); std_msgs::msg::Header header_msg; ros_ign_bridge::testing::createTestMsg(header_msg); // std_msgs::msg::String. - auto string_pub = node->create_publisher("string", 1000); + auto string_pub = node->create_publisher("string", 1); std_msgs::msg::String string_msg; ros_ign_bridge::testing::createTestMsg(string_msg); // geometry_msgs::msg::Quaternion. auto quaternion_pub = - node->create_publisher("quaternion", 1000); + node->create_publisher("quaternion", 1); geometry_msgs::msg::Quaternion quaternion_msg; ros_ign_bridge::testing::createTestMsg(quaternion_msg); // geometry_msgs::msg::Vector3. auto vector3_pub = - node->create_publisher("vector3", 1000); + node->create_publisher("vector3", 1); geometry_msgs::msg::Vector3 vector3_msg; ros_ign_bridge::testing::createTestMsg(vector3_msg); // sensor_msgs::msg::Clock. auto clock_pub = - node->create_publisher("clock", 1000); + node->create_publisher("clock", 1); rosgraph_msgs::msg::Clock clock_msg; ros_ign_bridge::testing::createTestMsg(clock_msg); // geometry_msgs::msg::Point. auto point_pub = - node->create_publisher("point", 1000); + node->create_publisher("point", 1); geometry_msgs::msg::Point point_msg; ros_ign_bridge::testing::createTestMsg(point_msg); // geometry_msgs::msg::Pose. auto pose_pub = - node->create_publisher("pose", 1000); + node->create_publisher("pose", 1); geometry_msgs::msg::Pose pose_msg; ros_ign_bridge::testing::createTestMsg(pose_msg); // geometry_msgs::msg::PoseWithCovariance. auto pose_cov_pub = node->create_publisher( - "pose_with_covariance", 1000); + "pose_with_covariance", 1); geometry_msgs::msg::PoseWithCovariance pose_cov_msg; ros_ign_bridge::testing::createTestMsg(pose_cov_msg); // geometry_msgs::msg::PoseStamped. auto pose_stamped_pub = - node->create_publisher("pose_stamped", 1000); + node->create_publisher("pose_stamped", 1); geometry_msgs::msg::PoseStamped pose_stamped_msg; ros_ign_bridge::testing::createTestMsg(pose_stamped_msg); // geometry_msgs::msg::Transform. auto transform_pub = - node->create_publisher("transform", 1000); + node->create_publisher("transform", 1); geometry_msgs::msg::Transform transform_msg; ros_ign_bridge::testing::createTestMsg(transform_msg); // geometry_msgs::msg::TransformStamped. auto transform_stamped_pub = - node->create_publisher("transform_stamped", 1000); + node->create_publisher("transform_stamped", 1); geometry_msgs::msg::TransformStamped transform_stamped_msg; ros_ign_bridge::testing::createTestMsg(transform_stamped_msg); // geometry_msgs::msg::Twist. auto twist_pub = - node->create_publisher("twist", 1000); + node->create_publisher("twist", 1); geometry_msgs::msg::Twist twist_msg; ros_ign_bridge::testing::createTestMsg(twist_msg); // geometry_msgs::msg::TwistWithCovariance. auto twist_cov_pub = node->create_publisher( - "twist_with_covariance", 1000); + "twist_with_covariance", 1); geometry_msgs::msg::TwistWithCovariance twist_cov_msg; ros_ign_bridge::testing::createTestMsg(twist_cov_msg); auto tf2_message_pub = - node->create_publisher("tf2_message", 1000); + node->create_publisher("tf2_message", 1); tf2_msgs::msg::TFMessage tf2_msg; ros_ign_bridge::testing::createTestMsg(tf2_msg); // geometry_msgs::msg::Wrench. auto wrench_pub = - node->create_publisher("wrench", 1000); + node->create_publisher("wrench", 1); geometry_msgs::msg::Wrench wrench_msg; ros_ign_bridge::testing::createTestMsg(wrench_msg); // ros_ign_interfaces::msg::Light. auto light_pub = - node->create_publisher("light", 1000); + node->create_publisher("light", 1); ros_ign_interfaces::msg::Light light_msg; ros_ign_bridge::testing::createTestMsg(light_msg); // ros_ign_interfaces::msg::JointWrench. auto joint_wrench_pub = - node->create_publisher("joint_wrench", 1000); + node->create_publisher("joint_wrench", 1); ros_ign_interfaces::msg::JointWrench joint_wrench_msg; ros_ign_bridge::testing::createTestMsg(joint_wrench_msg); // ros_ign_interfaces::msg::Entity. auto entity_pub = - node->create_publisher("entity", 1000); + node->create_publisher("entity", 1); ros_ign_interfaces::msg::Entity entity_msg; ros_ign_bridge::testing::createTestMsg(entity_msg); // ros_ign_interfaces::msg::Contact. auto contact_pub = - node->create_publisher("contact", 1000); + node->create_publisher("contact", 1); ros_ign_interfaces::msg::Contact contact_msg; ros_ign_bridge::testing::createTestMsg(contact_msg); // ros_ign_interfaces::msg::Contacts. auto contacts_pub = - node->create_publisher("contacts", 1000); + node->create_publisher("contacts", 1); ros_ign_interfaces::msg::Contacts contacts_msg; ros_ign_bridge::testing::createTestMsg(contacts_msg); // ros_ign_interfaces::msg::GuiCamera. auto gui_camera_pub = - node->create_publisher("gui_camera", 1000); + node->create_publisher("gui_camera", 1); ros_ign_interfaces::msg::GuiCamera gui_camera_msg; ros_ign_bridge::testing::createTestMsg(gui_camera_msg); // ros_ign_interfaces::msg::StringVec. auto stringmsg_v_pub = - node->create_publisher("stringmsg_v", 1000); + node->create_publisher("stringmsg_v", 1); ros_ign_interfaces::msg::StringVec stringmsg_v_msg; ros_ign_bridge::testing::createTestMsg(stringmsg_v_msg); // ros_ign_interfaces::msg::TrackVisual. auto track_visual_pub = - node->create_publisher("track_visual", 1000); + node->create_publisher("track_visual", 1); ros_ign_interfaces::msg::TrackVisual track_visual_msg; ros_ign_bridge::testing::createTestMsg(track_visual_msg); // ros_ign_interfaces::msg::VideoRecord. auto video_record_pub = - node->create_publisher("video_record", 1000); + node->create_publisher("video_record", 1); ros_ign_interfaces::msg::VideoRecord video_record_msg; ros_ign_bridge::testing::createTestMsg(video_record_msg); // // mav_msgs::msg::Actuators. // auto actuators_pub = - // node->create_publisher("actuators", 1000); + // node->create_publisher("actuators", 1); // mav_msgs::msg::Actuators actuators_msg; // ros_ign_bridge::testing::createTestMsg(actuators_msg); // nav_msgs::msg::Odometry. auto odometry_pub = - node->create_publisher("odometry", 1000); + node->create_publisher("odometry", 1); nav_msgs::msg::Odometry odometry_msg; ros_ign_bridge::testing::createTestMsg(odometry_msg); // nav_msgs::msg::Odometry. auto odometry_cov_pub = - node->create_publisher("odometry_with_covariance", 1000); + node->create_publisher("odometry_with_covariance", 1); nav_msgs::msg::Odometry odometry_cov_msg; ros_ign_bridge::testing::createTestMsg(odometry_cov_msg); // sensor_msgs::msg::Image. auto image_pub = - node->create_publisher("image", 1000); + node->create_publisher("image", 1); sensor_msgs::msg::Image image_msg; ros_ign_bridge::testing::createTestMsg(image_msg); // sensor_msgs::msg::CameraInfo. auto camera_info_pub = - node->create_publisher("camera_info", 1000); + node->create_publisher("camera_info", 1); sensor_msgs::msg::CameraInfo camera_info_msg; ros_ign_bridge::testing::createTestMsg(camera_info_msg); // sensor_msgs::msg::FluidPressure. auto fluid_pressure_pub = - node->create_publisher("fluid_pressure", 1000); + node->create_publisher("fluid_pressure", 1); sensor_msgs::msg::FluidPressure fluid_pressure_msg; ros_ign_bridge::testing::createTestMsg(fluid_pressure_msg); // sensor_msgs::msg::Imu. auto imu_pub = - node->create_publisher("imu", 1000); + node->create_publisher("imu", 1); sensor_msgs::msg::Imu imu_msg; ros_ign_bridge::testing::createTestMsg(imu_msg); // sensor_msgs::msg::JointState. auto joint_states_pub = - node->create_publisher("joint_states", 1000); + node->create_publisher("joint_states", 1); sensor_msgs::msg::JointState joint_states_msg; ros_ign_bridge::testing::createTestMsg(joint_states_msg); // sensor_msgs::msg::LaserScan. auto laserscan_pub = - node->create_publisher("laserscan", 1000); + node->create_publisher("laserscan", 1); sensor_msgs::msg::LaserScan laserscan_msg; ros_ign_bridge::testing::createTestMsg(laserscan_msg); // sensor_msgs::msg::MagneticField. auto magnetic_pub = - node->create_publisher("magnetic", 1000); + node->create_publisher("magnetic", 1); sensor_msgs::msg::MagneticField magnetic_msg; ros_ign_bridge::testing::createTestMsg(magnetic_msg); // sensor_msgs::msg::PointCloud2. auto pointcloud2_pub = - node->create_publisher("pointcloud2", 1000); + node->create_publisher("pointcloud2", 1); sensor_msgs::msg::PointCloud2 pointcloud2_msg; ros_ign_bridge::testing::createTestMsg(pointcloud2_msg); // sensor_msgs::msg::BatteryState. auto battery_state_pub = - node->create_publisher("battery_state", 1000); + node->create_publisher("battery_state", 1); sensor_msgs::msg::BatteryState battery_state_msg; ros_ign_bridge::testing::createTestMsg(battery_state_msg); // trajectory_msgs::msg::JointTrajectory. auto joint_trajectory_pub = - node->create_publisher("joint_trajectory", 1000); + node->create_publisher("joint_trajectory", 1); trajectory_msgs::msg::JointTrajectory joint_trajectory_msg; ros_ign_bridge::testing::createTestMsg(joint_trajectory_msg); diff --git a/ros_ign_bridge/test/subscribers/ros_subscriber/ros_subscriber.cpp b/ros_ign_bridge/test/subscribers/ros_subscriber/ros_subscriber.cpp index 49c4a359..c03606d4 100644 --- a/ros_ign_bridge/test/subscribers/ros_subscriber/ros_subscriber.cpp +++ b/ros_ign_bridge/test/subscribers/ros_subscriber/ros_subscriber.cpp @@ -19,14 +19,15 @@ #include "ros_subscriber.hpp" +static std::shared_ptr kTestNode; + ///////////////////////////////////////////////// -std::shared_ptr ros_subscriber::TestNode() +rclcpp::Node * ros_subscriber::TestNode() { - static std::shared_ptr kTestNode; if (kTestNode == nullptr) { kTestNode = rclcpp::Node::make_shared("ros_subscriber"); } - return kTestNode; + return kTestNode.get(); } ///////////////////////////////////////////////// @@ -34,5 +35,7 @@ int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); - return RUN_ALL_TESTS(); + auto ret = RUN_ALL_TESTS(); + kTestNode.reset(); + return ret; } diff --git a/ros_ign_bridge/test/subscribers/ros_subscriber/ros_subscriber.hpp b/ros_ign_bridge/test/subscribers/ros_subscriber/ros_subscriber.hpp index aa265688..f2809a91 100644 --- a/ros_ign_bridge/test/subscribers/ros_subscriber/ros_subscriber.hpp +++ b/ros_ign_bridge/test/subscribers/ros_subscriber/ros_subscriber.hpp @@ -26,7 +26,7 @@ namespace ros_subscriber { ///////////////////////////////////////////////// /// \brief Retrieve a common node used for testing -std::shared_ptr TestNode(); +rclcpp::Node * TestNode(); ///////////////////////////////////////////////// /// \brief A class for testing ROS topic subscription. @@ -39,7 +39,7 @@ class MyTestClass { using std::placeholders::_1; this->sub = TestNode()->create_subscription( - _topic, 1000, + _topic, 1, std::bind(&MyTestClass::Cb, this, _1)); } diff --git a/ros_ign_bridge/test/utils/test_utils.hpp b/ros_ign_bridge/test/utils/test_utils.hpp index d917f3b2..a5d14a4e 100644 --- a/ros_ign_bridge/test/utils/test_utils.hpp +++ b/ros_ign_bridge/test/utils/test_utils.hpp @@ -59,16 +59,16 @@ void waitUntilBoolVar( /// waitUntilBoolVar(myVar, 1ms, 10); template void waitUntilBoolVarAndSpin( - std::shared_ptr node, + rclcpp::Node * node, bool & _boolVar, const std::chrono::duration & _sleepEach, const int _retries) { int i = 0; - while (!_boolVar && i < _retries) { + while (!_boolVar && i < _retries && rclcpp::ok()) { ++i; std::this_thread::sleep_for(_sleepEach); - rclcpp::spin_some(node); + rclcpp::spin_some(node->get_node_base_interface()); } }