From e5f9beda5c23eaf0a8bf3a4ecf21b0084e06a795 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Tue, 5 Dec 2023 01:30:10 +0000 Subject: [PATCH] stash Signed-off-by: Kenji Brameld --- .../include/nao_lola_client/connection.hpp | 5 +- .../nao_lola_client/nao_lola_client.hpp | 6 +- nao_lola_client/src/connection.cpp | 2 +- nao_lola_client/src/nao_lola_client.cpp | 52 +++++- nao_lola_client/test/CMakeLists.txt | 17 +- nao_lola_client/test/test_nao_lola_client.cpp | 176 ++++++++++++++++++ 6 files changed, 242 insertions(+), 16 deletions(-) create mode 100644 nao_lola_client/test/test_nao_lola_client.cpp diff --git a/nao_lola_client/include/nao_lola_client/connection.hpp b/nao_lola_client/include/nao_lola_client/connection.hpp index e2492f3..01848d6 100644 --- a/nao_lola_client/include/nao_lola_client/connection.hpp +++ b/nao_lola_client/include/nao_lola_client/connection.hpp @@ -15,6 +15,7 @@ #ifndef NAO_LOLA_CLIENT__CONNECTION_HPP_ #define NAO_LOLA_CLIENT__CONNECTION_HPP_ +#include #include #include "boost/asio.hpp" #include "rclcpp/logger.hpp" @@ -22,11 +23,13 @@ #define MSGPACK_READ_LENGTH 896 +using RecvData = std::array; + class Connection { public: Connection(); - std::array receive(); + RecvData receive(); void send(std::string data); private: diff --git a/nao_lola_client/include/nao_lola_client/nao_lola_client.hpp b/nao_lola_client/include/nao_lola_client/nao_lola_client.hpp index 5a43848..d553907 100644 --- a/nao_lola_client/include/nao_lola_client/nao_lola_client.hpp +++ b/nao_lola_client/include/nao_lola_client/nao_lola_client.hpp @@ -52,11 +52,12 @@ class NaoLolaClient : public rclcpp::Node { public: explicit NaoLolaClient(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - virtual ~NaoLolaClient() {} + ~NaoLolaClient(); private: void createPublishers(); void createSubscriptions(); + void declareParameters(); rclcpp::Publisher::SharedPtr accelerometer_pub; rclcpp::Publisher::SharedPtr angle_pub; @@ -87,11 +88,14 @@ class NaoLolaClient : public rclcpp::Node rclcpp::Subscription::SharedPtr head_leds_sub; rclcpp::Subscription::SharedPtr sonar_usage_sub; + std::atomic stop_thread_; std::thread receive_thread_; Connection connection; MsgpackPacker packer; std::mutex packer_mutex; + + bool publish_joint_states_; }; #endif // NAO_LOLA_CLIENT__NAO_LOLA_CLIENT_HPP_ diff --git a/nao_lola_client/src/connection.cpp b/nao_lola_client/src/connection.cpp index f84f175..38805b0 100644 --- a/nao_lola_client/src/connection.cpp +++ b/nao_lola_client/src/connection.cpp @@ -38,7 +38,7 @@ std::array Connection::receive() std::array data; socket.receive(boost::asio::buffer(data), 0, ec); if (ec) { - RCLCPP_ERROR(logger, (std::string{"Could not read from LoLA: "} + ec.message()).c_str()); + throw std::runtime_error(std::string{"Could not read from LoLA: "} + ec.message()); } return data; } diff --git a/nao_lola_client/src/nao_lola_client.cpp b/nao_lola_client/src/nao_lola_client.cpp index 9d72b49..188f10e 100644 --- a/nao_lola_client/src/nao_lola_client.cpp +++ b/nao_lola_client/src/nao_lola_client.cpp @@ -17,18 +17,29 @@ #include "nao_lola_client/nao_lola_client.hpp" #include "nao_lola_client/msgpack_parser.hpp" #include "conversion.hpp" +#include "rcl_interfaces/msg/parameter_value.hpp" +#include "rcl_interfaces/msg/parameter_value.hpp" NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options) -: Node("NaoLolaClient", options) +: Node("NaoLolaClient", options), + stop_thread_(false) { + declareParameters(); createPublishers(); createSubscriptions(); // Start receive and send loop receive_thread_ = std::thread( [this]() { - while (rclcpp::ok()) { - auto recvData = connection.receive(); + while (rclcpp::ok() && !stop_thread_) { + RecvData recvData; + try { + recvData = connection.receive(); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 1000, e.what()); + continue; + } + MsgpackParser parsed(recvData.data(), recvData.size()); accelerometer_pub->publish(parsed.getAccelerometer()); @@ -46,9 +57,12 @@ NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options) battery_pub->publish(parsed.getBattery()); robot_config_pub->publish(parsed.getRobotConfig()); - auto joint_state = conversion::toJointState(parsed.getJointPositions()); - joint_state.header.stamp = this->now(); - joint_states_pub->publish(joint_state); + if (publish_joint_states_) + { + auto joint_state = conversion::toJointState(parsed.getJointPositions()); + joint_state.header.stamp = this->now(); + joint_states_pub->publish(joint_state); + } // In mutex, copy packer // Do the pack and send outside mutex to avoid retain lock for a long time @@ -62,6 +76,12 @@ NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options) }); } +NaoLolaClient::~NaoLolaClient() +{ + stop_thread_ = true; + receive_thread_.join(); +} + void NaoLolaClient::createPublishers() { RCLCPP_DEBUG(get_logger(), "Initialise publishers"); @@ -86,7 +106,11 @@ void NaoLolaClient::createPublishers() battery_pub = create_publisher("sensors/battery", 10); robot_config_pub = create_publisher("sensors/robot_config", 10); - joint_states_pub = create_publisher("joint_states", 10); + + if (publish_joint_states_) { + joint_states_pub = create_publisher("joint_states", 10); + } + RCLCPP_DEBUG(get_logger(), "Finished initialising publishers"); } @@ -194,5 +218,19 @@ void NaoLolaClient::createSubscriptions() RCLCPP_DEBUG(get_logger(), "Finished creating subscriptions"); } +void NaoLolaClient::declareParameters() +{ + publish_joint_states_ = declare_parameter( + "publish_joint_states", rclcpp::ParameterValue(true), + rcl_interfaces::msg::ParameterDescriptor() + .set__name("publish_joint_states") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) + .set__description( + "Whether to convert nao_lola sensor_msgs/JointPositions to sensor_msgs/JointState and " + "publish it on topic 'joint_states'") + .set__read_only(true) + ).get(); +} + #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(NaoLolaClient) diff --git a/nao_lola_client/test/CMakeLists.txt b/nao_lola_client/test/CMakeLists.txt index 2fe3f47..71e2638 100644 --- a/nao_lola_client/test/CMakeLists.txt +++ b/nao_lola_client/test/CMakeLists.txt @@ -1,15 +1,20 @@ +# Build test_nao_lola_client +ament_add_gtest(test_nao_lola_client + test_nao_lola_client.cpp) + +target_link_libraries(test_nao_lola_client + nao_lola_client_node) + # Build test_msgpack_parser ament_add_gtest(test_msgpack_parser - test_msgpack_parser.cpp) + test_msgpack_parser.cpp) target_link_libraries(test_msgpack_parser - msgpack_parser_lib -) + msgpack_parser_lib) # Build test_msgpack_packer ament_add_gtest(test_msgpack_packer - test_msgpack_packer.cpp) + test_msgpack_packer.cpp) target_link_libraries(test_msgpack_packer - msgpack_packer_lib -) \ No newline at end of file + msgpack_packer_lib) diff --git a/nao_lola_client/test/test_nao_lola_client.cpp b/nao_lola_client/test/test_nao_lola_client.cpp new file mode 100644 index 0000000..7c1e046 --- /dev/null +++ b/nao_lola_client/test/test_nao_lola_client.cpp @@ -0,0 +1,176 @@ +// Copyright 2021 Kenji Brameld +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "nao_lola_client/nao_lola_client.hpp" + +// class TestNaoLolaClient : public ::testing::Test +// { +// protected: +// static void SetUpTestCase() +// { +// rclcpp::init(0, nullptr); +// } + +// static void TearDownTestCase() +// { +// rclcpp::shutdown(); +// } + +// void test_parameter( +// const std::string & parameter_name, +// const rclcpp::ParameterValue & expected_value) +// { +// ASSERT_TRUE(node.has_parameter(parameter_name)); +// auto parameter = node.get_parameter(parameter_name); +// EXPECT_EQ(parameter.get_type(), expected_value.get_type()); +// EXPECT_EQ(parameter.get_parameter_value(), expected_value); +// } + +// void test_publisher(const std::string & topic_name, const std::string & topic_type) +// { +// auto publishers_info = node.get_publishers_info_by_topic(topic_name); +// ASSERT_EQ(publishers_info.size(), 1); +// EXPECT_EQ(publishers_info[0].topic_type(), topic_type); +// } + +// void test_subscription(const std::string & topic_name, const std::string & topic_type) +// { +// auto subscriptions_info = node.get_subscriptions_info_by_topic(topic_name); +// EXPECT_EQ(subscriptions_info.size(), 1); +// EXPECT_EQ(subscriptions_info[0].topic_type(), topic_type); +// } + +// NaoLolaClient node; +// }; + +// TEST_F(TestNaoLolaClient, TestParameters) +// { +// SCOPED_TRACE("TestParameters"); +// test_parameter("publish_joint_states", rclcpp::ParameterValue{true}); +// } + +// TEST_F(TestNaoLolaClient, TestPublishers) +// { +// SCOPED_TRACE("TestPublishers"); +// test_publisher("sensors/accelerometer", "nao_lola_sensor_msgs/msg/Accelerometer"); +// test_publisher("sensors/angle", "nao_lola_sensor_msgs/msg/Angle"); +// test_publisher("sensors/buttons", "nao_lola_sensor_msgs/msg/Buttons"); +// test_publisher("sensors/fsr", "nao_lola_sensor_msgs/msg/FSR"); +// test_publisher("sensors/gyroscope", "nao_lola_sensor_msgs/msg/Gyroscope"); +// test_publisher("sensors/joint_positions", "nao_lola_sensor_msgs/msg/JointPositions"); +// test_publisher("sensors/joint_stiffnesses", "nao_lola_sensor_msgs/msg/JointStiffnesses"); +// test_publisher("sensors/joint_temperatures", "nao_lola_sensor_msgs/msg/JointTemperatures"); +// test_publisher("sensors/joint_currents", "nao_lola_sensor_msgs/msg/JointCurrents"); +// test_publisher("sensors/joint_statuses", "nao_lola_sensor_msgs/msg/JointStatuses"); +// test_publisher("sensors/sonar", "nao_lola_sensor_msgs/msg/Sonar"); +// test_publisher("sensors/touch", "nao_lola_sensor_msgs/msg/Touch"); +// test_publisher("sensors/battery", "nao_lola_sensor_msgs/msg/Battery"); +// test_publisher("sensors/robot_config", "nao_lola_sensor_msgs/msg/RobotConfig"); +// } + +// TEST_F(TestNaoLolaClient, TestSubscriptions) +// { +// SCOPED_TRACE("TestSubscriptions"); +// test_subscription("effectors/joint_positions", "nao_lola_command_msgs/msg/JointPositions"); +// test_subscription("effectors/joint_stiffnesses", "nao_lola_command_msgs/msg/JointStiffnesses"); +// test_subscription("effectors/chest_led", "nao_lola_command_msgs/msg/ChestLed"); +// test_subscription("effectors/left_ear_leds", "nao_lola_command_msgs/msg/LeftEarLeds"); +// test_subscription("effectors/right_ear_leds", "nao_lola_command_msgs/msg/RightEarLeds"); +// test_subscription("effectors/left_eye_leds", "nao_lola_command_msgs/msg/LeftEyeLeds"); +// test_subscription("effectors/right_eye_leds", "nao_lola_command_msgs/msg/RightEyeLeds"); +// test_subscription("effectors/left_foot_led", "nao_lola_command_msgs/msg/LeftFootLed"); +// test_subscription("effectors/right_foot_led", "nao_lola_command_msgs/msg/RightFootLed"); +// test_subscription("effectors/head_leds", "nao_lola_command_msgs/msg/HeadLeds"); +// test_subscription("effectors/sonar_usage", "nao_lola_command_msgs/msg/SonaUsage"); +// } + +// TEST(TestNaoLolaClient, TestJointStatePublisherDisabled) +// { +// rclcpp::init(0, nullptr); +// auto node = NaoLolaClient( +// rclcpp::NodeOptions().parameter_overrides({{"publish_joint_states", false}})); +// EXPECT_TRUE(node.get_publishers_info_by_topic("joint_states").empty()); +// rclcpp::shutdown(); +// } + + +struct Topic +{ + Topic(const std::string & topic_name, const std::string & topic_type) + : topic_name(topic_name), topic_type(topic_type) {} + std::string topic_name; + std::string topic_type; +}; + +// Test Publishers +class TestPublisher : public ::testing::TestWithParam {}; +TEST_P(TestPublisher, ) { + auto p = GetParam(); + rclcpp::init(0, nullptr); + NaoLolaClient node; + auto publishers_info = node.get_publishers_info_by_topic(p.topic_name); + ASSERT_EQ(publishers_info.size(), 1); + EXPECT_EQ(publishers_info[0].topic_type(), p.topic_type); + rclcpp::shutdown(); +} + +INSTANTIATE_TEST_SUITE_P( + , + TestPublisher, + ::testing::Values( + Topic{"sensors/accelerometer", "nao_lola_sensor_msgs/msg/Accelerometer"}, + Topic{"sensors/angle", "nao_lola_sensor_msgs/msg/Angle"}, + Topic{"sensors/buttons", "nao_lola_sensor_msgs/msg/Buttons"}, + Topic{"sensors/fsr", "nao_lola_sensor_msgs/msg/FSR"}, + Topic{"sensors/gyroscope", "nao_lola_sensor_msgs/msg/Gyroscope"}, + Topic{"sensors/joint_positions", "nao_lola_sensor_msgs/msg/JointPositions"}, + Topic{"sensors/joint_stiffnesses", "nao_lola_sensor_msgs/msg/JointStiffnesses"}, + Topic{"sensors/joint_temperatures", "nao_lola_sensor_msgs/msg/JointTemperatures"}, + Topic{"sensors/joint_currents", "nao_lola_sensor_msgs/msg/JointCurrents"}, + Topic{"sensors/joint_statuses", "nao_lola_sensor_msgs/msg/JointStatuses"}, + Topic{"sensors/sonar", "nao_lola_sensor_msgs/msg/Sonar"}, + Topic{"sensors/touch", "nao_lola_sensor_msgs/msg/Touch"}, + Topic{"sensors/battery", "nao_lola_sensor_msgs/msg/Battery"}, + Topic{"sensors/robot_config", "nao_lola_sensor_msgs/msg/RobotConfig"}) +); + +// Test subscriptions +class TestSubscriptions : public ::testing::TestWithParam {}; +TEST_P(TestSubscriptions, ) { + auto p = GetParam(); + rclcpp::init(0, nullptr); + NaoLolaClient node; + auto subscriptions_info = node.get_subscriptions_info_by_topic(p.topic_name); + ASSERT_EQ(subscriptions_info.size(), 1); + EXPECT_EQ(subscriptions_info[0].topic_type(), p.topic_type); + rclcpp::shutdown(); +} + +INSTANTIATE_TEST_SUITE_P( + , + TestSubscriptions, + ::testing::Values( + Topic{"effectors/joint_positions", "nao_lola_command_msgs/msg/JointPositions"}, + Topic{"effectors/joint_stiffnesses", "nao_lola_command_msgs/msg/JointStiffnesses"}, + Topic{"effectors/chest_led", "nao_lola_command_msgs/msg/ChestLed"}, + Topic{"effectors/left_ear_leds", "nao_lola_command_msgs/msg/LeftEarLeds"}, + Topic{"effectors/right_ear_leds", "nao_lola_command_msgs/msg/RightEarLeds"}, + Topic{"effectors/left_eye_leds", "nao_lola_command_msgs/msg/LeftEyeLeds"}, + Topic{"effectors/right_eye_leds", "nao_lola_command_msgs/msg/RightEyeLeds"}, + Topic{"effectors/left_foot_led", "nao_lola_command_msgs/msg/LeftFootLed"}, + Topic{"effectors/right_foot_led", "nao_lola_command_msgs/msg/RightFootLed"}, + Topic{"effectors/head_leds", "nao_lola_command_msgs/msg/HeadLeds"}, + Topic{"effectors/sonar_usage", "nao_lola_command_msgs/msg/SonarUsage"}) +);