From c8bc8800b614bf6dff231f22bdbb8df6ec254c5a Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Tue, 5 Dec 2023 04:39:15 +0000 Subject: [PATCH] convert nao-specific joint messages to ros-general joint state msg by default, but allow user to disable if not needed through the "publish_joint_states" parameter. Signed-off-by: Kenji Brameld --- nao_lola_client/CMakeLists.txt | 3 +- .../nao_lola_client/nao_lola_client.hpp | 3 +- nao_lola_client/package.xml | 1 + nao_lola_client/src/conversion.cpp | 2 +- nao_lola_client/src/nao_lola_client.cpp | 21 +-- nao_lola_client/test/CMakeLists.txt | 7 - nao_lola_client/test/test_nao_lola_client.cpp | 176 ------------------ 7 files changed, 13 insertions(+), 200 deletions(-) delete mode 100644 nao_lola_client/test/test_nao_lola_client.cpp diff --git a/nao_lola_client/CMakeLists.txt b/nao_lola_client/CMakeLists.txt index 80c7755..fb5859d 100644 --- a/nao_lola_client/CMakeLists.txt +++ b/nao_lola_client/CMakeLists.txt @@ -19,6 +19,7 @@ include_directories( find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(nao_lola_sensor_msgs REQUIRED) find_package(nao_lola_command_msgs REQUIRED) find_package(sensor_msgs REQUIRED) @@ -57,7 +58,7 @@ add_library(nao_lola_client_node SHARED target_link_libraries(nao_lola_client_node msgpack_parser_lib msgpack_packer_lib) -ament_target_dependencies(nao_lola_client_node sensor_msgs rclcpp_components) +ament_target_dependencies(nao_lola_client_node rcl_interfaces sensor_msgs rclcpp_components) rclcpp_components_register_node(nao_lola_client_node PLUGIN "NaoLolaClient" 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 d553907..b27e5d2 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,7 +52,7 @@ class NaoLolaClient : public rclcpp::Node { public: explicit NaoLolaClient(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - ~NaoLolaClient(); + virtual ~NaoLolaClient() {} private: void createPublishers(); @@ -88,7 +88,6 @@ 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; diff --git a/nao_lola_client/package.xml b/nao_lola_client/package.xml index 5a7b126..afcba7a 100644 --- a/nao_lola_client/package.xml +++ b/nao_lola_client/package.xml @@ -15,6 +15,7 @@ rclcpp rclcpp_components + rcl_interfaces nao_lola_command_msgs nao_lola_sensor_msgs sensor_msgs diff --git a/nao_lola_client/src/conversion.cpp b/nao_lola_client/src/conversion.cpp index 67d69b2..1ddf819 100644 --- a/nao_lola_client/src/conversion.cpp +++ b/nao_lola_client/src/conversion.cpp @@ -22,7 +22,7 @@ namespace conversion { -std::vector joint_names = { +static const std::vector joint_names = { "HeadYaw", "HeadPitch", "LShoulderPitch", diff --git a/nao_lola_client/src/nao_lola_client.cpp b/nao_lola_client/src/nao_lola_client.cpp index 188f10e..706081d 100644 --- a/nao_lola_client/src/nao_lola_client.cpp +++ b/nao_lola_client/src/nao_lola_client.cpp @@ -14,15 +14,17 @@ #include #include + #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_descriptor.hpp" +#include "rcl_interfaces/msg/parameter_type.hpp" #include "rcl_interfaces/msg/parameter_value.hpp" +#include "conversion.hpp" + NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options) -: Node("NaoLolaClient", options), - stop_thread_(false) +: Node("NaoLolaClient", options) { declareParameters(); createPublishers(); @@ -31,7 +33,7 @@ NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options) // Start receive and send loop receive_thread_ = std::thread( [this]() { - while (rclcpp::ok() && !stop_thread_) { + while (rclcpp::ok()) { RecvData recvData; try { recvData = connection.receive(); @@ -57,8 +59,7 @@ NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options) battery_pub->publish(parsed.getBattery()); robot_config_pub->publish(parsed.getRobotConfig()); - if (publish_joint_states_) - { + if (publish_joint_states_) { auto joint_state = conversion::toJointState(parsed.getJointPositions()); joint_state.header.stamp = this->now(); joint_states_pub->publish(joint_state); @@ -76,12 +77,6 @@ NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options) }); } -NaoLolaClient::~NaoLolaClient() -{ - stop_thread_ = true; - receive_thread_.join(); -} - void NaoLolaClient::createPublishers() { RCLCPP_DEBUG(get_logger(), "Initialise publishers"); diff --git a/nao_lola_client/test/CMakeLists.txt b/nao_lola_client/test/CMakeLists.txt index 71e2638..f290823 100644 --- a/nao_lola_client/test/CMakeLists.txt +++ b/nao_lola_client/test/CMakeLists.txt @@ -1,10 +1,3 @@ -# 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) diff --git a/nao_lola_client/test/test_nao_lola_client.cpp b/nao_lola_client/test/test_nao_lola_client.cpp deleted file mode 100644 index 7c1e046..0000000 --- a/nao_lola_client/test/test_nao_lola_client.cpp +++ /dev/null @@ -1,176 +0,0 @@ -// 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"}) -);