From 3ad68f9dc2eb8eb904ad6b0dfc7d7a41359f6635 Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Tue, 25 Jun 2024 16:09:26 +0900 Subject: [PATCH] Revert unintended changes --- neonavigation_common/test/src/test_compat.cpp | 77 +++++++++---------- .../test/test/safety_limiter_rostest.test | 4 +- 2 files changed, 40 insertions(+), 41 deletions(-) diff --git a/neonavigation_common/test/src/test_compat.cpp b/neonavigation_common/test/src/test_compat.cpp index 792570db5..ace302d3d 100644 --- a/neonavigation_common/test/src/test_compat.cpp +++ b/neonavigation_common/test/src/test_compat.cpp @@ -29,9 +29,9 @@ #include -#include "rclcpp/rclcpp.hpp" -#include -#include +#include +#include +#include #include @@ -54,26 +54,26 @@ TEST(NeonavigationCompat, CompatMode) neonavigation_common::compat::current_level = 3; neonavigation_common::compat::default_level = neonavigation_common::compat::supported_level; - rclcpp::Node("/").setParam("neonavigation_compatible", 2); + ros::NodeHandle("/").setParam("neonavigation_compatible", 2); ASSERT_NO_THROW( { neonavigation_common::compat::checkCompatMode(); }); // NOLINT(whitespace/braces) - rclcpp::Node("/").setParam("neonavigation_compatible", 3); + ros::NodeHandle("/").setParam("neonavigation_compatible", 3); ASSERT_NO_THROW( { neonavigation_common::compat::checkCompatMode(); }); // NOLINT(whitespace/braces) - rclcpp::Node("/").setParam("neonavigation_compatible", 4); + ros::NodeHandle("/").setParam("neonavigation_compatible", 4); ASSERT_THROW( { neonavigation_common::compat::checkCompatMode(); }, // NOLINT(whitespace/braces) std::runtime_error); - rclcpp::Node("/").setParam("neonavigation_compatible", 1); + ros::NodeHandle("/").setParam("neonavigation_compatible", 1); ASSERT_THROW( { neonavigation_common::compat::checkCompatMode(); @@ -84,22 +84,22 @@ TEST(NeonavigationCompat, CompatMode) class NeonavigationCompatCallbacks { public: - rclcpp::Node nh_; - rclcpp::Node pnh_; - std_msgs::msg::Bool::ConstSharedPtr msg_; - mutable std_msgs::msg::Bool::ConstSharedPtr msg_const_; + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + std_msgs::Bool::ConstPtr msg_; + mutable std_msgs::Bool::ConstPtr msg_const_; bool srv_called_; - void cb(const std_msgs::msg::Bool::ConstSharedPtr& msg) + void cb(const std_msgs::Bool::ConstPtr& msg) { msg_ = msg; } - void cbConst(const std_msgs::msg::Bool::ConstSharedPtr& msg) const + void cbConst(const std_msgs::Bool::ConstPtr& msg) const { msg_const_ = msg; } - bool cbSrv(std_srvs::srv::Empty::Request& req, std_srvs::srv::Empty::Response& res) + bool cbSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { srv_called_ = true; return true; @@ -120,65 +120,65 @@ TEST(NeonavigationCompat, Subscribe) NeonavigationCompatCallbacks cls; - auto pub_old = cls.pnh_.advertise("test_old", 1, true); - auto pub_new = cls.nh_.advertise("test_new", 1, true); - std_msgs::msg::Bool msg; + ros::Publisher pub_old = cls.pnh_.advertise("test_old", 1, true); + ros::Publisher pub_new = cls.nh_.advertise("test_new", 1, true); + std_msgs::Bool msg; msg.data = false; pub_old.publish(msg); msg.data = true; pub_new.publish(msg); { - rclcpp::Node("/").setParam("neonavigation_compatible", 2); + ros::NodeHandle("/").setParam("neonavigation_compatible", 2); ros::Subscriber sub = neonavigation_common::compat::subscribe( cls.nh_, "test_new", cls.pnh_, "test_old", 1, &NeonavigationCompatCallbacks::cb, &cls); - rclcpp::Duration(0.1).sleep(); - rclcpp::spin_some(node); + ros::Duration(0.1).sleep(); + ros::spinOnce(); ASSERT_TRUE(static_cast(cls.msg_)); ASSERT_EQ(false, static_cast(cls.msg_->data)); } { - rclcpp::Node("/").setParam("neonavigation_compatible", 3); + ros::NodeHandle("/").setParam("neonavigation_compatible", 3); cls.msg_ = nullptr; ros::Subscriber sub = neonavigation_common::compat::subscribe( cls.nh_, "test_new", cls.pnh_, "test_old", 1, &NeonavigationCompatCallbacks::cb, &cls); - rclcpp::Duration(0.1).sleep(); - rclcpp::spin_some(node); + ros::Duration(0.1).sleep(); + ros::spinOnce(); ASSERT_TRUE(static_cast(cls.msg_)); ASSERT_EQ(true, static_cast(cls.msg_->data)); } { - rclcpp::Node("/").setParam("neonavigation_compatible", 2); + ros::NodeHandle("/").setParam("neonavigation_compatible", 2); cls.msg_ = nullptr; ros::Subscriber sub = neonavigation_common::compat::subscribe( cls.nh_, "test_new", cls.pnh_, "test_old", 1, &NeonavigationCompatCallbacks::cbConst, &cls); - rclcpp::Duration(0.1).sleep(); - rclcpp::spin_some(node); + ros::Duration(0.1).sleep(); + ros::spinOnce(); ASSERT_TRUE(static_cast(cls.msg_const_)); ASSERT_EQ(false, static_cast(cls.msg_const_->data)); } { - rclcpp::Node("/").setParam("neonavigation_compatible", 3); + ros::NodeHandle("/").setParam("neonavigation_compatible", 3); cls.msg_ = nullptr; ros::Subscriber sub = neonavigation_common::compat::subscribe( cls.nh_, "test_new", cls.pnh_, "test_old", 1, &NeonavigationCompatCallbacks::cbConst, &cls); - rclcpp::Duration(0.1).sleep(); - rclcpp::spin_some(node); + ros::Duration(0.1).sleep(); + ros::spinOnce(); ASSERT_TRUE(static_cast(cls.msg_const_)); ASSERT_EQ(true, static_cast(cls.msg_const_->data)); } @@ -191,33 +191,33 @@ TEST(NeonavigationCompat, AdvertiseService) neonavigation_common::compat::default_level = neonavigation_common::compat::supported_level; NeonavigationCompatCallbacks cls; - ros::ServiceClient cli_new = cls.nh_.serviceClient("srv_new"); - ros::ServiceClient cli_old = cls.pnh_.serviceClient("srv_old"); + ros::ServiceClient cli_new = cls.nh_.serviceClient("srv_new"); + ros::ServiceClient cli_old = cls.pnh_.serviceClient("srv_old"); ros::AsyncSpinner spinner(1); spinner.start(); { - rclcpp::Node("/").setParam("neonavigation_compatible", 2); + ros::NodeHandle("/").setParam("neonavigation_compatible", 2); ros::ServiceServer srv = neonavigation_common::compat::advertiseService( cls.nh_, "srv_new", cls.pnh_, "srv_old", &NeonavigationCompatCallbacks::cbSrv, &cls); - rclcpp::Duration(0.1).sleep(); - std_srvs::srv::Empty empty; + ros::Duration(0.1).sleep(); + std_srvs::Empty empty; ASSERT_TRUE(cli_old.call(empty.request, empty.response)); } { - rclcpp::Node("/").setParam("neonavigation_compatible", 3); + ros::NodeHandle("/").setParam("neonavigation_compatible", 3); ros::ServiceServer srv = neonavigation_common::compat::advertiseService( cls.nh_, "srv_new", cls.pnh_, "srv_old", &NeonavigationCompatCallbacks::cbSrv, &cls); - rclcpp::Duration(0.1).sleep(); - std_srvs::srv::Empty empty; + ros::Duration(0.1).sleep(); + std_srvs::Empty empty; ASSERT_TRUE(cli_new.call(empty.request, empty.response)); } @@ -227,8 +227,7 @@ TEST(NeonavigationCompat, AdvertiseService) int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("test_compat"); + ros::init(argc, argv, "test_compat"); return RUN_ALL_TESTS(); } diff --git a/safety_limiter/test/test/safety_limiter_rostest.test b/safety_limiter/test/test/safety_limiter_rostest.test index f82d371a8..160bd54c8 100644 --- a/safety_limiter/test/test/safety_limiter_rostest.test +++ b/safety_limiter/test/test/safety_limiter_rostest.test @@ -9,8 +9,8 @@ [[0.0, -0.1], [0.0, 0.1], [-2.0, 0.1], [-2.0, -0.1]] - - + +