publish imu and joint states

Signed-off-by: Kenji Brameld <kenjibrameld@gmail.com>

CMakeLists.txt

@@ -11,30 +11,33 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(rcss3d_agent REQUIRED)
-find_package(rcss3d_agent_msgs_to_soccer_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(nao_lola_command_msgs REQUIRED)
 find_package(nao_lola_sensor_msgs REQUIRED)
 find_package(rclcpp_components REQUIRED)
+find_package(rcss3d_agent REQUIRED)
+find_package(rcss3d_agent_msgs_to_soccer_interfaces REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(soccer_vision_3d_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
-  rcss3d_agent
-  rcss3d_agent_msgs_to_soccer_interfaces
+  geometry_msgs
   nao_lola_command_msgs
   nao_lola_sensor_msgs
   rclcpp_components
-  soccer_vision_3d_msgs
-  geometry_msgs)
+  rcss3d_agent
+  rcss3d_agent_msgs_to_soccer_interfaces
+  sensor_msgs
+  soccer_vision_3d_msgs)
 
 # Build
 add_library(${PROJECT_NAME}_node SHARED
-  src/rcss3d_nao_node.cpp
-  src/sim_to_nao.cpp
-  src/nao_to_sim.cpp
   src/complementary_filter.cpp
-  src/nao_joints_pid.cpp)
+  src/conversion.cpp
+  src/nao_joints_pid.cpp
+  src/nao_to_sim.cpp
+  src/rcss3d_nao_node.cpp
+  src/sim_to_nao.cpp)
 target_include_directories(rcss3d_nao_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)

include/rcss3d_nao/rcss3d_nao_node.hpp

@@ -28,6 +28,8 @@
 #include "nao_lola_sensor_msgs/msg/joint_positions.hpp"
 #include "rcss3d_agent_msgs/msg/percept.hpp"
 #include "rcss3d_agent_msgs/msg/beam.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
 #include "soccer_vision_3d_msgs/msg/ball_array.hpp"
 #include "soccer_vision_3d_msgs/msg/goalpost_array.hpp"
 #include "soccer_vision_3d_msgs/msg/marking_array.hpp"
@@ -69,12 +71,17 @@ class Rcss3dNaoNode : public rclcpp::Node
   rclcpp::Publisher<soccer_vision_3d_msgs::msg::GoalpostArray>::SharedPtr goalpostArrayPub;
   rclcpp::Publisher<soccer_vision_3d_msgs::msg::MarkingArray>::SharedPtr markingArrayPub;
   rclcpp::Publisher<soccer_vision_3d_msgs::msg::RobotArray>::SharedPtr robotArrayPub;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imuPub;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr jointStatesPub;
 
   rclcpp::Subscription<nao_lola_command_msgs::msg::JointPositions>::SharedPtr jointPositionsSub;
   rclcpp::Subscription<rcss3d_agent_msgs::msg::Beam>::SharedPtr beamSub;
 
   void perceptCallback(const rcss3d_agent_msgs::msg::Percept & percept);
   void beamToInitialPose(double x, double y, double theta);
+
+  bool publishImu;
+  bool publishJointStates;
 };
 
 }  // namespace rcss3d_nao

package.xml

@@ -13,13 +13,14 @@
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
 
-  <depend>rcss3d_agent</depend>
-  <depend>rcss3d_agent_msgs_to_soccer_interfaces</depend>
+  <depend>geometry_msgs</depend>
   <depend>nao_lola_command_msgs</depend>
   <depend>nao_lola_sensor_msgs</depend>
   <depend>rclcpp_components</depend>
+  <depend>rcss3d_agent</depend>
+  <depend>rcss3d_agent_msgs_to_soccer_interfaces</depend>
+  <depend>sensor_msgs</depend>
   <depend>soccer_vision_3d_msgs</depend>
-  <depend>geometry_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/conversion.cpp

@@ -0,0 +1,96 @@
+// 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::Imu toImu(
+  const nao_lola_sensor_msgs::msg::Accelerometer & accelerometer,
+  const nao_lola_sensor_msgs::msg::Gyroscope & gyroscope)
+{
+  sensor_msgs::msg::Imu imu;
+
+  // Frame id is set to the same as the accelerometer frame id. Technically, there is a very small
+  // difference between the accelerometer and gyroscope frame positions (2mm), but this is
+  // negligible.
+  imu.header.frame_id = "ImuTorsoAccelerometer_frame";
+
+  // Orientation is not available from the robot.
+  // The robot provides an AngleX and AngleY, but there is no easy way to store this in the imu msg.
+  // According to the documentation in sensor_msgs::msg::Imu.msg, if there is no orientation
+  // estimation, the orientation covariance should be set to -1.
+  imu.orientation_covariance[0] = -1;
+
+  // Linear Acceleration
+  imu.linear_acceleration.x = accelerometer.x;
+  imu.linear_acceleration.y = accelerometer.y;
+  imu.linear_acceleration.z = accelerometer.z;
+
+  // Angular Velocity
+  imu.angular_velocity.x = gyroscope.x;
+  imu.angular_velocity.y = gyroscope.y;
+  imu.angular_velocity.z = gyroscope.z;
+
+  return imu;
+}
+
+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

src/conversion.hpp

@@ -0,0 +1,36 @@
+// Copyright 2024 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/accelerometer.hpp"
+#include "nao_lola_sensor_msgs/msg/gyroscope.hpp"
+#include "nao_lola_sensor_msgs/msg/joint_positions.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+
+namespace conversion
+{
+
+sensor_msgs::msg::Imu toImu(
+  const nao_lola_sensor_msgs::msg::Accelerometer & accelerometer,
+  const nao_lola_sensor_msgs::msg::Gyroscope & gyroscope);
+
+sensor_msgs::msg::JointState toJointState(
+  const nao_lola_sensor_msgs::msg::JointPositions & joint_positions);
+
+}  // namespace conversion
+
+#endif  // CONVERSION_HPP_

src/rcss3d_nao_node.cpp

@@ -20,6 +20,7 @@
 #include "sim_to_nao.hpp"
 #include "nao_to_sim.hpp"
 #include "complementary_filter.hpp"
+#include "conversion.hpp"
 #include "nao_joints_pid.hpp"
 
 namespace rcss3d_nao
@@ -40,6 +41,8 @@ Rcss3dNaoNode::Rcss3dNaoNode(const rclcpp::NodeOptions & options)
   int unum = this->declare_parameter<int>("unum", 0);
   double x = this->declare_parameter<double>("x", 0.0);
   double y = this->declare_parameter<double>("y", 0.0);
+  publishImu = this->declare_parameter<bool>("publish_imu", true);
+  publishJointStates = this->declare_parameter<bool>("publish_joint_states", true);
 
   // Create Rcss3dAgent
   params = std::make_unique<rcss3d_agent::Params>(model, rcss3d_host, rcss3d_port, team, unum);
@@ -65,6 +68,14 @@ Rcss3dNaoNode::Rcss3dNaoNode(const rclcpp::NodeOptions & options)
   robotArrayPub =
     create_publisher<soccer_vision_3d_msgs::msg::RobotArray>("soccer_vision_3d/robots", 10);
 
+  if (publishImu) {
+    imuPub = create_publisher<sensor_msgs::msg::Imu>("imu", 10);
+  }
+
+  if (publishJointStates) {
+    jointStatesPub = create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
+  }
+
   // Register callback
   rcss3dAgent->registerPerceptCallback(
     std::bind(&Rcss3dNaoNode::perceptCallback, this, std::placeholders::_1));
@@ -176,6 +187,25 @@ void Rcss3dNaoNode::perceptCallback(const rcss3d_agent_msgs::msg::Percept & perc
     robotArrayPub->publish(
       rcss3d_agent_msgs_to_soccer_interfaces::getRobotArray(vision.players));
   }
+
+  // Get time stamp
+  auto stamp = now();
+
+  // IMU
+  if (publishImu) {
+    if (gyrFound && accFound) {
+      auto imu = conversion::toImu(acc, gyr);
+      imu.header.stamp = stamp;
+      imuPub->publish(imu);
+    }
+  }
+
+  // Joint States
+  if (publishJointStates) {
+    auto jointState = conversion::toJointState(joints);
+    jointState.header.stamp = stamp;
+    jointStatesPub->publish(jointState);
+  }
 }
 
 void Rcss3dNaoNode::beamToInitialPose(double x, double y, double theta)