diff --git a/nao_lola_client/CMakeLists.txt b/nao_lola_client/CMakeLists.txt index 0aa921e..fb5859d 100644 --- a/nao_lola_client/CMakeLists.txt +++ b/nao_lola_client/CMakeLists.txt @@ -19,15 +19,18 @@ 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) find_package(Boost COMPONENTS system program_options filesystem REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_components nao_lola_sensor_msgs - nao_lola_command_msgs) + nao_lola_command_msgs + sensor_msgs) # build msgpack_parser_lib add_library(msgpack_parser_lib SHARED @@ -50,12 +53,12 @@ ament_target_dependencies(msgpack_packer_lib # build nao_lola_client_node add_library(nao_lola_client_node SHARED src/nao_lola_client.cpp - src/connection.cpp) - + src/connection.cpp + src/conversion.cpp) target_link_libraries(nao_lola_client_node msgpack_parser_lib msgpack_packer_lib) -ament_target_dependencies(nao_lola_client_node 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/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 b10773d..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 @@ -46,6 +46,7 @@ #include "nao_lola_command_msgs/msg/joint_stiffnesses.hpp" #include "nao_lola_client/connection.hpp" #include "nao_lola_client/msgpack_packer.hpp" +#include "sensor_msgs/msg/joint_state.hpp" class NaoLolaClient : public rclcpp::Node { @@ -56,6 +57,7 @@ class NaoLolaClient : public rclcpp::Node private: void createPublishers(); void createSubscriptions(); + void declareParameters(); rclcpp::Publisher::SharedPtr accelerometer_pub; rclcpp::Publisher::SharedPtr angle_pub; @@ -71,6 +73,7 @@ class NaoLolaClient : public rclcpp::Node rclcpp::Publisher::SharedPtr touch_pub; rclcpp::Publisher::SharedPtr battery_pub; rclcpp::Publisher::SharedPtr robot_config_pub; + rclcpp::Publisher::SharedPtr joint_states_pub; rclcpp::Subscription::SharedPtr joint_positions_sub; rclcpp::Subscription::SharedPtr @@ -90,6 +93,8 @@ class NaoLolaClient : public rclcpp::Node MsgpackPacker packer; std::mutex packer_mutex; + + bool publish_joint_states_; }; #endif // NAO_LOLA_CLIENT__NAO_LOLA_CLIENT_HPP_ diff --git a/nao_lola_client/package.xml b/nao_lola_client/package.xml index 32f0d64..afcba7a 100644 --- a/nao_lola_client/package.xml +++ b/nao_lola_client/package.xml @@ -15,8 +15,10 @@ rclcpp rclcpp_components + rcl_interfaces nao_lola_command_msgs nao_lola_sensor_msgs + sensor_msgs boost 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/conversion.cpp b/nao_lola_client/src/conversion.cpp new file mode 100644 index 0000000..1ddf819 --- /dev/null +++ b/nao_lola_client/src/conversion.cpp @@ -0,0 +1,66 @@ +// Copyright 2023 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 "conversion.hpp" + +#include +#include + +#include "nao_lola_sensor_msgs/msg/joint_indexes.hpp" + +namespace conversion +{ + +static const std::vector joint_names = { + "HeadYaw", + "HeadPitch", + "LShoulderPitch", + "LShoulderRoll", + "LElbowYaw", + "LElbowRoll", + "LWristYaw", + "LHipYawPitch", + "LHipRoll", + "LHipPitch", + "LKneePitch", + "LAnklePitch", + "LAnkleRoll", + "RHipRoll", + "RHipPitch", + "RKneePitch", + "RAnklePitch", + "RAnkleRoll", + "RShoulderPitch", + "RShoulderRoll", + "RElbowYaw", + "RElbowRoll", + "RWristYaw", + "LHand", + "RHand", +}; + +sensor_msgs::msg::JointState toJointState( + const nao_lola_sensor_msgs::msg::JointPositions & joint_positions) +{ + sensor_msgs::msg::JointState joint_state; + joint_state.name = joint_names; + + for (unsigned i = 0; i < nao_lola_sensor_msgs::msg::JointIndexes::NUMJOINTS; ++i) { + joint_state.position.push_back(joint_positions.positions[i]); + } + + return joint_state; +} + +} // namespace conversion diff --git a/nao_lola_client/src/conversion.hpp b/nao_lola_client/src/conversion.hpp new file mode 100644 index 0000000..fda038b --- /dev/null +++ b/nao_lola_client/src/conversion.hpp @@ -0,0 +1,29 @@ +// Copyright 2023 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. + +#ifndef CONVERSION_HPP_ +#define CONVERSION_HPP_ + +#include "nao_lola_sensor_msgs/msg/joint_positions.hpp" +#include "sensor_msgs/msg/joint_state.hpp" + +namespace conversion +{ + +sensor_msgs::msg::JointState toJointState( + const nao_lola_sensor_msgs::msg::JointPositions & joint_positions); + +} // namespace conversion + +#endif // CONVERSION_HPP_ diff --git a/nao_lola_client/src/nao_lola_client.cpp b/nao_lola_client/src/nao_lola_client.cpp index 7276304..706081d 100644 --- a/nao_lola_client/src/nao_lola_client.cpp +++ b/nao_lola_client/src/nao_lola_client.cpp @@ -14,12 +14,19 @@ #include #include + #include "nao_lola_client/nao_lola_client.hpp" #include "nao_lola_client/msgpack_parser.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) { + declareParameters(); createPublishers(); createSubscriptions(); @@ -27,7 +34,14 @@ NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options) receive_thread_ = std::thread( [this]() { while (rclcpp::ok()) { - auto recvData = connection.receive(); + 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()); @@ -45,6 +59,11 @@ NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options) battery_pub->publish(parsed.getBattery()); robot_config_pub->publish(parsed.getRobotConfig()); + 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 @@ -82,6 +101,11 @@ void NaoLolaClient::createPublishers() battery_pub = create_publisher("sensors/battery", 10); robot_config_pub = create_publisher("sensors/robot_config", 10); + + if (publish_joint_states_) { + joint_states_pub = create_publisher("joint_states", 10); + } + RCLCPP_DEBUG(get_logger(), "Finished initialising publishers"); } @@ -189,5 +213,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..f290823 100644 --- a/nao_lola_client/test/CMakeLists.txt +++ b/nao_lola_client/test/CMakeLists.txt @@ -1,15 +1,13 @@ # 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)