From 453b7d62ef84ffa1d5897b19c4f692b2001ae669 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 21 Jan 2024 05:26:06 +0000 Subject: [PATCH] publish imu and joint states Signed-off-by: Kenji Brameld --- CMakeLists.txt | 25 ++++--- include/rcss3d_nao/rcss3d_nao_node.hpp | 7 ++ package.xml | 7 +- src/conversion.cpp | 96 ++++++++++++++++++++++++++ src/conversion.hpp | 36 ++++++++++ src/rcss3d_nao_node.cpp | 30 ++++++++ 6 files changed, 187 insertions(+), 14 deletions(-) create mode 100644 src/conversion.cpp create mode 100644 src/conversion.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ba0599f..e21eb09 100644 --- a/CMakeLists.txt +++ b/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 $ $) diff --git a/include/rcss3d_nao/rcss3d_nao_node.hpp b/include/rcss3d_nao/rcss3d_nao_node.hpp index 4ffe6b7..fb22910 100644 --- a/include/rcss3d_nao/rcss3d_nao_node.hpp +++ b/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::SharedPtr goalpostArrayPub; rclcpp::Publisher::SharedPtr markingArrayPub; rclcpp::Publisher::SharedPtr robotArrayPub; + rclcpp::Publisher::SharedPtr imuPub; + rclcpp::Publisher::SharedPtr jointStatesPub; rclcpp::Subscription::SharedPtr jointPositionsSub; rclcpp::Subscription::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 diff --git a/package.xml b/package.xml index 50fe047..cb5e320 100644 --- a/package.xml +++ b/package.xml @@ -13,13 +13,14 @@ ament_lint_common ament_cmake_gtest - rcss3d_agent - rcss3d_agent_msgs_to_soccer_interfaces + geometry_msgs nao_lola_command_msgs nao_lola_sensor_msgs rclcpp_components + rcss3d_agent + rcss3d_agent_msgs_to_soccer_interfaces + sensor_msgs soccer_vision_3d_msgs - geometry_msgs ament_cmake diff --git a/src/conversion.cpp b/src/conversion.cpp new file mode 100644 index 0000000..dae583b --- /dev/null +++ b/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 +#include + +#include "nao_lola_sensor_msgs/msg/joint_indexes.hpp" + +namespace conversion +{ + +static const std::vector 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 diff --git a/src/conversion.hpp b/src/conversion.hpp new file mode 100644 index 0000000..3da4fa6 --- /dev/null +++ b/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_ diff --git a/src/rcss3d_nao_node.cpp b/src/rcss3d_nao_node.cpp index 7341e8d..48faf39 100644 --- a/src/rcss3d_nao_node.cpp +++ b/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("unum", 0); double x = this->declare_parameter("x", 0.0); double y = this->declare_parameter("y", 0.0); + publishImu = this->declare_parameter("publish_imu", true); + publishJointStates = this->declare_parameter("publish_joint_states", true); // Create Rcss3dAgent params = std::make_unique(model, rcss3d_host, rcss3d_port, team, unum); @@ -65,6 +68,14 @@ Rcss3dNaoNode::Rcss3dNaoNode(const rclcpp::NodeOptions & options) robotArrayPub = create_publisher("soccer_vision_3d/robots", 10); + if (publishImu) { + imuPub = create_publisher("imu", 10); + } + + if (publishJointStates) { + jointStatesPub = create_publisher("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)