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 b27e5d2..10f141a 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/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::SharedPtr touch_pub; rclcpp::Publisher::SharedPtr battery_pub; rclcpp::Publisher::SharedPtr robot_config_pub; + rclcpp::Publisher::SharedPtr imu_pub; rclcpp::Publisher::SharedPtr joint_states_pub; rclcpp::Subscription::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_; }; diff --git a/nao_lola_client/src/conversion.cpp b/nao_lola_client/src/conversion.cpp index 1ddf819..dae583b 100644 --- a/nao_lola_client/src/conversion.cpp +++ b/nao_lola_client/src/conversion.cpp @@ -50,6 +50,36 @@ static const std::vector 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) { diff --git a/nao_lola_client/src/conversion.hpp b/nao_lola_client/src/conversion.hpp index fda038b..a18fc88 100644 --- a/nao_lola_client/src/conversion.hpp +++ b/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); diff --git a/nao_lola_client/src/nao_lola_client.cpp b/nao_lola_client/src/nao_lola_client.cpp index 706081d..36c2ed8 100644 --- a/nao_lola_client/src/nao_lola_client.cpp +++ b/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("joint_states", 10); } + if (publish_imu_) { + imu_pub = create_publisher("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(); + + 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(); } #include "rclcpp_components/register_node_macro.hpp"