ros-sports · ijnek · Jul 27, 2023 · Jul 27, 2023 · Jul 27, 2023 · Jul 27, 2023
diff --git 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
diff --git a/CHANGELOG.rst → nao_lola/CHANGELOG.rst b/CHANGELOG.rst → nao_lola/CHANGELOG.rst
diff --git a/CMakeLists.txt → nao_lola/CMakeLists.txt b/CMakeLists.txt → nao_lola/CMakeLists.txt
diff --git a/external/AMENT_IGNORE → nao_lola/external/AMENT_IGNORE b/external/AMENT_IGNORE → nao_lola/external/AMENT_IGNORE
diff --git a/nao_lola/external/msgpack-c b/nao_lola/external/msgpack-c
diff --git a/...ude/nao_lola/command_index_conversion.hpp → ...ude/nao_lola/command_index_conversion.hpp b/...ude/nao_lola/command_index_conversion.hpp → ...ude/nao_lola/command_index_conversion.hpp
diff --git a/include/nao_lola/connection.hpp → nao_lola/include/nao_lola/connection.hpp b/include/nao_lola/connection.hpp → nao_lola/include/nao_lola/connection.hpp
diff --git a/include/nao_lola/index_conversion.hpp → ...ola/include/nao_lola/index_conversion.hpp b/include/nao_lola/index_conversion.hpp → ...ola/include/nao_lola/index_conversion.hpp
diff --git a/include/nao_lola/lola_enums.hpp → nao_lola/include/nao_lola/lola_enums.hpp b/include/nao_lola/lola_enums.hpp → nao_lola/include/nao_lola/lola_enums.hpp
diff --git a/include/nao_lola/msgpack_packer.hpp → nao_lola/include/nao_lola/msgpack_packer.hpp b/include/nao_lola/msgpack_packer.hpp → nao_lola/include/nao_lola/msgpack_packer.hpp
diff --git a/include/nao_lola/msgpack_parser.hpp → nao_lola/include/nao_lola/msgpack_parser.hpp b/include/nao_lola/msgpack_parser.hpp → nao_lola/include/nao_lola/msgpack_parser.hpp
diff --git a/include/nao_lola/nao_lola.hpp → nao_lola/include/nao_lola/nao_lola.hpp b/include/nao_lola/nao_lola.hpp → nao_lola/include/nao_lola/nao_lola.hpp
diff --git a/include/nao_lola/sensor_index_conversion.hpp → ...lude/nao_lola/sensor_index_conversion.hpp b/include/nao_lola/sensor_index_conversion.hpp → ...lude/nao_lola/sensor_index_conversion.hpp
diff --git a/package.xml → nao_lola/package.xml b/package.xml → nao_lola/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
  <name>nao_lola</name>
  <version>0.3.1</version>
- <description>Packages that allow communicating with the NAO’s Lola middle-ware.</description>
+ <description>Packages that allow communicating with the NAO's Lola middle-ware.</description>
  <maintainer email="kenjibrameld@gmail.com">ijnek</maintainer>
  <license>Apache License 2.0</license>
 
@@ -20,5 +20,8 @@
 
  <export>
  <build_type>ament_cmake</build_type>
+ <deprecated>
+ Please use nao_lola_client. nao_lola will be removed in K-turtle.
+ </deprecated>
  </export>
 </package>
diff --git a/src/connection.cpp → nao_lola/src/connection.cpp b/src/connection.cpp → nao_lola/src/connection.cpp
diff --git a/src/msgpack_packer.cpp → nao_lola/src/msgpack_packer.cpp b/src/msgpack_packer.cpp → nao_lola/src/msgpack_packer.cpp
diff --git a/src/msgpack_parser.cpp → nao_lola/src/msgpack_parser.cpp b/src/msgpack_parser.cpp → nao_lola/src/msgpack_parser.cpp
diff --git a/src/nao_lola.cpp → nao_lola/src/nao_lola.cpp b/src/nao_lola.cpp → nao_lola/src/nao_lola.cpp
diff --git a/src/nao_lola_node.cpp → nao_lola/src/nao_lola_node.cpp b/src/nao_lola_node.cpp → nao_lola/src/nao_lola_node.cpp
diff --git a/test/CMakeLists.txt → nao_lola/test/CMakeLists.txt b/test/CMakeLists.txt → nao_lola/test/CMakeLists.txt
diff --git a/test/test_msgpack_packer.cpp → nao_lola/test/test_msgpack_packer.cpp b/test/test_msgpack_packer.cpp → nao_lola/test/test_msgpack_packer.cpp
diff --git a/test/test_msgpack_parser.cpp → nao_lola/test/test_msgpack_parser.cpp b/test/test_msgpack_parser.cpp → nao_lola/test/test_msgpack_parser.cpp
diff --git a/nao_lola_client/CMakeLists.txt 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
diff --git a/nao_lola_client/external/msgpack-c b/nao_lola_client/external/msgpack-c
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
@@ -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 <map>
+#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<int, LolaEnums::Joint> flip(std::map<LolaEnums::Joint, int> in);
+
+static const std::map<LolaEnums::Joint, int> 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<int, LolaEnums::Joint> joint_msg_to_lola = flip(joint_lola_to_msg);
+
+std::map<int, LolaEnums::Joint> flip(std::map<LolaEnums::Joint, int> in)
+{
+ std::map<int, LolaEnums::Joint> flipped;
+ for (std::map<LolaEnums::Joint, int>::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<int, LolaEnums::LeftEarLeds> 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<int, LolaEnums::RightEarLeds> 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<int, LolaEnums::LeftEyeLeds> 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<int, LolaEnums::RightEyeLeds> 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<int, LolaEnums::SkullLeds> 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
@@ -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 <string>
+#include "boost/asio.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#define MSGPACK_READ_LENGTH 896
+
+class Connection
+{
+public:
+ Connection();
+ std::array<char, MSGPACK_READ_LENGTH> 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_