diff --git a/.gitmodules b/.gitmodules index 1243abf..346279d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,8 @@ -[submodule "external/msgpack-c"] - path = external/msgpack-c +[submodule "nao_lola_client/external/msgpack-c"] + path = nao_lola_client/external/msgpack-c + url = https://github.com/msgpack/msgpack-c.git + branch = cpp_master +[submodule "nao_lola/external/msgpack-c"] + path = nao_lola/external/msgpack-c url = https://github.com/msgpack/msgpack-c.git branch = cpp_master diff --git a/external/msgpack-c b/external/msgpack-c deleted file mode 160000 index be4d971..0000000 --- a/external/msgpack-c +++ /dev/null @@ -1 +0,0 @@ -Subproject commit be4d971c62798eb59f8455dc77a4529748bcd08f diff --git a/CHANGELOG.rst b/nao_lola/CHANGELOG.rst similarity index 100% rename from CHANGELOG.rst rename to nao_lola/CHANGELOG.rst diff --git a/CMakeLists.txt b/nao_lola/CMakeLists.txt similarity index 100% rename from CMakeLists.txt rename to nao_lola/CMakeLists.txt diff --git a/external/AMENT_IGNORE b/nao_lola/external/AMENT_IGNORE similarity index 100% rename from external/AMENT_IGNORE rename to nao_lola/external/AMENT_IGNORE diff --git a/nao_lola/external/msgpack-c b/nao_lola/external/msgpack-c new file mode 160000 index 0000000..8c602e8 --- /dev/null +++ b/nao_lola/external/msgpack-c @@ -0,0 +1 @@ +Subproject commit 8c602e8579c7e7d65d6f9c6703c9699db3fb0488 diff --git a/include/nao_lola/command_index_conversion.hpp b/nao_lola/include/nao_lola/command_index_conversion.hpp similarity index 100% rename from include/nao_lola/command_index_conversion.hpp rename to nao_lola/include/nao_lola/command_index_conversion.hpp diff --git a/include/nao_lola/connection.hpp b/nao_lola/include/nao_lola/connection.hpp similarity index 100% rename from include/nao_lola/connection.hpp rename to nao_lola/include/nao_lola/connection.hpp diff --git a/include/nao_lola/index_conversion.hpp b/nao_lola/include/nao_lola/index_conversion.hpp similarity index 100% rename from include/nao_lola/index_conversion.hpp rename to nao_lola/include/nao_lola/index_conversion.hpp diff --git a/include/nao_lola/lola_enums.hpp b/nao_lola/include/nao_lola/lola_enums.hpp similarity index 100% rename from include/nao_lola/lola_enums.hpp rename to nao_lola/include/nao_lola/lola_enums.hpp diff --git a/include/nao_lola/msgpack_packer.hpp b/nao_lola/include/nao_lola/msgpack_packer.hpp similarity index 100% rename from include/nao_lola/msgpack_packer.hpp rename to nao_lola/include/nao_lola/msgpack_packer.hpp diff --git a/include/nao_lola/msgpack_parser.hpp b/nao_lola/include/nao_lola/msgpack_parser.hpp similarity index 100% rename from include/nao_lola/msgpack_parser.hpp rename to nao_lola/include/nao_lola/msgpack_parser.hpp diff --git a/include/nao_lola/nao_lola.hpp b/nao_lola/include/nao_lola/nao_lola.hpp similarity index 100% rename from include/nao_lola/nao_lola.hpp rename to nao_lola/include/nao_lola/nao_lola.hpp diff --git a/include/nao_lola/sensor_index_conversion.hpp b/nao_lola/include/nao_lola/sensor_index_conversion.hpp similarity index 100% rename from include/nao_lola/sensor_index_conversion.hpp rename to nao_lola/include/nao_lola/sensor_index_conversion.hpp diff --git a/package.xml b/nao_lola/package.xml similarity index 77% rename from package.xml rename to nao_lola/package.xml index 5d0c9e6..dfe4534 100644 --- a/package.xml +++ b/nao_lola/package.xml @@ -3,7 +3,7 @@ nao_lola 0.3.1 - Packages that allow communicating with the NAO’s Lola middle-ware. + Packages that allow communicating with the NAO's Lola middle-ware. ijnek Apache License 2.0 @@ -20,5 +20,8 @@ ament_cmake + + Please use nao_lola_client. nao_lola will be removed in K-turtle. + diff --git a/src/connection.cpp b/nao_lola/src/connection.cpp similarity index 100% rename from src/connection.cpp rename to nao_lola/src/connection.cpp diff --git a/src/msgpack_packer.cpp b/nao_lola/src/msgpack_packer.cpp similarity index 100% rename from src/msgpack_packer.cpp rename to nao_lola/src/msgpack_packer.cpp diff --git a/src/msgpack_parser.cpp b/nao_lola/src/msgpack_parser.cpp similarity index 100% rename from src/msgpack_parser.cpp rename to nao_lola/src/msgpack_parser.cpp diff --git a/src/nao_lola.cpp b/nao_lola/src/nao_lola.cpp similarity index 100% rename from src/nao_lola.cpp rename to nao_lola/src/nao_lola.cpp diff --git a/src/nao_lola_node.cpp b/nao_lola/src/nao_lola_node.cpp similarity index 100% rename from src/nao_lola_node.cpp rename to nao_lola/src/nao_lola_node.cpp diff --git a/test/CMakeLists.txt b/nao_lola/test/CMakeLists.txt similarity index 100% rename from test/CMakeLists.txt rename to nao_lola/test/CMakeLists.txt diff --git a/test/test_msgpack_packer.cpp b/nao_lola/test/test_msgpack_packer.cpp similarity index 100% rename from test/test_msgpack_packer.cpp rename to nao_lola/test/test_msgpack_packer.cpp diff --git a/test/test_msgpack_parser.cpp b/nao_lola/test/test_msgpack_parser.cpp similarity index 100% rename from test/test_msgpack_parser.cpp rename to nao_lola/test/test_msgpack_parser.cpp diff --git a/nao_lola_client/CMakeLists.txt b/nao_lola_client/CMakeLists.txt new file mode 100644 index 0000000..88de231 --- /dev/null +++ b/nao_lola_client/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.8) +project(nao_lola_client) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include_directories( + include + external/msgpack-c/include +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nao_lola_sensor_msgs REQUIRED) +find_package(nao_lola_command_msgs REQUIRED) +find_package(Boost COMPONENTS system program_options filesystem REQUIRED) + +# build msgpack_parser_lib +add_library(msgpack_parser_lib SHARED + src/msgpack_parser.cpp) + +ament_target_dependencies(msgpack_parser_lib + rclcpp + nao_lola_sensor_msgs + Boost) + +# build msgpack_packer_lib +add_library(msgpack_packer_lib SHARED + src/msgpack_packer.cpp) + +ament_target_dependencies(msgpack_packer_lib + rclcpp + nao_lola_command_msgs + Boost) + +# build nao_lola_client +add_executable(nao_lola_client + src/nao_lola_client_node.cpp + src/nao_lola_client.cpp + src/connection.cpp) + +target_link_libraries(nao_lola_client + msgpack_parser_lib + msgpack_packer_lib) + +# Install +install(TARGETS + nao_lola_client + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install( + TARGETS msgpack_parser_lib msgpack_packer_lib + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/nao_lola_client/external/AMENT_IGNORE b/nao_lola_client/external/AMENT_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/nao_lola_client/external/msgpack-c b/nao_lola_client/external/msgpack-c new file mode 160000 index 0000000..8c602e8 --- /dev/null +++ b/nao_lola_client/external/msgpack-c @@ -0,0 +1 @@ +Subproject commit 8c602e8579c7e7d65d6f9c6703c9699db3fb0488 diff --git a/nao_lola_client/include/nao_lola_client/command_index_conversion.hpp b/nao_lola_client/include/nao_lola_client/command_index_conversion.hpp new file mode 100644 index 0000000..08d0f48 --- /dev/null +++ b/nao_lola_client/include/nao_lola_client/command_index_conversion.hpp @@ -0,0 +1,146 @@ +// Copyright 2021 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 NAO_LOLA_CLIENT__COMMAND_INDEX_CONVERSION_HPP_ +#define NAO_LOLA_CLIENT__COMMAND_INDEX_CONVERSION_HPP_ + +#include +#include "nao_lola_client/lola_enums.hpp" +#include "nao_lola_command_msgs/msg/joint_indexes.hpp" +#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" +#include "nao_lola_command_msgs/msg/right_ear_leds.hpp" +#include "nao_lola_command_msgs/msg/left_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/head_leds.hpp" + +namespace IndexConversion +{ +std::map flip(std::map in); + +static const std::map joint_lola_to_msg = { + {LolaEnums::Joint::HeadYaw, nao_lola_command_msgs::msg::JointIndexes::HEADYAW}, + {LolaEnums::Joint::HeadPitch, nao_lola_command_msgs::msg::JointIndexes::HEADPITCH}, + {LolaEnums::Joint::LShoulderPitch, nao_lola_command_msgs::msg::JointIndexes::LSHOULDERPITCH}, + {LolaEnums::Joint::LShoulderRoll, nao_lola_command_msgs::msg::JointIndexes::LSHOULDERROLL}, + {LolaEnums::Joint::LElbowYaw, nao_lola_command_msgs::msg::JointIndexes::LELBOWYAW}, + {LolaEnums::Joint::LElbowRoll, nao_lola_command_msgs::msg::JointIndexes::LELBOWROLL}, + {LolaEnums::Joint::LWristYaw, nao_lola_command_msgs::msg::JointIndexes::LWRISTYAW}, + {LolaEnums::Joint::LHipYawPitch, nao_lola_command_msgs::msg::JointIndexes::LHIPYAWPITCH}, + {LolaEnums::Joint::LHipRoll, nao_lola_command_msgs::msg::JointIndexes::LHIPROLL}, + {LolaEnums::Joint::LHipPitch, nao_lola_command_msgs::msg::JointIndexes::LHIPPITCH}, + {LolaEnums::Joint::LKneePitch, nao_lola_command_msgs::msg::JointIndexes::LKNEEPITCH}, + {LolaEnums::Joint::LAnklePitch, nao_lola_command_msgs::msg::JointIndexes::LANKLEPITCH}, + {LolaEnums::Joint::LAnkleRoll, nao_lola_command_msgs::msg::JointIndexes::LANKLEROLL}, + {LolaEnums::Joint::RHipRoll, nao_lola_command_msgs::msg::JointIndexes::RHIPROLL}, + {LolaEnums::Joint::RHipPitch, nao_lola_command_msgs::msg::JointIndexes::RHIPPITCH}, + {LolaEnums::Joint::RKneePitch, nao_lola_command_msgs::msg::JointIndexes::RKNEEPITCH}, + {LolaEnums::Joint::RAnklePitch, nao_lola_command_msgs::msg::JointIndexes::RANKLEPITCH}, + {LolaEnums::Joint::RAnkleRoll, nao_lola_command_msgs::msg::JointIndexes::RANKLEROLL}, + {LolaEnums::Joint::RShoulderPitch, nao_lola_command_msgs::msg::JointIndexes::RSHOULDERPITCH}, + {LolaEnums::Joint::RShoulderRoll, nao_lola_command_msgs::msg::JointIndexes::RSHOULDERROLL}, + {LolaEnums::Joint::RElbowYaw, nao_lola_command_msgs::msg::JointIndexes::RELBOWYAW}, + {LolaEnums::Joint::RElbowRoll, nao_lola_command_msgs::msg::JointIndexes::RELBOWROLL}, + {LolaEnums::Joint::RWristYaw, nao_lola_command_msgs::msg::JointIndexes::RWRISTYAW}, + {LolaEnums::Joint::LHand, nao_lola_command_msgs::msg::JointIndexes::LHAND}, + {LolaEnums::Joint::RHand, nao_lola_command_msgs::msg::JointIndexes::RHAND}, +}; + +static const std::map joint_msg_to_lola = flip(joint_lola_to_msg); + +std::map flip(std::map in) +{ + std::map flipped; + for (std::map::iterator i = in.begin(); i != in.end(); ++i) { + flipped[i->second] = i->first; + } + + return flipped; +} + +// See http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#left-ear +static const std::map left_ear_leds_msg_to_lola +{ + {nao_lola_command_msgs::msg::LeftEarLeds::L0, LolaEnums::LeftEarLeds::Deg_0}, + {nao_lola_command_msgs::msg::LeftEarLeds::L1, LolaEnums::LeftEarLeds::Deg_36}, + {nao_lola_command_msgs::msg::LeftEarLeds::L2, LolaEnums::LeftEarLeds::Deg_72}, + {nao_lola_command_msgs::msg::LeftEarLeds::L3, LolaEnums::LeftEarLeds::Deg_108}, + {nao_lola_command_msgs::msg::LeftEarLeds::L4, LolaEnums::LeftEarLeds::Deg_144}, + {nao_lola_command_msgs::msg::LeftEarLeds::L5, LolaEnums::LeftEarLeds::Deg_180}, + {nao_lola_command_msgs::msg::LeftEarLeds::L6, LolaEnums::LeftEarLeds::Deg_216}, + {nao_lola_command_msgs::msg::LeftEarLeds::L7, LolaEnums::LeftEarLeds::Deg_252}, + {nao_lola_command_msgs::msg::LeftEarLeds::L8, LolaEnums::LeftEarLeds::Deg_288}, + {nao_lola_command_msgs::msg::LeftEarLeds::L9, LolaEnums::LeftEarLeds::Deg_324} +}; + +// See http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#right-ear +static const std::map right_ear_leds_msg_to_lola +{ + {nao_lola_command_msgs::msg::RightEarLeds::R0, LolaEnums::RightEarLeds::Deg_0}, + {nao_lola_command_msgs::msg::RightEarLeds::R1, LolaEnums::RightEarLeds::Deg_36}, + {nao_lola_command_msgs::msg::RightEarLeds::R2, LolaEnums::RightEarLeds::Deg_72}, + {nao_lola_command_msgs::msg::RightEarLeds::R3, LolaEnums::RightEarLeds::Deg_108}, + {nao_lola_command_msgs::msg::RightEarLeds::R4, LolaEnums::RightEarLeds::Deg_144}, + {nao_lola_command_msgs::msg::RightEarLeds::R5, LolaEnums::RightEarLeds::Deg_180}, + {nao_lola_command_msgs::msg::RightEarLeds::R6, LolaEnums::RightEarLeds::Deg_216}, + {nao_lola_command_msgs::msg::RightEarLeds::R7, LolaEnums::RightEarLeds::Deg_252}, + {nao_lola_command_msgs::msg::RightEarLeds::R8, LolaEnums::RightEarLeds::Deg_288}, + {nao_lola_command_msgs::msg::RightEarLeds::R9, LolaEnums::RightEarLeds::Deg_324} +}; + +// See http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#nao-v5-v4-and-v3-3 +static const std::map left_eye_leds_msg_to_lola +{ + {nao_lola_command_msgs::msg::LeftEyeLeds::L0, LolaEnums::LeftEyeLeds::Deg_45}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L1, LolaEnums::LeftEyeLeds::Deg_0}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L2, LolaEnums::LeftEyeLeds::Deg_315}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L3, LolaEnums::LeftEyeLeds::Deg_270}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L4, LolaEnums::LeftEyeLeds::Deg_225}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L5, LolaEnums::LeftEyeLeds::Deg_180}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L6, LolaEnums::LeftEyeLeds::Deg_135}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L7, LolaEnums::LeftEyeLeds::Deg_90}, +}; + +// See http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#nao-v5-v4-and-v3-3 +static const std::map right_eye_leds_msg_to_lola +{ + {nao_lola_command_msgs::msg::RightEyeLeds::R0, LolaEnums::RightEyeLeds::Deg_315}, + {nao_lola_command_msgs::msg::RightEyeLeds::R1, LolaEnums::RightEyeLeds::Deg_270}, + {nao_lola_command_msgs::msg::RightEyeLeds::R2, LolaEnums::RightEyeLeds::Deg_225}, + {nao_lola_command_msgs::msg::RightEyeLeds::R3, LolaEnums::RightEyeLeds::Deg_180}, + {nao_lola_command_msgs::msg::RightEyeLeds::R4, LolaEnums::RightEyeLeds::Deg_135}, + {nao_lola_command_msgs::msg::RightEyeLeds::R5, LolaEnums::RightEyeLeds::Deg_90}, + {nao_lola_command_msgs::msg::RightEyeLeds::R6, LolaEnums::RightEyeLeds::Deg_45}, + {nao_lola_command_msgs::msg::RightEyeLeds::R7, LolaEnums::RightEyeLeds::Deg_0}, +}; + +// See http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#head-tactile-sensor-led-locations +static const std::map head_leds_msg_to_lola +{ + {nao_lola_command_msgs::msg::HeadLeds::B0, LolaEnums::SkullLeds::Front_Right_1}, + {nao_lola_command_msgs::msg::HeadLeds::B1, LolaEnums::SkullLeds::Front_Right_0}, + {nao_lola_command_msgs::msg::HeadLeds::B2, LolaEnums::SkullLeds::Middle_Right_0}, + {nao_lola_command_msgs::msg::HeadLeds::B3, LolaEnums::SkullLeds::Rear_Right_0}, + {nao_lola_command_msgs::msg::HeadLeds::B4, LolaEnums::SkullLeds::Rear_Right_1}, + {nao_lola_command_msgs::msg::HeadLeds::B5, LolaEnums::SkullLeds::Rear_Right_2}, + {nao_lola_command_msgs::msg::HeadLeds::B6, LolaEnums::SkullLeds::Rear_Left_2}, + {nao_lola_command_msgs::msg::HeadLeds::B7, LolaEnums::SkullLeds::Rear_Left_1}, + {nao_lola_command_msgs::msg::HeadLeds::B8, LolaEnums::SkullLeds::Rear_Left_0}, + {nao_lola_command_msgs::msg::HeadLeds::B9, LolaEnums::SkullLeds::Middle_Left_0}, + {nao_lola_command_msgs::msg::HeadLeds::B10, LolaEnums::SkullLeds::Front_Left_0}, + {nao_lola_command_msgs::msg::HeadLeds::B11, LolaEnums::SkullLeds::Front_Left_1}, +}; + +} // namespace IndexConversion + +#endif // NAO_LOLA_CLIENT__COMMAND_INDEX_CONVERSION_HPP_ diff --git a/nao_lola_client/include/nao_lola_client/connection.hpp b/nao_lola_client/include/nao_lola_client/connection.hpp new file mode 100644 index 0000000..e2492f3 --- /dev/null +++ b/nao_lola_client/include/nao_lola_client/connection.hpp @@ -0,0 +1,38 @@ +// Copyright 2021 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 NAO_LOLA_CLIENT__CONNECTION_HPP_ +#define NAO_LOLA_CLIENT__CONNECTION_HPP_ + +#include +#include "boost/asio.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/rclcpp.hpp" + +#define MSGPACK_READ_LENGTH 896 + +class Connection +{ +public: + Connection(); + std::array receive(); + void send(std::string data); + +private: + boost::asio::io_service io_service; + boost::asio::local::stream_protocol::socket socket; + rclcpp::Logger logger; +}; + +#endif // NAO_LOLA_CLIENT__CONNECTION_HPP_ diff --git a/nao_lola_client/include/nao_lola_client/index_conversion.hpp b/nao_lola_client/include/nao_lola_client/index_conversion.hpp new file mode 100644 index 0000000..8b2c86a --- /dev/null +++ b/nao_lola_client/include/nao_lola_client/index_conversion.hpp @@ -0,0 +1,146 @@ +// Copyright 2021 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 NAO_LOLA_CLIENT__INDEX_CONVERSION_HPP_ +#define NAO_LOLA_CLIENT__INDEX_CONVERSION_HPP_ + +#include +#include "nao_lola_client/lola_enums.hpp" +#include "nao_lola_command_msgs/msg/joint_indexes.hpp" +#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" +#include "nao_lola_command_msgs/msg/right_ear_leds.hpp" +#include "nao_lola_command_msgs/msg/left_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/head_leds.hpp" + +namespace IndexConversion +{ +std::map flip(std::map in); + +static const std::map joint_lola_to_msg = { + {LolaEnums::Joint::HeadYaw, nao_lola_command_msgs::msg::JointIndexes::HEADYAW}, + {LolaEnums::Joint::HeadPitch, nao_lola_command_msgs::msg::JointIndexes::HEADPITCH}, + {LolaEnums::Joint::LShoulderPitch, nao_lola_command_msgs::msg::JointIndexes::LSHOULDERPITCH}, + {LolaEnums::Joint::LShoulderRoll, nao_lola_command_msgs::msg::JointIndexes::LSHOULDERROLL}, + {LolaEnums::Joint::LElbowYaw, nao_lola_command_msgs::msg::JointIndexes::LELBOWYAW}, + {LolaEnums::Joint::LElbowRoll, nao_lola_command_msgs::msg::JointIndexes::LELBOWROLL}, + {LolaEnums::Joint::LWristYaw, nao_lola_command_msgs::msg::JointIndexes::LWRISTYAW}, + {LolaEnums::Joint::LHipYawPitch, nao_lola_command_msgs::msg::JointIndexes::LHIPYAWPITCH}, + {LolaEnums::Joint::LHipRoll, nao_lola_command_msgs::msg::JointIndexes::LHIPROLL}, + {LolaEnums::Joint::LHipPitch, nao_lola_command_msgs::msg::JointIndexes::LHIPPITCH}, + {LolaEnums::Joint::LKneePitch, nao_lola_command_msgs::msg::JointIndexes::LKNEEPITCH}, + {LolaEnums::Joint::LAnklePitch, nao_lola_command_msgs::msg::JointIndexes::LANKLEPITCH}, + {LolaEnums::Joint::LAnkleRoll, nao_lola_command_msgs::msg::JointIndexes::LANKLEROLL}, + {LolaEnums::Joint::RHipRoll, nao_lola_command_msgs::msg::JointIndexes::RHIPROLL}, + {LolaEnums::Joint::RHipPitch, nao_lola_command_msgs::msg::JointIndexes::RHIPPITCH}, + {LolaEnums::Joint::RKneePitch, nao_lola_command_msgs::msg::JointIndexes::RKNEEPITCH}, + {LolaEnums::Joint::RAnklePitch, nao_lola_command_msgs::msg::JointIndexes::RANKLEPITCH}, + {LolaEnums::Joint::RAnkleRoll, nao_lola_command_msgs::msg::JointIndexes::RANKLEROLL}, + {LolaEnums::Joint::RShoulderPitch, nao_lola_command_msgs::msg::JointIndexes::RSHOULDERPITCH}, + {LolaEnums::Joint::RShoulderRoll, nao_lola_command_msgs::msg::JointIndexes::RSHOULDERROLL}, + {LolaEnums::Joint::RElbowYaw, nao_lola_command_msgs::msg::JointIndexes::RELBOWYAW}, + {LolaEnums::Joint::RElbowRoll, nao_lola_command_msgs::msg::JointIndexes::RELBOWROLL}, + {LolaEnums::Joint::RWristYaw, nao_lola_command_msgs::msg::JointIndexes::RWRISTYAW}, + {LolaEnums::Joint::LHand, nao_lola_command_msgs::msg::JointIndexes::LHAND}, + {LolaEnums::Joint::RHand, nao_lola_command_msgs::msg::JointIndexes::RHAND}, +}; + +static const std::map joint_msg_to_lola = flip(joint_lola_to_msg); + +std::map flip(std::map in) +{ + std::map flipped; + for (std::map::iterator i = in.begin(); i != in.end(); ++i) { + flipped[i->second] = i->first; + } + + return flipped; +} + +// See http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#left-ear +static const std::map left_ear_leds_msg_to_lola +{ + {nao_lola_command_msgs::msg::LeftEarLeds::L0, LolaEnums::LeftEarLeds::Deg_0}, + {nao_lola_command_msgs::msg::LeftEarLeds::L1, LolaEnums::LeftEarLeds::Deg_36}, + {nao_lola_command_msgs::msg::LeftEarLeds::L2, LolaEnums::LeftEarLeds::Deg_72}, + {nao_lola_command_msgs::msg::LeftEarLeds::L3, LolaEnums::LeftEarLeds::Deg_108}, + {nao_lola_command_msgs::msg::LeftEarLeds::L4, LolaEnums::LeftEarLeds::Deg_144}, + {nao_lola_command_msgs::msg::LeftEarLeds::L5, LolaEnums::LeftEarLeds::Deg_180}, + {nao_lola_command_msgs::msg::LeftEarLeds::L6, LolaEnums::LeftEarLeds::Deg_216}, + {nao_lola_command_msgs::msg::LeftEarLeds::L7, LolaEnums::LeftEarLeds::Deg_252}, + {nao_lola_command_msgs::msg::LeftEarLeds::L8, LolaEnums::LeftEarLeds::Deg_288}, + {nao_lola_command_msgs::msg::LeftEarLeds::L9, LolaEnums::LeftEarLeds::Deg_324} +}; + +// See http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#right-ear +static const std::map right_ear_leds_msg_to_lola +{ + {nao_lola_command_msgs::msg::RightEarLeds::R0, LolaEnums::RightEarLeds::Deg_0}, + {nao_lola_command_msgs::msg::RightEarLeds::R1, LolaEnums::RightEarLeds::Deg_36}, + {nao_lola_command_msgs::msg::RightEarLeds::R2, LolaEnums::RightEarLeds::Deg_72}, + {nao_lola_command_msgs::msg::RightEarLeds::R3, LolaEnums::RightEarLeds::Deg_108}, + {nao_lola_command_msgs::msg::RightEarLeds::R4, LolaEnums::RightEarLeds::Deg_144}, + {nao_lola_command_msgs::msg::RightEarLeds::R5, LolaEnums::RightEarLeds::Deg_180}, + {nao_lola_command_msgs::msg::RightEarLeds::R6, LolaEnums::RightEarLeds::Deg_216}, + {nao_lola_command_msgs::msg::RightEarLeds::R7, LolaEnums::RightEarLeds::Deg_252}, + {nao_lola_command_msgs::msg::RightEarLeds::R8, LolaEnums::RightEarLeds::Deg_288}, + {nao_lola_command_msgs::msg::RightEarLeds::R9, LolaEnums::RightEarLeds::Deg_324} +}; + +// See http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#nao-v5-v4-and-v3-3 +static const std::map left_eye_leds_msg_to_lola +{ + {nao_lola_command_msgs::msg::LeftEyeLeds::L0, LolaEnums::LeftEyeLeds::Deg_45}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L1, LolaEnums::LeftEyeLeds::Deg_0}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L2, LolaEnums::LeftEyeLeds::Deg_315}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L3, LolaEnums::LeftEyeLeds::Deg_270}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L4, LolaEnums::LeftEyeLeds::Deg_225}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L5, LolaEnums::LeftEyeLeds::Deg_180}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L6, LolaEnums::LeftEyeLeds::Deg_135}, + {nao_lola_command_msgs::msg::LeftEyeLeds::L7, LolaEnums::LeftEyeLeds::Deg_90}, +}; + +// See http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#nao-v5-v4-and-v3-3 +static const std::map right_eye_leds_msg_to_lola +{ + {nao_lola_command_msgs::msg::RightEyeLeds::R0, LolaEnums::RightEyeLeds::Deg_315}, + {nao_lola_command_msgs::msg::RightEyeLeds::R1, LolaEnums::RightEyeLeds::Deg_270}, + {nao_lola_command_msgs::msg::RightEyeLeds::R2, LolaEnums::RightEyeLeds::Deg_225}, + {nao_lola_command_msgs::msg::RightEyeLeds::R3, LolaEnums::RightEyeLeds::Deg_180}, + {nao_lola_command_msgs::msg::RightEyeLeds::R4, LolaEnums::RightEyeLeds::Deg_135}, + {nao_lola_command_msgs::msg::RightEyeLeds::R5, LolaEnums::RightEyeLeds::Deg_90}, + {nao_lola_command_msgs::msg::RightEyeLeds::R6, LolaEnums::RightEyeLeds::Deg_45}, + {nao_lola_command_msgs::msg::RightEyeLeds::R7, LolaEnums::RightEyeLeds::Deg_0}, +}; + +// See http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#head-tactile-sensor-led-locations +static const std::map head_leds_msg_to_lola +{ + {nao_lola_command_msgs::msg::HeadLeds::B0, LolaEnums::SkullLeds::Front_Right_1}, + {nao_lola_command_msgs::msg::HeadLeds::B1, LolaEnums::SkullLeds::Front_Right_0}, + {nao_lola_command_msgs::msg::HeadLeds::B2, LolaEnums::SkullLeds::Middle_Right_0}, + {nao_lola_command_msgs::msg::HeadLeds::B3, LolaEnums::SkullLeds::Rear_Right_0}, + {nao_lola_command_msgs::msg::HeadLeds::B4, LolaEnums::SkullLeds::Rear_Right_1}, + {nao_lola_command_msgs::msg::HeadLeds::B5, LolaEnums::SkullLeds::Rear_Right_2}, + {nao_lola_command_msgs::msg::HeadLeds::B6, LolaEnums::SkullLeds::Rear_Left_2}, + {nao_lola_command_msgs::msg::HeadLeds::B7, LolaEnums::SkullLeds::Rear_Left_1}, + {nao_lola_command_msgs::msg::HeadLeds::B8, LolaEnums::SkullLeds::Rear_Left_0}, + {nao_lola_command_msgs::msg::HeadLeds::B9, LolaEnums::SkullLeds::Middle_Left_0}, + {nao_lola_command_msgs::msg::HeadLeds::B10, LolaEnums::SkullLeds::Front_Left_0}, + {nao_lola_command_msgs::msg::HeadLeds::B11, LolaEnums::SkullLeds::Front_Left_1}, +}; + +} // namespace IndexConversion + +#endif // NAO_LOLA_CLIENT__INDEX_CONVERSION_HPP_ diff --git a/nao_lola_client/include/nao_lola_client/lola_enums.hpp b/nao_lola_client/include/nao_lola_client/lola_enums.hpp new file mode 100644 index 0000000..50a51f1 --- /dev/null +++ b/nao_lola_client/include/nao_lola_client/lola_enums.hpp @@ -0,0 +1,173 @@ +// Copyright 2021 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 NAO_LOLA_CLIENT__LOLA_ENUMS_HPP_ +#define NAO_LOLA_CLIENT__LOLA_ENUMS_HPP_ + +// The enums are exactly as defined (and in the same order) as what's provided +// in "Lola RoboCupper Official Documentation.pdf" sent to all Standard Platform League +// teams in RoboCup. + +namespace LolaEnums +{ + +enum class Joint +{ + 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, + NUM_JOINTS +}; + +enum class Battery {Charge, Current, Status, Temperature}; + +enum class Accelerometer {X, Y, Z}; + +enum class Gyroscope {X, Y, Z}; + +enum class Angles {X, Y}; + +enum class Sonar {Left, Right}; + +enum class FSR +{ + LFoot_FrontLeft, + LFoot_FrontRight, + LFoot_RearLeft, + LFoot_RearRight, + RFoot_FrontLeft, + RFoot_FrontRight, + RFoot_RearLeft, + RFoot_RearRight +}; + +enum class Touch +{ + ChestBoard_Button, + Head_Touch_Front, + Head_Touch_Middle, + Head_Touch_Rear, + LFoot_Bumper_Left, + LFoot_Bumper_Right, + LHand_Touch_Back, + LHand_Touch_Left, + LHand_Touch_Right, + RFoot_Bumper_Left, + RFoot_Bumper_Right, + RHand_Touch_Back, + RHand_Touch_Left, + RHand_Touch_Right +}; + +enum class RobotConfig +{ + Body_BodyId, + Body_Version, + Head_FullHeadId, + Head_Version +}; + +enum class LeftEarLeds +{ + Deg_0, + Deg_36, + Deg_72, + Deg_108, + Deg_144, + Deg_180, + Deg_216, + Deg_252, + Deg_288, + Deg_324 +}; + +enum class RightEarLeds +{ + Deg_324, + Deg_288, + Deg_252, + Deg_216, + Deg_180, + Deg_144, + Deg_108, + Deg_72, + Deg_36, + Deg_0 +}; + +enum class LeftEyeLeds +{ + Deg_45, + Deg_0, + Deg_315, + Deg_270, + Deg_225, + Deg_180, + Deg_135, + Deg_90 +}; + +enum class RightEyeLeds +{ + Deg_0, + Deg_45, + Deg_90, + Deg_135, + Deg_180, + Deg_225, + Deg_270, + Deg_315 +}; + +enum class SkullLeds +{ + Front_Left_1, + Front_Left_0, + Middle_Left_0, + Rear_Left_0, + Rear_Left_1, + Rear_Left_2, + Rear_Right_2, + Rear_Right_1, + Rear_Right_0, + Middle_Right_0, + Front_Right_0, + Front_Right_1 +}; + +} // namespace LolaEnums + +#endif // NAO_LOLA_CLIENT__LOLA_ENUMS_HPP_ diff --git a/nao_lola_client/include/nao_lola_client/msgpack_packer.hpp b/nao_lola_client/include/nao_lola_client/msgpack_packer.hpp new file mode 100644 index 0000000..7427fdd --- /dev/null +++ b/nao_lola_client/include/nao_lola_client/msgpack_packer.hpp @@ -0,0 +1,71 @@ +// Copyright 2021 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 NAO_LOLA_CLIENT__MSGPACK_PACKER_HPP_ +#define NAO_LOLA_CLIENT__MSGPACK_PACKER_HPP_ + +#include +#include +#include +#include "nao_lola_command_msgs/msg/joint_positions.hpp" +#include "nao_lola_command_msgs/msg/joint_stiffnesses.hpp" +#include "nao_lola_command_msgs/msg/chest_led.hpp" +#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" +#include "nao_lola_command_msgs/msg/right_ear_leds.hpp" +#include "nao_lola_command_msgs/msg/left_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/left_foot_led.hpp" +#include "nao_lola_command_msgs/msg/right_foot_led.hpp" +#include "nao_lola_command_msgs/msg/head_leds.hpp" +#include "nao_lola_command_msgs/msg/sonar_usage.hpp" +#include "nao_lola_client/lola_enums.hpp" +#include "rclcpp/logger.hpp" + + +class MsgpackPacker +{ +public: + MsgpackPacker() + : logger(rclcpp::get_logger("msgpack packer")) {} + std::string getPacked(); + + void setJointPositions(const nao_lola_command_msgs::msg::JointPositions & jointPositions); + void setJointStiffnesses(const nao_lola_command_msgs::msg::JointStiffnesses & jointStiffnesses); + void setChestLed(const nao_lola_command_msgs::msg::ChestLed & chestLed); + void setLeftEarLeds(const nao_lola_command_msgs::msg::LeftEarLeds & leftEarLeds); + void setRightEarLeds(const nao_lola_command_msgs::msg::RightEarLeds & rightEarLeds); + void setLeftEyeLeds(const nao_lola_command_msgs::msg::LeftEyeLeds & leftEyeLeds); + void setRightEyeLeds(const nao_lola_command_msgs::msg::RightEyeLeds & rightEyeLeds); + void setLeftFootLed(const nao_lola_command_msgs::msg::LeftFootLed & leftFootLed); + void setRightFootLed(const nao_lola_command_msgs::msg::RightFootLed & rightFootLed); + void setHeadLeds(const nao_lola_command_msgs::msg::HeadLeds & headLeds); + void setSonarUsage(const nao_lola_command_msgs::msg::SonarUsage & sonarUsage); + +private: + std::array(LolaEnums::Joint::NUM_JOINTS)> position; + std::array(LolaEnums::Joint::NUM_JOINTS)> stiffness; + std::array chest; + std::array(nao_lola_command_msgs::msg::LeftEarLeds::NUM_LEDS)> l_ear; + std::array(nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS)> r_ear; + std::array(nao_lola_command_msgs::msg::LeftEyeLeds::NUM_LEDS)> l_eye; + std::array(nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS)> r_eye; + std::array l_foot; + std::array r_foot; + std::array(nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS)> skull; + std::array sonar; + + rclcpp::Logger logger; +}; + +#endif // NAO_LOLA_CLIENT__MSGPACK_PACKER_HPP_ diff --git a/nao_lola_client/include/nao_lola_client/msgpack_parser.hpp b/nao_lola_client/include/nao_lola_client/msgpack_parser.hpp new file mode 100644 index 0000000..2d15c5a --- /dev/null +++ b/nao_lola_client/include/nao_lola_client/msgpack_parser.hpp @@ -0,0 +1,63 @@ +// Copyright 2021 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 NAO_LOLA_CLIENT__MSGPACK_PARSER_HPP_ +#define NAO_LOLA_CLIENT__MSGPACK_PARSER_HPP_ + +#include +#include +#include +#include "msgpack.hpp" +#include "nao_lola_sensor_msgs/msg/joint_currents.hpp" +#include "nao_lola_sensor_msgs/msg/joint_positions.hpp" +#include "nao_lola_sensor_msgs/msg/joint_stiffnesses.hpp" +#include "nao_lola_sensor_msgs/msg/joint_temperatures.hpp" +#include "nao_lola_sensor_msgs/msg/joint_statuses.hpp" +#include "nao_lola_sensor_msgs/msg/buttons.hpp" +#include "nao_lola_sensor_msgs/msg/accelerometer.hpp" +#include "nao_lola_sensor_msgs/msg/gyroscope.hpp" +#include "nao_lola_sensor_msgs/msg/angle.hpp" +#include "nao_lola_sensor_msgs/msg/sonar.hpp" +#include "nao_lola_sensor_msgs/msg/fsr.hpp" +#include "nao_lola_sensor_msgs/msg/touch.hpp" +#include "nao_lola_sensor_msgs/msg/battery.hpp" +#include "nao_lola_sensor_msgs/msg/robot_config.hpp" + +class MsgpackParser +{ +public: + explicit MsgpackParser(char data[], int size); + + nao_lola_sensor_msgs::msg::Accelerometer getAccelerometer(); + nao_lola_sensor_msgs::msg::Angle getAngle(); + nao_lola_sensor_msgs::msg::Buttons getButtons(); + nao_lola_sensor_msgs::msg::FSR getFSR(); + nao_lola_sensor_msgs::msg::Gyroscope getGyroscope(); + nao_lola_sensor_msgs::msg::JointCurrents getJointCurrents(); + nao_lola_sensor_msgs::msg::JointPositions getJointPositions(); + nao_lola_sensor_msgs::msg::JointStiffnesses getJointStiffnesses(); + nao_lola_sensor_msgs::msg::JointTemperatures getJointTemperatures(); + nao_lola_sensor_msgs::msg::JointStatuses getJointStatuses(); + nao_lola_sensor_msgs::msg::Sonar getSonar(); + nao_lola_sensor_msgs::msg::Touch getTouch(); + nao_lola_sensor_msgs::msg::Battery getBattery(); + nao_lola_sensor_msgs::msg::RobotConfig getRobotConfig(); + +private: + msgpack::object_handle oh; // Keep this variable throughout the lifetime of this object + std::map unpacked; +}; + + +#endif // NAO_LOLA_CLIENT__MSGPACK_PARSER_HPP_ 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 new file mode 100644 index 0000000..b065189 --- /dev/null +++ b/nao_lola_client/include/nao_lola_client/nao_lola_client.hpp @@ -0,0 +1,95 @@ +// Copyright 2021 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 NAO_LOLA_CLIENT__NAO_LOLA_CLIENT_HPP_ +#define NAO_LOLA_CLIENT__NAO_LOLA_CLIENT_HPP_ + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nao_lola_sensor_msgs/msg/joint_positions.hpp" +#include "nao_lola_sensor_msgs/msg/joint_stiffnesses.hpp" +#include "nao_lola_sensor_msgs/msg/joint_temperatures.hpp" +#include "nao_lola_sensor_msgs/msg/joint_currents.hpp" +#include "nao_lola_sensor_msgs/msg/joint_statuses.hpp" +#include "nao_lola_sensor_msgs/msg/buttons.hpp" +#include "nao_lola_sensor_msgs/msg/accelerometer.hpp" +#include "nao_lola_sensor_msgs/msg/gyroscope.hpp" +#include "nao_lola_sensor_msgs/msg/angle.hpp" +#include "nao_lola_sensor_msgs/msg/sonar.hpp" +#include "nao_lola_sensor_msgs/msg/fsr.hpp" +#include "nao_lola_sensor_msgs/msg/touch.hpp" +#include "nao_lola_sensor_msgs/msg/battery.hpp" +#include "nao_lola_sensor_msgs/msg/robot_config.hpp" +#include "nao_lola_command_msgs/msg/chest_led.hpp" +#include "nao_lola_command_msgs/msg/left_ear_leds.hpp" +#include "nao_lola_command_msgs/msg/right_ear_leds.hpp" +#include "nao_lola_command_msgs/msg/left_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/right_eye_leds.hpp" +#include "nao_lola_command_msgs/msg/left_foot_led.hpp" +#include "nao_lola_command_msgs/msg/right_foot_led.hpp" +#include "nao_lola_command_msgs/msg/head_leds.hpp" +#include "nao_lola_command_msgs/msg/sonar_usage.hpp" +#include "nao_lola_command_msgs/msg/joint_positions.hpp" +#include "nao_lola_command_msgs/msg/joint_stiffnesses.hpp" +#include "nao_lola_client/connection.hpp" +#include "nao_lola_client/msgpack_packer.hpp" + +class NaoLolaClient : public rclcpp::Node +{ +public: + NaoLolaClient(); + virtual ~NaoLolaClient() {} + +private: + void createPublishers(); + void createSubscriptions(); + + rclcpp::Publisher::SharedPtr accelerometer_pub; + rclcpp::Publisher::SharedPtr angle_pub; + rclcpp::Publisher::SharedPtr buttons_pub; + rclcpp::Publisher::SharedPtr fsr_pub; + rclcpp::Publisher::SharedPtr gyroscope_pub; + rclcpp::Publisher::SharedPtr joint_positions_pub; + rclcpp::Publisher::SharedPtr joint_stiffnesses_pub; + rclcpp::Publisher::SharedPtr joint_temperatures_pub; + rclcpp::Publisher::SharedPtr joint_currents_pub; + rclcpp::Publisher::SharedPtr joint_statuses_pub; + rclcpp::Publisher::SharedPtr sonar_pub; + rclcpp::Publisher::SharedPtr touch_pub; + rclcpp::Publisher::SharedPtr battery_pub; + rclcpp::Publisher::SharedPtr robot_config_pub; + + rclcpp::Subscription::SharedPtr joint_positions_sub; + rclcpp::Subscription::SharedPtr + joint_stiffnesses_sub; + rclcpp::Subscription::SharedPtr chest_led_sub; + rclcpp::Subscription::SharedPtr left_ear_leds_sub; + rclcpp::Subscription::SharedPtr right_ear_leds_sub; + rclcpp::Subscription::SharedPtr left_eye_leds_sub; + rclcpp::Subscription::SharedPtr right_eye_leds_sub; + rclcpp::Subscription::SharedPtr left_foot_led_sub; + rclcpp::Subscription::SharedPtr right_foot_led_sub; + rclcpp::Subscription::SharedPtr head_leds_sub; + rclcpp::Subscription::SharedPtr sonar_usage_sub; + + std::thread receive_thread_; + Connection connection; + + MsgpackPacker packer; + std::mutex packer_mutex; +}; + +#endif // NAO_LOLA_CLIENT__NAO_LOLA_CLIENT_HPP_ diff --git a/nao_lola_client/include/nao_lola_client/sensor_index_conversion.hpp b/nao_lola_client/include/nao_lola_client/sensor_index_conversion.hpp new file mode 100644 index 0000000..e454011 --- /dev/null +++ b/nao_lola_client/include/nao_lola_client/sensor_index_conversion.hpp @@ -0,0 +1,68 @@ +// Copyright 2021 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 NAO_LOLA_CLIENT__SENSOR_INDEX_CONVERSION_HPP_ +#define NAO_LOLA_CLIENT__SENSOR_INDEX_CONVERSION_HPP_ + +#include +#include "nao_lola_client/lola_enums.hpp" +#include "nao_lola_sensor_msgs/msg/joint_indexes.hpp" + +namespace IndexConversion +{ +std::map flip(std::map in); + +static const std::map joint_lola_to_msg = { + {LolaEnums::Joint::HeadYaw, nao_lola_sensor_msgs::msg::JointIndexes::HEADYAW}, + {LolaEnums::Joint::HeadPitch, nao_lola_sensor_msgs::msg::JointIndexes::HEADPITCH}, + {LolaEnums::Joint::LShoulderPitch, nao_lola_sensor_msgs::msg::JointIndexes::LSHOULDERPITCH}, + {LolaEnums::Joint::LShoulderRoll, nao_lola_sensor_msgs::msg::JointIndexes::LSHOULDERROLL}, + {LolaEnums::Joint::LElbowYaw, nao_lola_sensor_msgs::msg::JointIndexes::LELBOWYAW}, + {LolaEnums::Joint::LElbowRoll, nao_lola_sensor_msgs::msg::JointIndexes::LELBOWROLL}, + {LolaEnums::Joint::LWristYaw, nao_lola_sensor_msgs::msg::JointIndexes::LWRISTYAW}, + {LolaEnums::Joint::LHipYawPitch, nao_lola_sensor_msgs::msg::JointIndexes::LHIPYAWPITCH}, + {LolaEnums::Joint::LHipRoll, nao_lola_sensor_msgs::msg::JointIndexes::LHIPROLL}, + {LolaEnums::Joint::LHipPitch, nao_lola_sensor_msgs::msg::JointIndexes::LHIPPITCH}, + {LolaEnums::Joint::LKneePitch, nao_lola_sensor_msgs::msg::JointIndexes::LKNEEPITCH}, + {LolaEnums::Joint::LAnklePitch, nao_lola_sensor_msgs::msg::JointIndexes::LANKLEPITCH}, + {LolaEnums::Joint::LAnkleRoll, nao_lola_sensor_msgs::msg::JointIndexes::LANKLEROLL}, + {LolaEnums::Joint::RHipRoll, nao_lola_sensor_msgs::msg::JointIndexes::RHIPROLL}, + {LolaEnums::Joint::RHipPitch, nao_lola_sensor_msgs::msg::JointIndexes::RHIPPITCH}, + {LolaEnums::Joint::RKneePitch, nao_lola_sensor_msgs::msg::JointIndexes::RKNEEPITCH}, + {LolaEnums::Joint::RAnklePitch, nao_lola_sensor_msgs::msg::JointIndexes::RANKLEPITCH}, + {LolaEnums::Joint::RAnkleRoll, nao_lola_sensor_msgs::msg::JointIndexes::RANKLEROLL}, + {LolaEnums::Joint::RShoulderPitch, nao_lola_sensor_msgs::msg::JointIndexes::RSHOULDERPITCH}, + {LolaEnums::Joint::RShoulderRoll, nao_lola_sensor_msgs::msg::JointIndexes::RSHOULDERROLL}, + {LolaEnums::Joint::RElbowYaw, nao_lola_sensor_msgs::msg::JointIndexes::RELBOWYAW}, + {LolaEnums::Joint::RElbowRoll, nao_lola_sensor_msgs::msg::JointIndexes::RELBOWROLL}, + {LolaEnums::Joint::RWristYaw, nao_lola_sensor_msgs::msg::JointIndexes::RWRISTYAW}, + {LolaEnums::Joint::LHand, nao_lola_sensor_msgs::msg::JointIndexes::LHAND}, + {LolaEnums::Joint::RHand, nao_lola_sensor_msgs::msg::JointIndexes::RHAND}, +}; + +static const std::map joint_msg_to_lola = flip(joint_lola_to_msg); + +std::map flip(std::map in) +{ + std::map flipped; + for (std::map::iterator i = in.begin(); i != in.end(); ++i) { + flipped[i->second] = i->first; + } + + return flipped; +} + +} // namespace IndexConversion + +#endif // NAO_LOLA_CLIENT__SENSOR_INDEX_CONVERSION_HPP_ diff --git a/nao_lola_client/package.xml b/nao_lola_client/package.xml new file mode 100644 index 0000000..6b745f1 --- /dev/null +++ b/nao_lola_client/package.xml @@ -0,0 +1,24 @@ + + + + nao_lola_client + 0.3.1 + Packages that allow communicating with the NAO's Lola middle-ware. + ijnek + Apache License 2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + rclcpp + nao_lola_command_msgs + nao_lola_sensor_msgs + boost + + + ament_cmake + + diff --git a/nao_lola_client/src/connection.cpp b/nao_lola_client/src/connection.cpp new file mode 100644 index 0000000..a7ae000 --- /dev/null +++ b/nao_lola_client/src/connection.cpp @@ -0,0 +1,44 @@ +// Copyright 2021 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 +#include "nao_lola_client/connection.hpp" + +#define ENDPOINT "/tmp/robocup" + +Connection::Connection() +: io_service(), socket(io_service), logger(rclcpp::get_logger("lola connection")) +{ + boost::system::error_code ec; + socket.connect(ENDPOINT, ec); + if (ec) { + RCLCPP_ERROR(logger, (std::string{"Could not connect to LoLA: "} + ec.message()).c_str()); + } +} + +std::array Connection::receive() +{ + boost::system::error_code ec; + std::array data; + socket.receive(boost::asio::buffer(data), 0, ec); + if (ec) { + RCLCPP_ERROR(logger, (std::string{"Could not read from LoLA: "} + ec.message()).c_str()); + } + return data; +} + +void Connection::send(std::string data) +{ + socket.send(boost::asio::buffer(data)); +} diff --git a/nao_lola_client/src/msgpack_packer.cpp b/nao_lola_client/src/msgpack_packer.cpp new file mode 100644 index 0000000..dc631ca --- /dev/null +++ b/nao_lola_client/src/msgpack_packer.cpp @@ -0,0 +1,161 @@ +// Copyright 2021 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 +#include +#include +#include +#include +#include "nao_lola_client/msgpack_packer.hpp" +#include "msgpack.hpp" +#include "nao_lola_client/command_index_conversion.hpp" +#include "rclcpp/rclcpp.hpp" + +std::string MsgpackPacker::getPacked() +{ + msgpack::zone z; + std::map map; + + map.insert(std::make_pair("Position", msgpack::object(position, z))); + map.insert(std::make_pair("Stiffness", msgpack::object(stiffness, z))); + map.insert(std::make_pair("Chest", msgpack::object(chest, z))); + map.insert(std::make_pair("LEar", msgpack::object(l_ear, z))); + map.insert(std::make_pair("REar", msgpack::object(r_ear, z))); + map.insert(std::make_pair("LEye", msgpack::object(l_eye, z))); + map.insert(std::make_pair("REye", msgpack::object(r_eye, z))); + map.insert(std::make_pair("LFoot", msgpack::object(l_foot, z))); + map.insert(std::make_pair("RFoot", msgpack::object(r_foot, z))); + map.insert(std::make_pair("Skull", msgpack::object(skull, z))); + map.insert(std::make_pair("Sonar", msgpack::object(sonar, z))); + + std::stringstream buffer; + msgpack::pack(buffer, map); + std::string packed = buffer.str(); + + return packed; +} + +void MsgpackPacker::setJointPositions( + const nao_lola_command_msgs::msg::JointPositions & jointPositions) +{ + if (jointPositions.indexes.size() != jointPositions.positions.size()) { + RCLCPP_ERROR( + logger, + "Incorrect message received for nao_lola_command_msgs::msg::JointPositions. " + "Angles and Indexes vector must have the same length. " + "Angles vector has length %zu, while indexes vector has length %zu", + jointPositions.positions.size(), jointPositions.indexes.size()); + } + + for (unsigned i = 0; i < jointPositions.indexes.size(); ++i) { + int msg_joint_index = jointPositions.indexes[i]; + float joint_angle = jointPositions.positions[i]; + LolaEnums::Joint lola_joint_index = IndexConversion::joint_msg_to_lola.at(msg_joint_index); + position.at(static_cast(lola_joint_index)) = joint_angle; + } +} + +void MsgpackPacker::setJointStiffnesses( + const nao_lola_command_msgs::msg::JointStiffnesses & jointStiffnesses) +{ + if (jointStiffnesses.indexes.size() != jointStiffnesses.stiffnesses.size()) { + RCLCPP_ERROR( + logger, + "Incorrect message received for nao_lola_command_msgs::msg::JointStiffnesses. " + "Stiffnesses and Indexes vector must have the same length. " + "Stiffnesses vector has length %zu, while indexes vector has length %zu", + jointStiffnesses.stiffnesses.size(), jointStiffnesses.indexes.size()); + } + + for (unsigned i = 0; i < jointStiffnesses.indexes.size(); ++i) { + int msg_joint_index = jointStiffnesses.indexes[i]; + float joint_stiffness = jointStiffnesses.stiffnesses[i]; + LolaEnums::Joint lola_joint_index = IndexConversion::joint_msg_to_lola.at(msg_joint_index); + stiffness.at(static_cast(lola_joint_index)) = joint_stiffness; + } +} + +void MsgpackPacker::setChestLed(const nao_lola_command_msgs::msg::ChestLed & chestLed) +{ + chest.at(0) = chestLed.color.r; + chest.at(1) = chestLed.color.g; + chest.at(2) = chestLed.color.b; +} + +void MsgpackPacker::setLeftEarLeds(const nao_lola_command_msgs::msg::LeftEarLeds & leftEarLeds) +{ + for (unsigned i = 0; i < leftEarLeds.NUM_LEDS; ++i) { + LolaEnums::LeftEarLeds lola_index = IndexConversion::left_ear_leds_msg_to_lola.at(i); + l_ear.at(static_cast(lola_index)) = leftEarLeds.intensities[i]; + } +} + +void MsgpackPacker::setRightEarLeds( + const nao_lola_command_msgs::msg::RightEarLeds & rightEarLeds) +{ + for (unsigned i = 0; i < rightEarLeds.NUM_LEDS; ++i) { + LolaEnums::RightEarLeds lola_index = IndexConversion::right_ear_leds_msg_to_lola.at(i); + r_ear.at(static_cast(lola_index)) = rightEarLeds.intensities[i]; + } +} + +void MsgpackPacker::setLeftEyeLeds(const nao_lola_command_msgs::msg::LeftEyeLeds & leftEyeLeds) +{ + for (unsigned i = 0; i < leftEyeLeds.NUM_LEDS; ++i) { + LolaEnums::LeftEyeLeds lola_index = IndexConversion::left_eye_leds_msg_to_lola.at(i); + l_eye.at(static_cast(lola_index)) = leftEyeLeds.colors[i].r; + l_eye.at(static_cast(lola_index) + 8) = leftEyeLeds.colors[i].g; + l_eye.at(static_cast(lola_index) + 16) = leftEyeLeds.colors[i].b; + } +} + +void MsgpackPacker::setRightEyeLeds( + const nao_lola_command_msgs::msg::RightEyeLeds & rightEyeLeds) +{ + for (unsigned i = 0; i < rightEyeLeds.NUM_LEDS; ++i) { + LolaEnums::RightEyeLeds lola_index = IndexConversion::right_eye_leds_msg_to_lola.at(i); + r_eye.at(static_cast(lola_index)) = rightEyeLeds.colors[i].r; + r_eye.at(static_cast(lola_index) + 8) = rightEyeLeds.colors[i].g; + r_eye.at(static_cast(lola_index) + 16) = rightEyeLeds.colors[i].b; + } +} + +void MsgpackPacker::setLeftFootLed(const nao_lola_command_msgs::msg::LeftFootLed & leftFootLed) +{ + l_foot.at(0) = leftFootLed.color.r; + l_foot.at(1) = leftFootLed.color.g; + l_foot.at(2) = leftFootLed.color.b; +} + +void MsgpackPacker::setRightFootLed( + const nao_lola_command_msgs::msg::RightFootLed & rightFootLed) +{ + r_foot.at(0) = rightFootLed.color.r; + r_foot.at(1) = rightFootLed.color.g; + r_foot.at(2) = rightFootLed.color.b; +} + +void MsgpackPacker::setHeadLeds(const nao_lola_command_msgs::msg::HeadLeds & headLeds) +{ + for (unsigned i = 0; i < headLeds.NUM_LEDS; ++i) { + LolaEnums::SkullLeds lola_index = IndexConversion::head_leds_msg_to_lola.at(i); + skull.at(static_cast(lola_index)) = headLeds.intensities[i]; + } +} + +void MsgpackPacker::setSonarUsage(const nao_lola_command_msgs::msg::SonarUsage & sonarUsage) +{ + sonar.at(0) = sonarUsage.left; + sonar.at(1) = sonarUsage.right; +} diff --git a/nao_lola_client/src/msgpack_parser.cpp b/nao_lola_client/src/msgpack_parser.cpp new file mode 100644 index 0000000..5e11908 --- /dev/null +++ b/nao_lola_client/src/msgpack_parser.cpp @@ -0,0 +1,194 @@ +// Copyright 2021 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 +#include +#include +#include "nao_lola_client/msgpack_parser.hpp" +#include "nao_lola_client/lola_enums.hpp" +#include "nao_lola_client/sensor_index_conversion.hpp" + + +MsgpackParser::MsgpackParser(char data[], int size) +{ + oh = msgpack::unpack(data, size); + + unpacked = oh.get().as>(); +} + +nao_lola_sensor_msgs::msg::Accelerometer MsgpackParser::getAccelerometer() +{ + nao_lola_sensor_msgs::msg::Accelerometer acc; + std::vector vec = unpacked.at("Accelerometer").as>(); + acc.x = vec.at(static_cast(LolaEnums::Accelerometer::X)); + acc.y = vec.at(static_cast(LolaEnums::Accelerometer::Y)); + acc.z = vec.at(static_cast(LolaEnums::Accelerometer::Z)); + return acc; +} + +nao_lola_sensor_msgs::msg::Angle MsgpackParser::getAngle() +{ + nao_lola_sensor_msgs::msg::Angle ang; + std::vector vec = unpacked.at("Angles").as>(); + ang.x = vec.at(static_cast(LolaEnums::Angles::X)); + ang.y = vec.at(static_cast(LolaEnums::Angles::Y)); + return ang; +} + +nao_lola_sensor_msgs::msg::Buttons MsgpackParser::getButtons() +{ + nao_lola_sensor_msgs::msg::Buttons but; + std::vector vec = unpacked.at("Touch").as>(); + but.chest = vec.at(static_cast(LolaEnums::Touch::ChestBoard_Button)); + but.l_foot_bumper_left = vec.at(static_cast(LolaEnums::Touch::LFoot_Bumper_Left)); + but.l_foot_bumper_right = vec.at(static_cast(LolaEnums::Touch::LFoot_Bumper_Right)); + but.r_foot_bumper_left = vec.at(static_cast(LolaEnums::Touch::RFoot_Bumper_Left)); + but.r_foot_bumper_right = vec.at(static_cast(LolaEnums::Touch::RFoot_Bumper_Right)); + return but; +} + +nao_lola_sensor_msgs::msg::FSR MsgpackParser::getFSR() +{ + nao_lola_sensor_msgs::msg::FSR fsr; + std::vector vec = unpacked.at("FSR").as>(); + fsr.l_foot_front_left = vec.at(static_cast(LolaEnums::FSR::LFoot_FrontLeft)); + fsr.l_foot_front_right = vec.at(static_cast(LolaEnums::FSR::LFoot_FrontRight)); + fsr.l_foot_back_left = vec.at(static_cast(LolaEnums::FSR::LFoot_RearLeft)); + fsr.l_foot_back_right = vec.at(static_cast(LolaEnums::FSR::LFoot_RearRight)); + fsr.r_foot_front_left = vec.at(static_cast(LolaEnums::FSR::RFoot_FrontLeft)); + fsr.r_foot_front_right = vec.at(static_cast(LolaEnums::FSR::RFoot_FrontRight)); + fsr.r_foot_back_left = vec.at(static_cast(LolaEnums::FSR::RFoot_RearLeft)); + fsr.r_foot_back_right = vec.at(static_cast(LolaEnums::FSR::RFoot_RearRight)); + return fsr; +} + +nao_lola_sensor_msgs::msg::Gyroscope MsgpackParser::getGyroscope() +{ + nao_lola_sensor_msgs::msg::Gyroscope gyr; + std::vector vec = unpacked.at("Gyroscope").as>(); + gyr.x = vec.at(static_cast(LolaEnums::Gyroscope::X)); + gyr.y = vec.at(static_cast(LolaEnums::Gyroscope::Y)); + gyr.z = vec.at(static_cast(LolaEnums::Gyroscope::Z)); + return gyr; +} + +nao_lola_sensor_msgs::msg::JointPositions MsgpackParser::getJointPositions() +{ + nao_lola_sensor_msgs::msg::JointPositions jointPositions; + + std::vector positions = unpacked.at("Position").as>(); + for (int i = 0; i < static_cast(LolaEnums::Joint::NUM_JOINTS); ++i) { + int msg_index = IndexConversion::joint_lola_to_msg.at(static_cast(i)); + jointPositions.positions.at(msg_index) = positions.at(i); + } + return jointPositions; +} + +nao_lola_sensor_msgs::msg::JointStiffnesses MsgpackParser::getJointStiffnesses() +{ + nao_lola_sensor_msgs::msg::JointStiffnesses jointStiffnesses; + + std::vector stiffnesses = unpacked.at("Stiffness").as>(); + for (int i = 0; i < static_cast(LolaEnums::Joint::NUM_JOINTS); ++i) { + int msg_index = IndexConversion::joint_lola_to_msg.at(static_cast(i)); + jointStiffnesses.stiffnesses.at(msg_index) = stiffnesses.at(i); + } + return jointStiffnesses; +} + +nao_lola_sensor_msgs::msg::JointTemperatures MsgpackParser::getJointTemperatures() +{ + nao_lola_sensor_msgs::msg::JointTemperatures jointTemperatures; + + std::vector temperatures = unpacked.at("Temperature").as>(); + for (int i = 0; i < static_cast(LolaEnums::Joint::NUM_JOINTS); ++i) { + int msg_index = IndexConversion::joint_lola_to_msg.at(static_cast(i)); + jointTemperatures.temperatures.at(msg_index) = temperatures.at(i); + } + return jointTemperatures; +} + +nao_lola_sensor_msgs::msg::JointCurrents MsgpackParser::getJointCurrents() +{ + nao_lola_sensor_msgs::msg::JointCurrents jointCurrents; + + std::vector currents = unpacked.at("Current").as>(); + for (int i = 0; i < static_cast(LolaEnums::Joint::NUM_JOINTS); ++i) { + int msg_index = IndexConversion::joint_lola_to_msg.at(static_cast(i)); + jointCurrents.currents.at(msg_index) = currents.at(i); + } + return jointCurrents; +} + +nao_lola_sensor_msgs::msg::JointStatuses MsgpackParser::getJointStatuses() +{ + nao_lola_sensor_msgs::msg::JointStatuses jointStatuses; + + // The LoLA RoboCupper docs say "status" is an integer data type, that's wrong. + std::vector statuses = unpacked.at("Status").as>(); + for (int i = 0; i < static_cast(LolaEnums::Joint::NUM_JOINTS); ++i) { + int msg_index = IndexConversion::joint_lola_to_msg.at(static_cast(i)); + jointStatuses.statuses.at(msg_index) = statuses.at(i); + } + return jointStatuses; +} + +nao_lola_sensor_msgs::msg::Sonar MsgpackParser::getSonar() +{ + nao_lola_sensor_msgs::msg::Sonar snr; + std::vector vec = unpacked.at("Sonar").as>(); + snr.left = vec.at(static_cast(LolaEnums::Sonar::Left)); + snr.right = vec.at(static_cast(LolaEnums::Sonar::Right)); + return snr; +} + + +nao_lola_sensor_msgs::msg::Touch MsgpackParser::getTouch() +{ + nao_lola_sensor_msgs::msg::Touch tch; + std::vector vec = unpacked.at("Touch").as>(); + tch.head_front = vec.at(static_cast(LolaEnums::Touch::Head_Touch_Front)); + tch.head_middle = vec.at(static_cast(LolaEnums::Touch::Head_Touch_Middle)); + tch.head_rear = vec.at(static_cast(LolaEnums::Touch::Head_Touch_Rear)); + return tch; +} + +nao_lola_sensor_msgs::msg::Battery MsgpackParser::getBattery() +{ + nao_lola_sensor_msgs::msg::Battery btr; + std::vector vec = unpacked.at("Battery").as>(); + // Convert charge to [0% - 100%] + btr.charge = vec.at(static_cast(LolaEnums::Battery::Charge)) * 100.0; + btr.current = vec.at(static_cast(LolaEnums::Battery::Current)); + btr.temperature = vec.at(static_cast(LolaEnums::Battery::Temperature)); + + + // Check whether robot is charging, with BHuman's equation used as reference: + // https://github.com/bhuman/BHumanCodeRelease/tree/coderelease2019/Src/Modules/Infrastructure/NaoProvider/NaoProvider.cpp#L320 + float status = vec.at(static_cast(LolaEnums::Battery::Status)); + btr.charging = ((static_cast(status) & 0x80) != 0); + + return btr; +} + +nao_lola_sensor_msgs::msg::RobotConfig MsgpackParser::getRobotConfig() +{ + nao_lola_sensor_msgs::msg::RobotConfig cfg; + std::vector vec = unpacked.at("RobotConfig").as>(); + cfg.body_id = vec.at(static_cast(LolaEnums::RobotConfig::Body_BodyId)); + cfg.body_version = vec.at(static_cast(LolaEnums::RobotConfig::Body_Version)); + cfg.head_id = vec.at(static_cast(LolaEnums::RobotConfig::Head_FullHeadId)); + cfg.head_version = vec.at(static_cast(LolaEnums::RobotConfig::Head_Version)); + return cfg; +} diff --git a/nao_lola_client/src/nao_lola_client.cpp b/nao_lola_client/src/nao_lola_client.cpp new file mode 100644 index 0000000..33fe6f0 --- /dev/null +++ b/nao_lola_client/src/nao_lola_client.cpp @@ -0,0 +1,190 @@ +// Copyright 2021 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 +#include +#include "nao_lola_client/nao_lola_client.hpp" +#include "nao_lola_client/msgpack_parser.hpp" + +NaoLolaClient::NaoLolaClient() +: Node("NaoLolaClient") +{ + createPublishers(); + createSubscriptions(); + + // Start receive and send loop + receive_thread_ = std::thread( + [this]() { + while (rclcpp::ok()) { + auto recvData = connection.receive(); + MsgpackParser parsed(recvData.data(), recvData.size()); + + accelerometer_pub->publish(parsed.getAccelerometer()); + angle_pub->publish(parsed.getAngle()); + buttons_pub->publish(parsed.getButtons()); + fsr_pub->publish(parsed.getFSR()); + gyroscope_pub->publish(parsed.getGyroscope()); + joint_positions_pub->publish(parsed.getJointPositions()); + joint_stiffnesses_pub->publish(parsed.getJointStiffnesses()); + joint_temperatures_pub->publish(parsed.getJointTemperatures()); + joint_currents_pub->publish(parsed.getJointCurrents()); + joint_statuses_pub->publish(parsed.getJointStatuses()); + sonar_pub->publish(parsed.getSonar()); + touch_pub->publish(parsed.getTouch()); + battery_pub->publish(parsed.getBattery()); + robot_config_pub->publish(parsed.getRobotConfig()); + + + // In mutex, copy packer + // Do the pack and send outside mutex to avoid retain lock for a long time + MsgpackPacker packerCopy; + { + std::lock_guard guard(packer_mutex); + packerCopy = packer; + } + connection.send(packerCopy.getPacked()); + } + }); +} + +void NaoLolaClient::createPublishers() +{ + RCLCPP_DEBUG(get_logger(), "Initialise publishers"); + accelerometer_pub = create_publisher( + "sensors/accelerometer", 10); + angle_pub = create_publisher("sensors/angle", 10); + buttons_pub = create_publisher("sensors/buttons", 10); + fsr_pub = create_publisher("sensors/fsr", 10); + gyroscope_pub = create_publisher("sensors/gyroscope", 10); + joint_positions_pub = create_publisher( + "sensors/joint_positions", 10); + joint_stiffnesses_pub = create_publisher( + "sensors/joint_stiffnesses", 10); + joint_temperatures_pub = create_publisher( + "sensors/joint_temperatures", 10); + joint_currents_pub = create_publisher( + "sensors/joint_currents", 10); + joint_statuses_pub = create_publisher( + "sensors/joint_statuses", 10); + sonar_pub = create_publisher("sensors/sonar", 10); + touch_pub = create_publisher("sensors/touch", 10); + battery_pub = create_publisher("sensors/battery", 10); + robot_config_pub = + create_publisher("sensors/robot_config", 10); + RCLCPP_DEBUG(get_logger(), "Finished initialising publishers"); +} + +void NaoLolaClient::createSubscriptions() +{ + RCLCPP_DEBUG(get_logger(), "Initialise subscriptions"); + joint_positions_sub = + create_subscription( + "effectors/joint_positions", 1, + [this](const nao_lola_command_msgs::msg::JointPositions & jointPositions) { + std::lock_guard guard(packer_mutex); + packer.setJointPositions(jointPositions); + } + ); + + joint_stiffnesses_sub = + create_subscription( + "effectors/joint_stiffnesses", 1, + [this](const nao_lola_command_msgs::msg::JointStiffnesses & jointStiffnesses) { + std::lock_guard guard(packer_mutex); + packer.setJointStiffnesses(jointStiffnesses); + } + ); + + chest_led_sub = + create_subscription( + "effectors/chest_led", 1, + [this](const nao_lola_command_msgs::msg::ChestLed & chestLed) { + std::lock_guard guard(packer_mutex); + packer.setChestLed(chestLed); + } + ); + + left_ear_leds_sub = + create_subscription( + "effectors/left_ear_leds", 1, + [this](const nao_lola_command_msgs::msg::LeftEarLeds & leftEarLeds) { + std::lock_guard guard(packer_mutex); + packer.setLeftEarLeds(leftEarLeds); + } + ); + + right_ear_leds_sub = + create_subscription( + "effectors/right_ear_leds", 1, + [this](const nao_lola_command_msgs::msg::RightEarLeds & rightEarLeds) { + std::lock_guard guard(packer_mutex); + packer.setRightEarLeds(rightEarLeds); + } + ); + + left_eye_leds_sub = + create_subscription( + "effectors/left_eye_leds", 1, + [this](const nao_lola_command_msgs::msg::LeftEyeLeds & leftEyeLeds) { + std::lock_guard guard(packer_mutex); + packer.setLeftEyeLeds(leftEyeLeds); + } + ); + + right_eye_leds_sub = + create_subscription( + "effectors/right_eye_leds", 1, + [this](const nao_lola_command_msgs::msg::RightEyeLeds & rightEyeLeds) { + std::lock_guard guard(packer_mutex); + packer.setRightEyeLeds(rightEyeLeds); + } + ); + + left_foot_led_sub = + create_subscription( + "effectors/left_foot_led", 1, + [this](const nao_lola_command_msgs::msg::LeftFootLed & leftFootLed) { + std::lock_guard guard(packer_mutex); + packer.setLeftFootLed(leftFootLed); + } + ); + + right_foot_led_sub = + create_subscription( + "effectors/right_foot_led", 1, + [this](const nao_lola_command_msgs::msg::RightFootLed & rightFootLed) { + std::lock_guard guard(packer_mutex); + packer.setRightFootLed(rightFootLed); + } + ); + + head_leds_sub = + create_subscription( + "effectors/head_leds", 1, + [this](const nao_lola_command_msgs::msg::HeadLeds & headLeds) { + std::lock_guard guard(packer_mutex); + packer.setHeadLeds(headLeds); + } + ); + + sonar_usage_sub = + create_subscription( + "effectors/sonar_usage", 1, + [this](const nao_lola_command_msgs::msg::SonarUsage & sonarUsage) { + std::lock_guard guard(packer_mutex); + packer.setSonarUsage(sonarUsage); + } + ); + RCLCPP_DEBUG(get_logger(), "Finished creating subscriptions"); +} diff --git a/nao_lola_client/src/nao_lola_client_node.cpp b/nao_lola_client/src/nao_lola_client_node.cpp new file mode 100644 index 0000000..e12e15d --- /dev/null +++ b/nao_lola_client/src/nao_lola_client_node.cpp @@ -0,0 +1,25 @@ +// Copyright 2021 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 +#include "rclcpp/rclcpp.hpp" +#include "nao_lola_client/nao_lola_client.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/nao_lola_client/test/CMakeLists.txt b/nao_lola_client/test/CMakeLists.txt new file mode 100644 index 0000000..2fe3f47 --- /dev/null +++ b/nao_lola_client/test/CMakeLists.txt @@ -0,0 +1,15 @@ +# Build test_msgpack_parser +ament_add_gtest(test_msgpack_parser + test_msgpack_parser.cpp) + +target_link_libraries(test_msgpack_parser + msgpack_parser_lib +) + +# Build test_msgpack_packer +ament_add_gtest(test_msgpack_packer + test_msgpack_packer.cpp) + +target_link_libraries(test_msgpack_packer + msgpack_packer_lib +) \ No newline at end of file diff --git a/nao_lola_client/test/test_msgpack_packer.cpp b/nao_lola_client/test/test_msgpack_packer.cpp new file mode 100644 index 0000000..a6a778f --- /dev/null +++ b/nao_lola_client/test/test_msgpack_packer.cpp @@ -0,0 +1,292 @@ +// Copyright 2021 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 +#include +#include +#include +#include +#include "msgpack.hpp" +#include "nao_lola_client/msgpack_packer.hpp" +#include "nao_lola_command_msgs/msg/joint_positions.hpp" +#include "nao_lola_command_msgs/msg/joint_stiffnesses.hpp" +#include "nao_lola_command_msgs/msg/joint_indexes.hpp" +#include "nao_lola_client/lola_enums.hpp" + + +static std::vector getFloatVector(std::string packed, std::string mapKey); +static std::vector getBoolVector(std::string packed, std::string mapKey); +static std::map unpack(std::string packed); + +class TestMsgpackPacker : public ::testing::Test +{ +public: + MsgpackPacker packer; +}; + +TEST_F(TestMsgpackPacker, TestJointPositions) +{ + nao_lola_command_msgs::msg::JointPositions command; + command.indexes.push_back(nao_lola_command_msgs::msg::JointIndexes::HEADYAW); + command.positions.push_back(1.01); + command.indexes.push_back(nao_lola_command_msgs::msg::JointIndexes::RHAND); + command.positions.push_back(2.0); + + packer.setJointPositions(command); + std::string packed = packer.getPacked(); + + std::vector position(static_cast(LolaEnums::Joint::NUM_JOINTS), 0); + position.at(static_cast(LolaEnums::Joint::HeadYaw)) = 1.01; + position.at(static_cast(LolaEnums::Joint::RHand)) = 2.0; + EXPECT_EQ(getFloatVector(packed, "Position"), position); +} + +TEST_F(TestMsgpackPacker, TestJointStiffnesses) +{ + nao_lola_command_msgs::msg::JointStiffnesses command; + command.indexes.push_back(nao_lola_command_msgs::msg::JointIndexes::HEADPITCH); + command.stiffnesses.push_back(0.3); + command.indexes.push_back(nao_lola_command_msgs::msg::JointIndexes::LHAND); + command.stiffnesses.push_back(0.7); + + packer.setJointStiffnesses(command); + std::string packed = packer.getPacked(); + + std::vector stiffness(static_cast(LolaEnums::Joint::NUM_JOINTS), 0); + stiffness.at(static_cast(LolaEnums::Joint::HeadPitch)) = 0.3; + stiffness.at(static_cast(LolaEnums::Joint::LHand)) = 0.7; + EXPECT_EQ(getFloatVector(packed, "Stiffness"), stiffness); +} + +TEST_F(TestMsgpackPacker, TestChestLed) +{ + nao_lola_command_msgs::msg::ChestLed chestLed; + chestLed.color.r = 0.1; + chestLed.color.g = 0.5; + chestLed.color.b = 1.0; + + packer.setChestLed(chestLed); + std::string packed = packer.getPacked(); + + std::vector expected{0.1, 0.5, 1.0}; + EXPECT_EQ(getFloatVector(packed, "Chest"), expected); +} + +TEST_F(TestMsgpackPacker, TestLeftEarLeds) +{ + nao_lola_command_msgs::msg::LeftEarLeds leftEarLeds; + leftEarLeds.intensities[leftEarLeds.L0] = 0.1; + leftEarLeds.intensities[leftEarLeds.L1] = 0.2; + leftEarLeds.intensities[leftEarLeds.L2] = 0.3; + leftEarLeds.intensities[leftEarLeds.L3] = 0.4; + leftEarLeds.intensities[leftEarLeds.L4] = 0.5; + leftEarLeds.intensities[leftEarLeds.L5] = 0.6; + leftEarLeds.intensities[leftEarLeds.L6] = 0.7; + leftEarLeds.intensities[leftEarLeds.L7] = 0.8; + leftEarLeds.intensities[leftEarLeds.L8] = 0.9; + leftEarLeds.intensities[leftEarLeds.L9] = 1.0; + + packer.setLeftEarLeds(leftEarLeds); + std::string packed = packer.getPacked(); + + std::vector expected{ + 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0}; + EXPECT_EQ(getFloatVector(packed, "LEar"), expected); +} + + +TEST_F(TestMsgpackPacker, TestRightEarLeds) +{ + nao_lola_command_msgs::msg::RightEarLeds rightEarLeds; + rightEarLeds.intensities[rightEarLeds.R0] = 0.1; + rightEarLeds.intensities[rightEarLeds.R1] = 0.2; + rightEarLeds.intensities[rightEarLeds.R2] = 0.3; + rightEarLeds.intensities[rightEarLeds.R3] = 0.4; + rightEarLeds.intensities[rightEarLeds.R4] = 0.5; + rightEarLeds.intensities[rightEarLeds.R5] = 0.6; + rightEarLeds.intensities[rightEarLeds.R6] = 0.7; + rightEarLeds.intensities[rightEarLeds.R7] = 0.8; + rightEarLeds.intensities[rightEarLeds.R8] = 0.9; + rightEarLeds.intensities[rightEarLeds.R9] = 1.0; + + packer.setRightEarLeds(rightEarLeds); + std::string packed = packer.getPacked(); + + std::vector expected{ + 1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1}; + EXPECT_EQ(getFloatVector(packed, "REar"), expected); +} + +TEST_F(TestMsgpackPacker, TestLeftEyeLeds) +{ + // Explanation of eye correspondence: http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#nao-v5-v4-and-v3-3 + nao_lola_command_msgs::msg::LeftEyeLeds leftEyeLeds; + leftEyeLeds.colors[leftEyeLeds.L0].r = 0.01; + leftEyeLeds.colors[leftEyeLeds.L0].g = 0.02; + leftEyeLeds.colors[leftEyeLeds.L0].b = 0.03; + leftEyeLeds.colors[leftEyeLeds.L1].r = 0.04; + leftEyeLeds.colors[leftEyeLeds.L1].g = 0.05; + leftEyeLeds.colors[leftEyeLeds.L1].b = 0.06; + leftEyeLeds.colors[leftEyeLeds.L2].r = 0.07; + leftEyeLeds.colors[leftEyeLeds.L2].g = 0.08; + leftEyeLeds.colors[leftEyeLeds.L2].b = 0.09; + leftEyeLeds.colors[leftEyeLeds.L3].r = 0.10; + leftEyeLeds.colors[leftEyeLeds.L3].g = 0.11; + leftEyeLeds.colors[leftEyeLeds.L3].b = 0.12; + leftEyeLeds.colors[leftEyeLeds.L4].r = 0.13; + leftEyeLeds.colors[leftEyeLeds.L4].g = 0.14; + leftEyeLeds.colors[leftEyeLeds.L4].b = 0.15; + leftEyeLeds.colors[leftEyeLeds.L5].r = 0.16; + leftEyeLeds.colors[leftEyeLeds.L5].g = 0.17; + leftEyeLeds.colors[leftEyeLeds.L5].b = 0.18; + leftEyeLeds.colors[leftEyeLeds.L6].r = 0.19; + leftEyeLeds.colors[leftEyeLeds.L6].g = 0.20; + leftEyeLeds.colors[leftEyeLeds.L6].b = 0.21; + leftEyeLeds.colors[leftEyeLeds.L7].r = 0.22; + leftEyeLeds.colors[leftEyeLeds.L7].g = 0.23; + leftEyeLeds.colors[leftEyeLeds.L7].b = 0.24; + + packer.setLeftEyeLeds(leftEyeLeds); + std::string packed = packer.getPacked(); + + std::vector expected{ + 0.01, 0.04, 0.07, 0.10, 0.13, 0.16, 0.19, 0.22, + 0.02, 0.05, 0.08, 0.11, 0.14, 0.17, 0.20, 0.23, + 0.03, 0.06, 0.09, 0.12, 0.15, 0.18, 0.21, 0.24}; + EXPECT_EQ(getFloatVector(packed, "LEye"), expected); +} + +TEST_F(TestMsgpackPacker, TestRightEyeLeds) +{ + // Explanation of eye correspondence: http://doc.aldebaran.com/2-5/family/robots/leds_robot.html#nao-v5-v4-and-v3-3 + nao_lola_command_msgs::msg::RightEyeLeds rightEyeLeds; + rightEyeLeds.colors[rightEyeLeds.R0].r = 0.01; + rightEyeLeds.colors[rightEyeLeds.R0].g = 0.02; + rightEyeLeds.colors[rightEyeLeds.R0].b = 0.03; + rightEyeLeds.colors[rightEyeLeds.R1].r = 0.04; + rightEyeLeds.colors[rightEyeLeds.R1].g = 0.05; + rightEyeLeds.colors[rightEyeLeds.R1].b = 0.06; + rightEyeLeds.colors[rightEyeLeds.R2].r = 0.07; + rightEyeLeds.colors[rightEyeLeds.R2].g = 0.08; + rightEyeLeds.colors[rightEyeLeds.R2].b = 0.09; + rightEyeLeds.colors[rightEyeLeds.R3].r = 0.10; + rightEyeLeds.colors[rightEyeLeds.R3].g = 0.11; + rightEyeLeds.colors[rightEyeLeds.R3].b = 0.12; + rightEyeLeds.colors[rightEyeLeds.R4].r = 0.13; + rightEyeLeds.colors[rightEyeLeds.R4].g = 0.14; + rightEyeLeds.colors[rightEyeLeds.R4].b = 0.15; + rightEyeLeds.colors[rightEyeLeds.R5].r = 0.16; + rightEyeLeds.colors[rightEyeLeds.R5].g = 0.17; + rightEyeLeds.colors[rightEyeLeds.R5].b = 0.18; + rightEyeLeds.colors[rightEyeLeds.R6].r = 0.19; + rightEyeLeds.colors[rightEyeLeds.R6].g = 0.20; + rightEyeLeds.colors[rightEyeLeds.R6].b = 0.21; + rightEyeLeds.colors[rightEyeLeds.R7].r = 0.22; + rightEyeLeds.colors[rightEyeLeds.R7].g = 0.23; + rightEyeLeds.colors[rightEyeLeds.R7].b = 0.24; + + packer.setRightEyeLeds(rightEyeLeds); + std::string packed = packer.getPacked(); + + std::vector expected{ + 0.22, 0.19, 0.16, 0.13, 0.10, 0.07, 0.04, 0.01, + 0.23, 0.20, 0.17, 0.14, 0.11, 0.08, 0.05, 0.02, + 0.24, 0.21, 0.18, 0.15, 0.12, 0.09, 0.06, 0.03}; + EXPECT_EQ(getFloatVector(packed, "REye"), expected); +} + +TEST_F(TestMsgpackPacker, TestLeftFootLed) +{ + nao_lola_command_msgs::msg::LeftFootLed leftFootLed; + leftFootLed.color.r = 0.2; + leftFootLed.color.g = 0.3; + leftFootLed.color.b = 0.4; + + packer.setLeftFootLed(leftFootLed); + std::string packed = packer.getPacked(); + + std::vector expected{0.2, 0.3, 0.4}; + EXPECT_EQ(getFloatVector(packed, "LFoot"), expected); +} + +TEST_F(TestMsgpackPacker, TestRightFootLed) +{ + nao_lola_command_msgs::msg::RightFootLed rightFootLed; + rightFootLed.color.r = 0.5; + rightFootLed.color.g = 0.6; + rightFootLed.color.b = 0.7; + + packer.setRightFootLed(rightFootLed); + std::string packed = packer.getPacked(); + + std::vector expected{0.5, 0.6, 0.7}; + EXPECT_EQ(getFloatVector(packed, "RFoot"), expected); +} + +TEST_F(TestMsgpackPacker, TestHeadLeds) +{ + nao_lola_command_msgs::msg::HeadLeds headLeds; + headLeds.intensities[headLeds.B0] = 0.00; + headLeds.intensities[headLeds.B1] = 0.01; + headLeds.intensities[headLeds.B2] = 0.02; + headLeds.intensities[headLeds.B3] = 0.03; + headLeds.intensities[headLeds.B4] = 0.04; + headLeds.intensities[headLeds.B5] = 0.05; + headLeds.intensities[headLeds.B6] = 0.06; + headLeds.intensities[headLeds.B7] = 0.07; + headLeds.intensities[headLeds.B8] = 0.08; + headLeds.intensities[headLeds.B9] = 0.09; + headLeds.intensities[headLeds.B10] = 0.10; + headLeds.intensities[headLeds.B11] = 0.11; + + packer.setHeadLeds(headLeds); + std::string packed = packer.getPacked(); + + std::vector expected{ + 0.11, 0.10, 0.09, 0.08, 0.07, 0.06, 0.05, 0.04, 0.03, 0.02, 0.01, 0.00}; + EXPECT_EQ(getFloatVector(packed, "Skull"), expected); +} + +TEST_F(TestMsgpackPacker, TestSonarUsage) +{ + nao_lola_command_msgs::msg::SonarUsage sonarUsage; + sonarUsage.left = true; + sonarUsage.right = false; + + packer.setSonarUsage(sonarUsage); + std::string packed = packer.getPacked(); + + std::vector expected{true, false}; + EXPECT_EQ(getBoolVector(packed, "Sonar"), expected); +} + +// Helper functions +static std::vector getFloatVector(std::string packed, std::string mapKey) +{ + std::map unpacked = unpack(packed); + return unpacked.at(mapKey).as>(); +} + +static std::vector getBoolVector(std::string packed, std::string mapKey) +{ + std::map unpacked = unpack(packed); + return unpacked.at(mapKey).as>(); +} + +static std::map unpack(std::string packed) +{ + msgpack::object_handle oh = + msgpack::unpack(packed.data(), packed.size()); + return oh.get().as>(); +} diff --git a/nao_lola_client/test/test_msgpack_parser.cpp b/nao_lola_client/test/test_msgpack_parser.cpp new file mode 100644 index 0000000..8149879 --- /dev/null +++ b/nao_lola_client/test/test_msgpack_parser.cpp @@ -0,0 +1,231 @@ +// Copyright 2021 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 +#include +#include +#include +#include +#include "nao_lola_client/msgpack_parser.hpp" +#include "nao_lola_sensor_msgs/msg/joint_indexes.hpp" + +std::vector status = +{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3}; +std::vector stiffness = +{0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8}; +std::vector accelerometer = {-3.0656251907348633, -0.39278322458267212, -9.3214168548583984}; +std::vector battery = {0.9, 0.5, 0.0, 37.0}; +std::vector current = +{0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2}; +std::vector touch = {1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0}; +std::vector fsr = +{0.014380865730345249, 0.29265055060386658, 0.47892898321151733, 0.62120223045349121, + 0.28502300381660461, 0.70163685083389282, 0.40598109364509583, 0.086648054420948029}; +std::vector angles = {0.037582572549581528, -0.35991066694259644}; +std::vector position = +{0.59361600875854492, 0.49544000625610352, 1.2133520841598511, 0.33283615112304688, + 0.76849198341369629, -0.16869807243347168, -0.39427995681762695, -0.50617814064025879, + 0.297637939453125, -0.34050607681274414, 2.1506261825561523, -1.1137261390686035, + -0.062852144241333008, -0.10733795166015625, -0.23014187812805176, 2.1399722099304199, + -1.2056820392608643, -0.10120201110839844, 1.0600361824035645, 0.2039799690246582, + -0.46791195869445801, 0.066004037857055664, 0.47089600563049316, 0.010400056838989258, + 0.011199951171875}; +std::vector sonar = {0.3, 1.3}; +std::vector gyroscope = +{-0.00026631611399352551, -0.001065264455974102, 0.001065264455974102}; +std::vector temperature = +{38.0, 38.0, 38.0, 38.0, 38.0, 38.0, 38.0, 33.0, 27.0, 27.0, 27.0, 27.0, + 27.0, 27.0, 27.0, 27.0, 27.0, 27.0, 38.0, 38.0, 38.0, 38.0, 38.0, 38.0, 39.0}; +std::vector robotConfig = +{"P0000073A07S94700012", "6.0.0", "P0000074A05S93M00061", "6.0.0"}; + +class TestMsgpackParser : public ::testing::Test +{ +public: + std::shared_ptr parser; + +protected: + virtual void SetUp() + { + // A way of packing a hashmap containing floats and strings and other + // data types are explained in this comment: + // https://github.com/msgpack/msgpack-c/issues/651#issuecomment-365197261 + msgpack::zone z; + + std::map map{ + {"Status", msgpack::object(status, z)}, + {"Stiffness", msgpack::object(stiffness, z)}, + {"Accelerometer", msgpack::object(accelerometer, z)}, + {"Battery", msgpack::object(battery, z)}, + {"Current", msgpack::object(current, z)}, + {"Touch", msgpack::object(touch, z)}, + {"FSR", msgpack::object(fsr, z)}, + {"Angles", msgpack::object(angles, z)}, + {"Position", msgpack::object(position, z)}, + {"Sonar", msgpack::object(sonar, z)}, + {"Gyroscope", msgpack::object(gyroscope, z)}, + {"Temperature", msgpack::object(temperature, z)}, + {"RobotConfig", msgpack::object(robotConfig, z)} + }; + + // serialize the buffer + std::stringstream buffer; + msgpack::pack(buffer, map); + + // deserialize the buffer + // DO NOT use a std::string because serialized data may contain null characters + char c; + std::vector packed; + packed.reserve(1000); + while (buffer.get(c)) { + packed.push_back(c); + } + parser = std::make_shared(packed.data(), packed.size()); + } +}; + +TEST_F(TestMsgpackParser, TestAccelerometer) +{ + nao_lola_sensor_msgs::msg::Accelerometer acc = parser->getAccelerometer(); + EXPECT_NEAR(acc.x, -3.0656251907348633, 0.000001); + EXPECT_NEAR(acc.y, -0.39278322458267212, 0.000001); + EXPECT_NEAR(acc.z, -9.3214168548583984, 0.000001); +} + +TEST_F(TestMsgpackParser, TestAngle) +{ + nao_lola_sensor_msgs::msg::Angle ang = parser->getAngle(); + EXPECT_NEAR(ang.x, 0.037582572549581528, 0.000001); + EXPECT_NEAR(ang.y, -0.35991066694259644, 0.000001); +} + +TEST_F(TestMsgpackParser, TestButtons) +{ + nao_lola_sensor_msgs::msg::Buttons but = parser->getButtons(); + EXPECT_TRUE(but.chest); + EXPECT_TRUE(but.l_foot_bumper_left); + EXPECT_FALSE(but.l_foot_bumper_right); + EXPECT_FALSE(but.r_foot_bumper_left); + EXPECT_TRUE(but.r_foot_bumper_right); +} + +TEST_F(TestMsgpackParser, TestFSR) +{ + nao_lola_sensor_msgs::msg::FSR fsr = parser->getFSR(); + EXPECT_NEAR(fsr.l_foot_front_left, 0.014380865730345249, 0.000001); + EXPECT_NEAR(fsr.l_foot_front_right, 0.29265055060386658, 0.000001); + EXPECT_NEAR(fsr.l_foot_back_left, 0.47892898321151733, 0.000001); + EXPECT_NEAR(fsr.l_foot_back_right, 0.62120223045349121, 0.000001); + EXPECT_NEAR(fsr.r_foot_front_left, 0.28502300381660461, 0.000001); + EXPECT_NEAR(fsr.r_foot_front_right, 0.70163685083389282, 0.000001); + EXPECT_NEAR(fsr.r_foot_back_left, 0.40598109364509583, 0.000001); + EXPECT_NEAR(fsr.r_foot_back_right, 0.086648054420948029, 0.000001); +} + +TEST_F(TestMsgpackParser, TestGyroscope) +{ + nao_lola_sensor_msgs::msg::Gyroscope gyr = parser->getGyroscope(); + EXPECT_NEAR(gyr.x, -0.00026631611399352551, 0.000001); + EXPECT_NEAR(gyr.y, -0.001065264455974102, 0.000001); + EXPECT_NEAR(gyr.z, 0.001065264455974102, 0.000001); +} + +TEST_F(TestMsgpackParser, TestJointPositions) +{ + nao_lola_sensor_msgs::msg::JointPositions jointPositions = parser->getJointPositions(); + EXPECT_NEAR( + jointPositions.positions.at( + nao_lola_sensor_msgs::msg::JointIndexes::HEADYAW), 0.59361600875854492, 0.000001); + EXPECT_NEAR( + jointPositions.positions.at( + nao_lola_sensor_msgs::msg::JointIndexes::RHAND), 0.011199951171875, 0.00001); +} + +TEST_F(TestMsgpackParser, TestJointStiffnesses) +{ + nao_lola_sensor_msgs::msg::JointStiffnesses jointStiffnesses = parser->getJointStiffnesses(); + EXPECT_NEAR( + jointStiffnesses.stiffnesses.at( + nao_lola_sensor_msgs::msg::JointIndexes::HEADYAW), 0.3, 0.000001); + EXPECT_NEAR( + jointStiffnesses.stiffnesses.at( + nao_lola_sensor_msgs::msg::JointIndexes::RHAND), 0.8, 0.00001); +} + +TEST_F(TestMsgpackParser, TestJointTemperatures) +{ + nao_lola_sensor_msgs::msg::JointTemperatures jointTemperatures = parser->getJointTemperatures(); + EXPECT_NEAR( + jointTemperatures.temperatures.at( + nao_lola_sensor_msgs::msg::JointIndexes::HEADYAW), 38.0, 0.000001); + EXPECT_NEAR( + jointTemperatures.temperatures.at( + nao_lola_sensor_msgs::msg::JointIndexes::RHAND), 39.0, 0.00001); +} + +TEST_F(TestMsgpackParser, TestJointCurrents) +{ + nao_lola_sensor_msgs::msg::JointCurrents jointCurrents = parser->getJointCurrents(); + EXPECT_NEAR( + jointCurrents.currents.at(nao_lola_sensor_msgs::msg::JointIndexes::HEADYAW), 0.1, + 0.000001); + EXPECT_NEAR( + jointCurrents.currents.at( + nao_lola_sensor_msgs::msg::JointIndexes::RHAND), 0.2, 0.00001); +} + +TEST_F(TestMsgpackParser, TestJointStatuses) +{ + nao_lola_sensor_msgs::msg::JointStatuses jointStatuses = parser->getJointStatuses(); + EXPECT_EQ(jointStatuses.statuses.at(nao_lola_sensor_msgs::msg::JointIndexes::HEADYAW), 1); + EXPECT_EQ(jointStatuses.statuses.at(nao_lola_sensor_msgs::msg::JointIndexes::RHAND), 3); +} + +TEST_F(TestMsgpackParser, TestSonar) +{ + nao_lola_sensor_msgs::msg::Sonar snr = parser->getSonar(); + EXPECT_NEAR(snr.left, 0.3, 0.000001); + EXPECT_NEAR(snr.right, 1.3, 0.000001); +} + +TEST_F(TestMsgpackParser, TestTouch) +{ + nao_lola_sensor_msgs::msg::Touch tch = parser->getTouch(); + EXPECT_FALSE(tch.head_front); + EXPECT_TRUE(tch.head_middle); + EXPECT_FALSE(tch.head_rear); +} + +TEST_F(TestMsgpackParser, TestBattery) +{ + nao_lola_sensor_msgs::msg::Battery btr = parser->getBattery(); + // From Lola, charge has range 0.0 - 1.0, but in Battery msg, we have 0.0% - 100.0%. + // So, we multiply by 100 below. + EXPECT_NEAR(btr.charge, 0.9 * 100, 0.000001); + EXPECT_NEAR(btr.current, 0.5, 0.000001); + EXPECT_FALSE(btr.charging); + EXPECT_NEAR(btr.temperature, 37.0, 0.000001); +} + + +TEST_F(TestMsgpackParser, TestRobotConfig) +{ + nao_lola_sensor_msgs::msg::RobotConfig robotConfig = parser->getRobotConfig(); + EXPECT_EQ(robotConfig.body_id, "P0000073A07S94700012"); + EXPECT_EQ(robotConfig.body_version, "6.0.0"); + EXPECT_EQ(robotConfig.head_id, "P0000074A05S93M00061"); + EXPECT_EQ(robotConfig.head_version, "6.0.0"); +} diff --git a/nao_lola_command_msgs/CHANGELOG.rst b/nao_lola_command_msgs/CHANGELOG.rst new file mode 100644 index 0000000..e6b1047 --- /dev/null +++ b/nao_lola_command_msgs/CHANGELOG.rst @@ -0,0 +1,15 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nao_command_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.4 (2022-01-24) +------------------ +* update comments in JointPositions and JointStiffnesses +* update package descriptions, and remove unused dependency +* Contributors: Kenji Brameld + +0.0.3 (2021-07-17) +------------------ +* move all nao_command_msgs files that were in wrong directory +* separated msgs into two different packages, sensor and command packages +* Contributors: Kenji Brameld diff --git a/nao_lola_command_msgs/CMakeLists.txt b/nao_lola_command_msgs/CMakeLists.txt new file mode 100644 index 0000000..554da00 --- /dev/null +++ b/nao_lola_command_msgs/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5) +project(nao_lola_command_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ChestLed.msg" + "msg/HeadLeds.msg" + "msg/JointIndexes.msg" + "msg/JointPositions.msg" + "msg/JointStiffnesses.msg" + "msg/LeftEarLeds.msg" + "msg/LeftEyeLeds.msg" + "msg/LeftFootLed.msg" + "msg/RightEarLeds.msg" + "msg/RightEyeLeds.msg" + "msg/RightFootLed.msg" + "msg/SonarUsage.msg" + DEPENDENCIES std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/nao_lola_command_msgs/msg/ChestLed.msg b/nao_lola_command_msgs/msg/ChestLed.msg new file mode 100644 index 0000000..69a9688 --- /dev/null +++ b/nao_lola_command_msgs/msg/ChestLed.msg @@ -0,0 +1,4 @@ +# Expect ranges for R, G and B are 0.0 - 1.0. +# The alpha value (A) is ignored. + +std_msgs/ColorRGBA color diff --git a/nao_lola_command_msgs/msg/HeadLeds.msg b/nao_lola_command_msgs/msg/HeadLeds.msg new file mode 100644 index 0000000..5885da9 --- /dev/null +++ b/nao_lola_command_msgs/msg/HeadLeds.msg @@ -0,0 +1,17 @@ +# For locations, refer to https://developer.softbankrobotics.com/nao6/nao-documentation/nao-developer-guide/technical-overview/leds#-head-tactile-sensor-led-locations--sbr-fake-anchor + +int32 B0=0 +int32 B1=1 +int32 B2=2 +int32 B3=3 +int32 B4=4 +int32 B5=5 +int32 B6=6 +int32 B7=7 +int32 B8=8 +int32 B9=9 +int32 B10=10 +int32 B11=11 +int32 NUM_LEDS=12 + +float32[12] intensities # 0.0 - 1.0 \ No newline at end of file diff --git a/nao_lola_command_msgs/msg/JointIndexes.msg b/nao_lola_command_msgs/msg/JointIndexes.msg new file mode 100644 index 0000000..c8340df --- /dev/null +++ b/nao_lola_command_msgs/msg/JointIndexes.msg @@ -0,0 +1,26 @@ +uint8 HEADYAW=0 +uint8 HEADPITCH=1 +uint8 LSHOULDERPITCH=2 +uint8 LSHOULDERROLL=3 +uint8 LELBOWYAW=4 +uint8 LELBOWROLL=5 +uint8 LWRISTYAW=6 +uint8 LHIPYAWPITCH=7 +uint8 LHIPROLL=8 +uint8 LHIPPITCH=9 +uint8 LKNEEPITCH=10 +uint8 LANKLEPITCH=11 +uint8 LANKLEROLL=12 +uint8 RHIPROLL=13 +uint8 RHIPPITCH=14 +uint8 RKNEEPITCH=15 +uint8 RANKLEPITCH=16 +uint8 RANKLEROLL=17 +uint8 RSHOULDERPITCH=18 +uint8 RSHOULDERROLL=19 +uint8 RELBOWYAW=20 +uint8 RELBOWROLL=21 +uint8 RWRISTYAW=22 +uint8 LHAND=23 +uint8 RHAND=24 +uint8 NUMJOINTS=25 \ No newline at end of file diff --git a/nao_lola_command_msgs/msg/JointPositions.msg b/nao_lola_command_msgs/msg/JointPositions.msg new file mode 100644 index 0000000..aee5e8e --- /dev/null +++ b/nao_lola_command_msgs/msg/JointPositions.msg @@ -0,0 +1,9 @@ +# Message to specify positions for the NAO's motor joints. +# +# Each joint is uniquely identified by its index (See JointIndexes.msg) +# +# The two arrays in this message should have the same size, where the first item +# in indexes, corresponds to the first item in positions, etc. + +uint8[] indexes # See JointIndexes.msg (eg. JointIndexes::HEADYAW) +float32[] positions # radians \ No newline at end of file diff --git a/nao_lola_command_msgs/msg/JointStiffnesses.msg b/nao_lola_command_msgs/msg/JointStiffnesses.msg new file mode 100644 index 0000000..de9808d --- /dev/null +++ b/nao_lola_command_msgs/msg/JointStiffnesses.msg @@ -0,0 +1,9 @@ +# Message to specify stiffnesses for the NAO's motor joints. +# +# Each joint is uniquely identified by its index (See JointIndexes.msg) +# +# The two arrays in this message should have the same size, where the first item +# in indexes, corresponds to the first item in stiffnesses, etc. + +uint8[] indexes # See JointIndexes.msg (eg. JointIndexes::HEADYAW) +float32[] stiffnesses # 0.0 - 1.0 \ No newline at end of file diff --git a/nao_lola_command_msgs/msg/LeftEarLeds.msg b/nao_lola_command_msgs/msg/LeftEarLeds.msg new file mode 100644 index 0000000..d4eb197 --- /dev/null +++ b/nao_lola_command_msgs/msg/LeftEarLeds.msg @@ -0,0 +1,16 @@ +# For locations, refer to +# https://developer.softbankrobotics.com/nao6/nao-documentation/nao-developer-guide/technical-overview/leds#-left-ear--sbr-fake-anchor + +int32 L0=0 +int32 L1=1 +int32 L2=2 +int32 L3=3 +int32 L4=4 +int32 L5=5 +int32 L6=6 +int32 L7=7 +int32 L8=8 +int32 L9=9 +int32 NUM_LEDS=10 + +float32[10] intensities # 0.0 - 1.0 \ No newline at end of file diff --git a/nao_lola_command_msgs/msg/LeftEyeLeds.msg b/nao_lola_command_msgs/msg/LeftEyeLeds.msg new file mode 100644 index 0000000..1b53c5f --- /dev/null +++ b/nao_lola_command_msgs/msg/LeftEyeLeds.msg @@ -0,0 +1,34 @@ +# Message identifying colors for each RGB LED in the NAO's eyes. +# Expect ranges for R, G and B are 0.0 - 1.0. +# The alpha value (A) is ignored. + +# LED locations and their ids are shown below +# _ # +# / (_) \ # +# / \ # +# / L0 \ # +# | ____ L1 ____ L7 | # +# | / \ / \ | # +# | | ** | L2 | ** | L6 | # +# | \____/ \____/ | # +# | L3 L5 | # +# \ L4 / # +# \ / # +# \ ___ / # +# \__ \_/ __/ # +# \__ ___/ # +# \_______________________/ # + +# For more details, refer to http://doc.aldebaran.com/2-1/family/robots/leds_robot.html#nao-v5-v4-and-v3-3 + +int32 L0=0 +int32 L1=1 +int32 L2=2 +int32 L3=3 +int32 L4=4 +int32 L5=5 +int32 L6=6 +int32 L7=7 +int32 NUM_LEDS=8 + +std_msgs/ColorRGBA[8] colors \ No newline at end of file diff --git a/nao_lola_command_msgs/msg/LeftFootLed.msg b/nao_lola_command_msgs/msg/LeftFootLed.msg new file mode 100644 index 0000000..c212066 --- /dev/null +++ b/nao_lola_command_msgs/msg/LeftFootLed.msg @@ -0,0 +1,4 @@ +# Expect ranges for R, G and B are 0.0 - 1.0. +# The alpha value (A) is ignored. + +std_msgs/ColorRGBA color \ No newline at end of file diff --git a/nao_lola_command_msgs/msg/RightEarLeds.msg b/nao_lola_command_msgs/msg/RightEarLeds.msg new file mode 100644 index 0000000..5347304 --- /dev/null +++ b/nao_lola_command_msgs/msg/RightEarLeds.msg @@ -0,0 +1,16 @@ +# For locations, refer to +# https://developer.softbankrobotics.com/nao6/nao-documentation/nao-developer-guide/technical-overview/leds#-right-ear--sbr-fake-anchor + +int32 R0=0 +int32 R1=1 +int32 R2=2 +int32 R3=3 +int32 R4=4 +int32 R5=5 +int32 R6=6 +int32 R7=7 +int32 R8=8 +int32 R9=9 +int32 NUM_LEDS=10 + +float32[10] intensities # 0.0 - 1.0 \ No newline at end of file diff --git a/nao_lola_command_msgs/msg/RightEyeLeds.msg b/nao_lola_command_msgs/msg/RightEyeLeds.msg new file mode 100644 index 0000000..37c4278 --- /dev/null +++ b/nao_lola_command_msgs/msg/RightEyeLeds.msg @@ -0,0 +1,34 @@ +# Message identifying colors for each RGB LED in the NAO's right eye. +# Expect ranges for R, G and B are 0.0 - 1.0. +# The alpha value (A) is ignored. + +# LED locations and their ids are shown below +# _ # +# / (_) \ # +# / \ # +# / R0 \ # +# | R7 ____ R1 ____ | # +# | / \ / \ | # +# | R6 | ** | R2 | ** | | # +# | \____/ \____/ | # +# | R5 R3 | # +# \ R4 / # +# \ / # +# \ ___ / # +# \__ \_/ __/ # +# \__ ___/ # +# \_______________________/ # + +# For more details, refer to http://doc.aldebaran.com/2-1/family/robots/leds_robot.html#nao-v5-v4-and-v3-3 + +int32 R0=0 +int32 R1=1 +int32 R2=2 +int32 R3=3 +int32 R4=4 +int32 R5=5 +int32 R6=6 +int32 R7=7 +int32 NUM_LEDS=8 + +std_msgs/ColorRGBA[8] colors \ No newline at end of file diff --git a/nao_lola_command_msgs/msg/RightFootLed.msg b/nao_lola_command_msgs/msg/RightFootLed.msg new file mode 100644 index 0000000..cfe53e1 --- /dev/null +++ b/nao_lola_command_msgs/msg/RightFootLed.msg @@ -0,0 +1,4 @@ +# Expect ranges for R, G and B are 0.0 - 1.0. +# The alpha value (A) is ignored. + +std_msgs/ColorRGBA color \ No newline at end of file diff --git a/nao_lola_command_msgs/msg/SonarUsage.msg b/nao_lola_command_msgs/msg/SonarUsage.msg new file mode 100644 index 0000000..ce67eaa --- /dev/null +++ b/nao_lola_command_msgs/msg/SonarUsage.msg @@ -0,0 +1,3 @@ +# This is used in commanding the robot whether or not to use the left and right sonars. +bool left # Set to true, to use left sonar +bool right # Set to true, to use right sonar \ No newline at end of file diff --git a/nao_lola_command_msgs/package.xml b/nao_lola_command_msgs/package.xml new file mode 100644 index 0000000..6d70d63 --- /dev/null +++ b/nao_lola_command_msgs/package.xml @@ -0,0 +1,24 @@ + + + + nao_lola_command_msgs + 0.0.4 + Package defining command msgs to be sent to NAO robot. + ijnek + Apache License 2.0 + + ament_cmake + + std_msgs + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/nao_lola_conversion/CMakeLists.txt b/nao_lola_conversion/CMakeLists.txt new file mode 100644 index 0000000..1a22f52 --- /dev/null +++ b/nao_lola_conversion/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.8) +project(nao_lola_conversion) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(message_filters REQUIRED) +find_package(nao_sensor_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + ament_cmake + message_filters + nao_sensor_msgs + rclcpp + rclcpp_components + sensor_msgs + tf2_geometry_msgs) + +# Build +add_library(${PROJECT_NAME}_node SHARED + src/nao_lola_conversion.cpp) +target_include_directories(${PROJECT_NAME}_node PUBLIC + $ + $) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "nao_lola_conversion::NaoLolaConversion" + EXECUTABLE ${PROJECT_NAME}) + +ament_target_dependencies(${PROJECT_NAME}_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install +install( + DIRECTORY include/ + DESTINATION include) +install( + TARGETS ${PROJECT_NAME}_node + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include) + +# Test +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/nao_lola_conversion/include/nao_lola_conversion/nao_lola_conversion.hpp b/nao_lola_conversion/include/nao_lola_conversion/nao_lola_conversion.hpp new file mode 100644 index 0000000..ef207f0 --- /dev/null +++ b/nao_lola_conversion/include/nao_lola_conversion/nao_lola_conversion.hpp @@ -0,0 +1,55 @@ +// 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. + +#ifndef NAO_LOLA_CONVERSION__NAO_LOLA_CONVERSION_HPP_ +#define NAO_LOLA_CONVERSION__NAO_LOLA_CONVERSION_HPP_ + +#include + +#include "message_filters/subscriber.h" +#include "message_filters/time_synchronizer.h" +#include "nao_sensor_msgs/msg/accelerometer.hpp" +#include "nao_sensor_msgs/msg/gyroscope.hpp" +#include "rclcpp/node.hpp" +#include "sensor_msgs/msg/imu.hpp" + +namespace nao_lola_conversion +{ + +class NaoLolaConversion : public rclcpp::Node +{ +public: + explicit NaoLolaConversion(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); + +private: + // Subscriptions + message_filters::Subscriber accelerometer_sub_; + message_filters::Subscriber gyroscope_sub_; + + // Publishers + rclcpp::Publisher::SharedPtr imu_pub_; + + // Synchronizer + std::shared_ptr> synchronizer_; + + // Callbacks + void synchronizerCallback( + const nao_sensor_msgs::msg::Accelerometer::SharedPtr & accelerometer, + const nao_sensor_msgs::msg::Gyroscope::SharedPtr & gyroscope); +}; + +} // namespace nao_lola_conversion + +#endif // NAO_LOLA_CONVERSION__NAO_LOLA_CONVERSION_HPP_ diff --git a/nao_lola_conversion/package.xml b/nao_lola_conversion/package.xml new file mode 100644 index 0000000..7522d23 --- /dev/null +++ b/nao_lola_conversion/package.xml @@ -0,0 +1,25 @@ + + + + nao_lola_conversion + 0.0.0 + A package that converts Nao Lola messages to / from common ROS interfaces + ijnek + Apache License 2.0 + + ament_cmake + + message_filters + nao_sensor_msgs + rclcpp + rclcpp_components + sensor_msgs + tf2_geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/nao_lola_conversion/src/nao_lola_conversion.cpp b/nao_lola_conversion/src/nao_lola_conversion.cpp new file mode 100644 index 0000000..a4ba81d --- /dev/null +++ b/nao_lola_conversion/src/nao_lola_conversion.cpp @@ -0,0 +1,67 @@ +// 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 "nao_lola_conversion/nao_lola_conversion.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace nao_lola_conversion +{ + +using nao_sensor_msgs::msg::Accelerometer; +using nao_sensor_msgs::msg::Gyroscope; +using sensor_msgs::msg::Imu; + +NaoLolaConversion::NaoLolaConversion(const rclcpp::NodeOptions & options) +: rclcpp::Node{"nao_lola_conversion", options} +{ + // Message filter subscribers + rclcpp::QoS qos(10); + auto rmw_qos_profile = qos.get_rmw_qos_profile(); + accelerometer_sub_.subscribe(this, "sensors/accelerometer", rmw_qos_profile); + gyroscope_sub_.subscribe(this, "sensors/gyroscope", rmw_qos_profile); + + // Publishers + imu_pub_ = create_publisher("sensors/imu", 10); + + // Synchronizer + synchronizer_ = + std::make_shared>( + accelerometer_sub_, gyroscope_sub_, 10); + synchronizer_->registerCallback(&NaoLolaConversion::synchronizerCallback, this); +} + +void NaoLolaConversion::synchronizerCallback( + const Accelerometer::SharedPtr & accelerometer, + const Gyroscope::SharedPtr & gyroscope) +{ + Imu imu; + imu.header.frame_id = "ImuTorsoAccelerometer_frame"; + + // 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; + + imu_pub_->publish(imu); +} + +} // namespace nao_lola_conversion + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(nao_lola_conversion::NaoLolaConversion) diff --git a/nao_lola_sensor_msgs/CMakeLists.txt b/nao_lola_sensor_msgs/CMakeLists.txt new file mode 100644 index 0000000..f1d5d18 --- /dev/null +++ b/nao_lola_sensor_msgs/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.5) +project(nao_lola_sensor_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Accelerometer.msg" + "msg/Angle.msg" + "msg/Battery.msg" + "msg/Buttons.msg" + "msg/FSR.msg" + "msg/Gyroscope.msg" + "msg/JointCurrents.msg" + "msg/JointIndexes.msg" + "msg/JointPositions.msg" + "msg/JointStatuses.msg" + "msg/JointStiffnesses.msg" + "msg/JointTemperatures.msg" + "msg/RobotConfig.msg" + "msg/Sonar.msg" + "msg/Touch.msg" +) + +ament_export_dependencies(rosidl_default_runtime) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/nao_lola_sensor_msgs/msg/Accelerometer.msg b/nao_lola_sensor_msgs/msg/Accelerometer.msg new file mode 100644 index 0000000..913224a --- /dev/null +++ b/nao_lola_sensor_msgs/msg/Accelerometer.msg @@ -0,0 +1,21 @@ +# Holds Accelerometer information for a NAO V6 +# Units are in m/s^2 +# Follows conventions here: http://doc.aldebaran.com/2-1/family/robots/inertial_robot.html +# Note that gravity is measured by an accelerometer. +# When standing, the robot will measure 9.8m/s^2 POSITIVE in the z-direction, since its +# receiving an acceleration upwards from the floor when compared to a freefall state. + +# x is moving forwards / backwards (from robots perspective) +# - Forwards is positive +# - Backwards is negative +float32 x + +# y is moving left / right (from robot's perspective) +# - Left is positive +# - Right is negative +float32 y + +# z is moving up / down +# - Standing straight is negative +# - Upside down is positive +float32 z \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/Angle.msg b/nao_lola_sensor_msgs/msg/Angle.msg new file mode 100644 index 0000000..6fcf2b3 --- /dev/null +++ b/nao_lola_sensor_msgs/msg/Angle.msg @@ -0,0 +1,12 @@ +# Holds Angle information for a NAO V6 +# Units are in rad + +# x axis is rotating Nao left / right (vertical rotation) +# - If Nao is facing towards you, rotating it to the left is positive +# - Conversely, rotating it to the right is negative +float32 x + +# y axis is rotating Nao back / forward +# - Forward is positive angle +# - Backwards is negative angle +float32 y \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/Battery.msg b/nao_lola_sensor_msgs/msg/Battery.msg new file mode 100644 index 0000000..6b4ee93 --- /dev/null +++ b/nao_lola_sensor_msgs/msg/Battery.msg @@ -0,0 +1,4 @@ +float32 charge # 0% - 100% +bool charging # True if robot is getting charged +float32 current # In Amperes, negative for discharge, positive for charge +float32 temperature # Celcius (°C) \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/Buttons.msg b/nao_lola_sensor_msgs/msg/Buttons.msg new file mode 100644 index 0000000..a0fbe1f --- /dev/null +++ b/nao_lola_sensor_msgs/msg/Buttons.msg @@ -0,0 +1,5 @@ +bool chest +bool l_foot_bumper_left +bool l_foot_bumper_right +bool r_foot_bumper_left +bool r_foot_bumper_right \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/FSR.msg b/nao_lola_sensor_msgs/msg/FSR.msg new file mode 100644 index 0000000..8166499 --- /dev/null +++ b/nao_lola_sensor_msgs/msg/FSR.msg @@ -0,0 +1,13 @@ +# FSR stands for Force Sensitive Resistors. +# These sensors measure a resistance change according to the pressure applied. +# Units used are in Newtons (equivalently kg/m/s^2) +# http://doc.aldebaran.com/2-1/family/robots/fsr_robot.html + +float32 l_foot_front_left +float32 l_foot_front_right +float32 l_foot_back_left +float32 l_foot_back_right +float32 r_foot_front_left +float32 r_foot_front_right +float32 r_foot_back_left +float32 r_foot_back_right \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/Gyroscope.msg b/nao_lola_sensor_msgs/msg/Gyroscope.msg new file mode 100644 index 0000000..397451f --- /dev/null +++ b/nao_lola_sensor_msgs/msg/Gyroscope.msg @@ -0,0 +1,19 @@ +# Holds Gyroscope information for a NAO V6 +# Units are in rad/s +# Follows conventions here: http://doc.aldebaran.com/2-1/family/robots/inertial_robot.html + +# x is rotating right / left vertically (from robots perspective) +# - Falling right (from robot's perspective) is positive +# - Falling left (from robot's perspective) is negative +float32 x + +# y is rotating over toes and heels +# - Rotating over toes is positive +# - Rotating over heels is negative +float32 y + +# NOTE: This is flipped on the V5, and because of that all of gyroscopez may be inverted in a lot of codebases. +# z is rotating left / right horizontally (from robot's perspective) +# - Turning to the left is positive +# - Turning to the right is negative +float32 z \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/JointCurrents.msg b/nao_lola_sensor_msgs/msg/JointCurrents.msg new file mode 100644 index 0000000..b84d415 --- /dev/null +++ b/nao_lola_sensor_msgs/msg/JointCurrents.msg @@ -0,0 +1 @@ +float32[25] currents # Amperes (A), in order of JointIndexes.msg \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/JointIndexes.msg b/nao_lola_sensor_msgs/msg/JointIndexes.msg new file mode 100644 index 0000000..c8340df --- /dev/null +++ b/nao_lola_sensor_msgs/msg/JointIndexes.msg @@ -0,0 +1,26 @@ +uint8 HEADYAW=0 +uint8 HEADPITCH=1 +uint8 LSHOULDERPITCH=2 +uint8 LSHOULDERROLL=3 +uint8 LELBOWYAW=4 +uint8 LELBOWROLL=5 +uint8 LWRISTYAW=6 +uint8 LHIPYAWPITCH=7 +uint8 LHIPROLL=8 +uint8 LHIPPITCH=9 +uint8 LKNEEPITCH=10 +uint8 LANKLEPITCH=11 +uint8 LANKLEROLL=12 +uint8 RHIPROLL=13 +uint8 RHIPPITCH=14 +uint8 RKNEEPITCH=15 +uint8 RANKLEPITCH=16 +uint8 RANKLEROLL=17 +uint8 RSHOULDERPITCH=18 +uint8 RSHOULDERROLL=19 +uint8 RELBOWYAW=20 +uint8 RELBOWROLL=21 +uint8 RWRISTYAW=22 +uint8 LHAND=23 +uint8 RHAND=24 +uint8 NUMJOINTS=25 \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/JointPositions.msg b/nao_lola_sensor_msgs/msg/JointPositions.msg new file mode 100644 index 0000000..c2f187c --- /dev/null +++ b/nao_lola_sensor_msgs/msg/JointPositions.msg @@ -0,0 +1,3 @@ +# An array of joint positions, corresponding to their indexes in the JointIndexes.msg. + +float32[25] positions # radians \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/JointStatuses.msg b/nao_lola_sensor_msgs/msg/JointStatuses.msg new file mode 100644 index 0000000..d2605be --- /dev/null +++ b/nao_lola_sensor_msgs/msg/JointStatuses.msg @@ -0,0 +1,7 @@ +# Temperature status enums, computed accordingly to the temperature limitation to protect the motors. +int32 STATUS_NORMAL=0 # normal +int32 STATUS_HOT=1 # high, start to reduce stiffness +int32 STATUS_VERY_HOT=2 # very hot, stiffness reduced over 30% +int32 STATUS_CRITICALLY_HOT=3 # critically hot, stiffness is set to 0 + +int32[25] statuses # Status codes, in order of JointIndexes.msg \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/JointStiffnesses.msg b/nao_lola_sensor_msgs/msg/JointStiffnesses.msg new file mode 100644 index 0000000..e54e3f4 --- /dev/null +++ b/nao_lola_sensor_msgs/msg/JointStiffnesses.msg @@ -0,0 +1,3 @@ +# An array of joint stiffnesses, corresponding to their indexes in the JointIndexes.msg. + +float32[25] stiffnesses # 0.0 - 1.0 \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/JointTemperatures.msg b/nao_lola_sensor_msgs/msg/JointTemperatures.msg new file mode 100644 index 0000000..de0966f --- /dev/null +++ b/nao_lola_sensor_msgs/msg/JointTemperatures.msg @@ -0,0 +1 @@ +float32[25] temperatures # Celcius (°C), in order of JointIndexes.msg \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/RobotConfig.msg b/nao_lola_sensor_msgs/msg/RobotConfig.msg new file mode 100644 index 0000000..bc06108 --- /dev/null +++ b/nao_lola_sensor_msgs/msg/RobotConfig.msg @@ -0,0 +1,4 @@ +string body_id # eg."P0000073A07S94700012" +string body_version # eg. "6.0.0" +string head_id # eg. "P0000074A05S93M00061" +string head_version # eg. "6.0.0" \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/Sonar.msg b/nao_lola_sensor_msgs/msg/Sonar.msg new file mode 100644 index 0000000..67d78cf --- /dev/null +++ b/nao_lola_sensor_msgs/msg/Sonar.msg @@ -0,0 +1,2 @@ +float32 left +float32 right \ No newline at end of file diff --git a/nao_lola_sensor_msgs/msg/Touch.msg b/nao_lola_sensor_msgs/msg/Touch.msg new file mode 100644 index 0000000..8620d68 --- /dev/null +++ b/nao_lola_sensor_msgs/msg/Touch.msg @@ -0,0 +1,3 @@ +bool head_front +bool head_middle +bool head_rear \ No newline at end of file diff --git a/nao_lola_sensor_msgs/package.xml b/nao_lola_sensor_msgs/package.xml new file mode 100644 index 0000000..d4744da --- /dev/null +++ b/nao_lola_sensor_msgs/package.xml @@ -0,0 +1,22 @@ + + + + nao_lola_sensor_msgs + 0.0.4 + Package defining sensor msgs to be received from NAO robot. + ijnek + Apache License 2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +