optionally publish imu

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

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/imu.hpp"
 #include "sensor_msgs/msg/joint_state.hpp"
 
 class NaoLolaClient : public rclcpp::Node
@@ -73,6 +74,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::Imu>::SharedPtr imu_pub;
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_states_pub;
 
  rclcpp::Subscription<nao_lola_command_msgs::msg::JointPositions>::SharedPtr joint_positions_sub;
@@ -94,6 +96,7 @@ class NaoLolaClient : public rclcpp::Node
  MsgpackPacker packer;
  std::mutex packer_mutex;
 
+ bool publish_imu_;
  bool publish_joint_states_;
 };
 

nao_lola_client/src/conversion.cpp

@@ -50,6 +50,36 @@ static const std::vector<std::string> joint_names = {
  "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)
 {

nao_lola_client/src/conversion.hpp

@@ -15,12 +15,19 @@
 #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);
 

nao_lola_client/src/nao_lola_client.cpp

@@ -59,9 +59,17 @@ NaoLolaClient::NaoLolaClient(const rclcpp::NodeOptions & options)
  battery_pub->publish(parsed.getBattery());
  robot_config_pub->publish(parsed.getRobotConfig());
 
+ auto stamp = now();
+
+ if (publish_imu_) {
+ auto imu = conversion::toImu(parsed.getAccelerometer(), parsed.getGyroscope());
+ imu.header.stamp = stamp;
+ imu_pub->publish(imu);
+ }
+
  if (publish_joint_states_) {
  auto joint_state = conversion::toJointState(parsed.getJointPositions());
- joint_state.header.stamp = this->now();
+ joint_state.header.stamp = stamp;
  joint_states_pub->publish(joint_state);
  }
 
@@ -106,6 +114,10 @@ void NaoLolaClient::createPublishers()
  joint_states_pub = create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
  }
 
+ if (publish_imu_) {
+ imu_pub = create_publisher<sensor_msgs::msg::Imu>("imu", 10);
+ }
+
  RCLCPP_DEBUG(get_logger(), "Finished initialising publishers");
 }
 
@@ -225,6 +237,17 @@ void NaoLolaClient::declareParameters()
  "publish it on topic 'joint_states'")
  .set__read_only(true)
  ).get<bool>();
+
+ publish_imu_ = declare_parameter(
+ "publish_imu", rclcpp::ParameterValue(true),
+ rcl_interfaces::msg::ParameterDescriptor()
+ .set__name("publish_imu")
+ .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL)
+ .set__description(
+ "Whether to convert nao_lola sensor_msgs/JointPositions and nao_lola_sensor_msgs/Gyroscope to"
+ " sensor_msgs/Imu and publish it on topic 'imu'")
+ .set__read_only(true)
+ ).get<bool>();
 }
 
 #include "rclcpp_components/register_node_macro.hpp"