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 <kenjibrameld@gmail.com>

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"

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<nao_lola_command_msgs::msg::HeadLeds>::SharedPtr head_leds_sub;
  rclcpp::Subscription<nao_lola_command_msgs::msg::SonarUsage>::SharedPtr sonar_usage_sub;
 
- std::atomic<bool> stop_thread_;
  std::thread receive_thread_;
  Connection connection;
 

nao_lola_client/package.xml

@@ -15,6 +15,7 @@
 
  <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>

nao_lola_client/src/conversion.cpp

@@ -22,7 +22,7 @@
 namespace conversion
 {
 
-std::vector<std::string> joint_names = {
+static const std::vector<std::string> joint_names = {
  "HeadYaw",
  "HeadPitch",
  "LShoulderPitch",

nao_lola_client/src/nao_lola_client.cpp

@@ -14,15 +14,17 @@
 
 #include <string>
 #include <memory>
+
 #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");

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)