Convert nao-specific joint messages to sensor_msgs::JointState and publish on joint_states topic

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"

nao_lola_client/include/nao_lola_client/connection.hpp

@@ -15,18 +15,21 @@
 #ifndef NAO_LOLA_CLIENT__CONNECTION_HPP_
 #define NAO_LOLA_CLIENT__CONNECTION_HPP_
 
+#include <array>
 #include <string>
 #include "boost/asio.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #define MSGPACK_READ_LENGTH 896
 
+using RecvData = std::array<char, MSGPACK_READ_LENGTH>;
+
 class Connection
 {
 public:
   Connection();
-  std::array<char, MSGPACK_READ_LENGTH> receive();
+  RecvData receive();
   void send(std::string data);
 
 private:

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<nao_lola_sensor_msgs::msg::Accelerometer>::SharedPtr accelerometer_pub;
   rclcpp::Publisher<nao_lola_sensor_msgs::msg::Angle>::SharedPtr angle_pub;
@@ -71,6 +73,7 @@ class NaoLolaClient : public rclcpp::Node
   rclcpp::Publisher<nao_lola_sensor_msgs::msg::Touch>::SharedPtr touch_pub;
   rclcpp::Publisher<nao_lola_sensor_msgs::msg::Battery>::SharedPtr battery_pub;
   rclcpp::Publisher<nao_lola_sensor_msgs::msg::RobotConfig>::SharedPtr robot_config_pub;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_states_pub;
 
   rclcpp::Subscription<nao_lola_command_msgs::msg::JointPositions>::SharedPtr joint_positions_sub;
   rclcpp::Subscription<nao_lola_command_msgs::msg::JointStiffnesses>::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_

nao_lola_client/package.xml

@@ -15,8 +15,10 @@
 
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>rcl_interfaces</depend>
   <depend>nao_lola_command_msgs</depend>
   <depend>nao_lola_sensor_msgs</depend>
+  <depend>sensor_msgs</depend>
   <depend>boost</depend>
 
   <export>

nao_lola_client/src/connection.cpp

@@ -38,7 +38,7 @@ std::array<char, MSGPACK_READ_LENGTH> Connection::receive()
   std::array<char, MSGPACK_READ_LENGTH> 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;
 }

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 <string>
+#include <vector>
+
+#include "nao_lola_sensor_msgs/msg/joint_indexes.hpp"
+
+namespace conversion
+{
+
+static const std::vector<std::string> 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

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_

nao_lola_client/src/nao_lola_client.cpp

@@ -14,20 +14,34 @@
 
 #include <string>
 #include <memory>
+
 #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();
 
   // Start receive and send loop
   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<nao_lola_sensor_msgs::msg::Battery>("sensors/battery", 10);
   robot_config_pub =
     create_publisher<nao_lola_sensor_msgs::msg::RobotConfig>("sensors/robot_config", 10);
+
+  if (publish_joint_states_) {
+    joint_states_pub = create_publisher<sensor_msgs::msg::JointState>("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<bool>();
+}
+
 #include "rclcpp_components/register_node_macro.hpp"
 RCLCPP_COMPONENTS_REGISTER_NODE(NaoLolaClient)

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
-)
+  msgpack_packer_lib)