From fe48290f1b33a11aa260bcb7f22e15ffe587cdf9 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 11 Dec 2023 02:43:22 +0000 Subject: [PATCH] optimize header includes from rclcpp and msgpack that were taking long Signed-off-by: Kenji Brameld --- .../include/nao_lola_client/connection.hpp | 1 - .../include/nao_lola_client/msgpack_parser.hpp | 2 +- .../include/nao_lola_client/nao_lola_client.hpp | 4 +++- nao_lola_client/src/connection.cpp | 7 +++++-- nao_lola_client/src/msgpack_packer.cpp | 11 +++++++++-- nao_lola_client/src/msgpack_parser.cpp | 6 ++++++ nao_lola_client/src/nao_lola_client.cpp | 2 +- nao_lola_client/test/test_msgpack_packer.cpp | 9 ++++++++- nao_lola_client/test/test_msgpack_parser.cpp | 8 ++++++++ 9 files changed, 41 insertions(+), 9 deletions(-) diff --git a/nao_lola_client/include/nao_lola_client/connection.hpp b/nao_lola_client/include/nao_lola_client/connection.hpp index 01848d6..177ea5b 100644 --- a/nao_lola_client/include/nao_lola_client/connection.hpp +++ b/nao_lola_client/include/nao_lola_client/connection.hpp @@ -19,7 +19,6 @@ #include #include "boost/asio.hpp" #include "rclcpp/logger.hpp" -#include "rclcpp/rclcpp.hpp" #define MSGPACK_READ_LENGTH 896 diff --git a/nao_lola_client/include/nao_lola_client/msgpack_parser.hpp b/nao_lola_client/include/nao_lola_client/msgpack_parser.hpp index 2d15c5a..37fc5a4 100644 --- a/nao_lola_client/include/nao_lola_client/msgpack_parser.hpp +++ b/nao_lola_client/include/nao_lola_client/msgpack_parser.hpp @@ -18,7 +18,7 @@ #include #include #include -#include "msgpack.hpp" +#include "msgpack/object.hpp" #include "nao_lola_sensor_msgs/msg/joint_currents.hpp" #include "nao_lola_sensor_msgs/msg/joint_positions.hpp" #include "nao_lola_sensor_msgs/msg/joint_stiffnesses.hpp" 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 10f141a..51a6bf6 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 @@ -18,7 +18,9 @@ #include #include #include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/subscription.hpp" #include "nao_lola_sensor_msgs/msg/joint_positions.hpp" #include "nao_lola_sensor_msgs/msg/joint_stiffnesses.hpp" #include "nao_lola_sensor_msgs/msg/joint_temperatures.hpp" diff --git a/nao_lola_client/src/connection.cpp b/nao_lola_client/src/connection.cpp index 38805b0..a0dfe1d 100644 --- a/nao_lola_client/src/connection.cpp +++ b/nao_lola_client/src/connection.cpp @@ -14,6 +14,9 @@ #include #include "nao_lola_client/connection.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/utilities.hpp" #define ENDPOINT "/tmp/robocup" @@ -25,9 +28,9 @@ Connection::Connection() do { socket.connect(ENDPOINT, ec); if (ec) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( logger, clock, 1000, - (std::string{"Could not connect to LoLA, retrying: "} + ec.message()).c_str()); + "Could not connect to LoLA, retrying: " << ec.message()); } } while (ec && rclcpp::ok()); } diff --git a/nao_lola_client/src/msgpack_packer.cpp b/nao_lola_client/src/msgpack_packer.cpp index c605ec4..2d1812b 100644 --- a/nao_lola_client/src/msgpack_packer.cpp +++ b/nao_lola_client/src/msgpack_packer.cpp @@ -18,9 +18,16 @@ #include #include #include "nao_lola_client/msgpack_packer.hpp" -#include "msgpack.hpp" +#include "msgpack/object.hpp" +#include "msgpack/pack.hpp" +#include "msgpack/adaptor/bool.hpp" +#include "msgpack/adaptor/cpp11/array.hpp" +#include "msgpack/adaptor/float.hpp" +#include "msgpack/adaptor/map.hpp" +#include "msgpack/adaptor/string.hpp" +#include "msgpack/zone.hpp" #include "nao_lola_client/command_index_conversion.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logging.hpp" std::string MsgpackPacker::getPacked() { diff --git a/nao_lola_client/src/msgpack_parser.cpp b/nao_lola_client/src/msgpack_parser.cpp index 5e11908..8e90ea8 100644 --- a/nao_lola_client/src/msgpack_parser.cpp +++ b/nao_lola_client/src/msgpack_parser.cpp @@ -15,6 +15,12 @@ #include #include #include +#include "msgpack/object.hpp" +#include "msgpack/adaptor/float.hpp" +#include "msgpack/adaptor/map.hpp" +#include "msgpack/adaptor/string.hpp" +#include "msgpack/adaptor/vector.hpp" +#include "msgpack/unpack.hpp" #include "nao_lola_client/msgpack_parser.hpp" #include "nao_lola_client/lola_enums.hpp" #include "nao_lola_client/sensor_index_conversion.hpp" diff --git a/nao_lola_client/src/nao_lola_client.cpp b/nao_lola_client/src/nao_lola_client.cpp index 36c2ed8..758710a 100644 --- a/nao_lola_client/src/nao_lola_client.cpp +++ b/nao_lola_client/src/nao_lola_client.cpp @@ -38,7 +38,7 @@ NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options) try { recvData = connection.receive(); } catch (const std::runtime_error & e) { - RCLCPP_ERROR_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 1000, e.what()); + RCLCPP_ERROR_STREAM_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 1000, e.what()); continue; } diff --git a/nao_lola_client/test/test_msgpack_packer.cpp b/nao_lola_client/test/test_msgpack_packer.cpp index a6a778f..9bba663 100644 --- a/nao_lola_client/test/test_msgpack_packer.cpp +++ b/nao_lola_client/test/test_msgpack_packer.cpp @@ -17,7 +17,14 @@ #include #include #include -#include "msgpack.hpp" +#include "msgpack/object.hpp" +#include "msgpack/adaptor/bool.hpp" +#include "msgpack/adaptor/float.hpp" +#include "msgpack/adaptor/map.hpp" +#include "msgpack/adaptor/string.hpp" +#include "msgpack/adaptor/vector.hpp" +#include "msgpack/adaptor/vector_bool.hpp" +#include "msgpack/unpack.hpp" #include "nao_lola_client/msgpack_packer.hpp" #include "nao_lola_command_msgs/msg/joint_positions.hpp" #include "nao_lola_command_msgs/msg/joint_stiffnesses.hpp" diff --git a/nao_lola_client/test/test_msgpack_parser.cpp b/nao_lola_client/test/test_msgpack_parser.cpp index 8149879..be8b74f 100644 --- a/nao_lola_client/test/test_msgpack_parser.cpp +++ b/nao_lola_client/test/test_msgpack_parser.cpp @@ -17,6 +17,14 @@ #include #include #include +#include "msgpack/object.hpp" +#include "msgpack/adaptor/float.hpp" +#include "msgpack/adaptor/int.hpp" +#include "msgpack/adaptor/vector.hpp" +#include "msgpack/adaptor/map.hpp" +#include "msgpack/adaptor/string.hpp" +#include "msgpack/pack.hpp" +#include "msgpack/zone.hpp" #include "nao_lola_client/msgpack_parser.hpp" #include "nao_lola_sensor_msgs/msg/joint_indexes.hpp"