Merge remote-tracking branch 'origin/build/update-to-jazzy' into enhancement/try-new-cm

Author: mbsaloka

CMakeLists.txt

@@ -15,6 +15,7 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(dynamixel_sdk REQUIRED)
+find_package(jitsuyo REQUIRED)
 find_package(kansei_interfaces REQUIRED)
 find_package(keisan REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -70,6 +71,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 
 ament_target_dependencies(${PROJECT_NAME}
   dynamixel_sdk
+  jitsuyo
   kansei_interfaces
   keisan
   rclcpp
@@ -170,6 +172,7 @@ endif()
 
 ament_export_dependencies(
   dynamixel_sdk
+  jitsuyo
   kansei_interfaces
   keisan
   rclcpp

include/tachimawari/control/controller/packet/protocol_1/model/packet.hpp

@@ -21,6 +21,7 @@
 #ifndef TACHIMAWARI__CONTROL__CONTROLLER__PACKET__PROTOCOL_1__MODEL__PACKET_HPP_
 #define TACHIMAWARI__CONTROL__CONTROLLER__PACKET__PROTOCOL_1__MODEL__PACKET_HPP_
 
+#include <cstdint>
 #include <string>
 #include <vector>
 

include/tachimawari/control/controller/packet/protocol_1/utils/word.hpp

@@ -21,6 +21,7 @@
 #ifndef TACHIMAWARI__CONTROL__CONTROLLER__PACKET__PROTOCOL_1__UTILS__WORD_HPP_
 #define TACHIMAWARI__CONTROL__CONTROLLER__PACKET__PROTOCOL_1__UTILS__WORD_HPP_
 
+#include <cstdint>
 #include <string>
 
 namespace tachimawari::control::protocol_1

include/tachimawari/control/sdk/module/dynamixel_sdk.hpp

@@ -25,6 +25,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <unordered_map>
 
 #include "dynamixel_sdk/dynamixel_sdk.h"
 #include "tachimawari/control/manager/control_manager.hpp"

include/tachimawari/control/sdk/packet/model/group_bulk_read.hpp

@@ -25,6 +25,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <unordered_map>
 
 #include "dynamixel_sdk/dynamixel_sdk.h"
 #include "tachimawari/joint/model/joint.hpp"

include/tachimawari/joint/model/joint_id.hpp

@@ -22,6 +22,7 @@
 #define TACHIMAWARI__JOINT__MODEL__JOINT_ID_HPP_
 
 #include <array>
+#include <cstdint>
 #include <map>
 #include <string>
 

launch/tachimawari_launch.py

@@ -0,0 +1,40 @@
+# Copyright (c) 2024 ICHIRO ITS
+# 
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+# 
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+# 
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import os
+import socket
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+  hostname = socket.gethostname()
+  config_path = os.path.expanduser(f'~/ros2-ws/configuration/{hostname}/ipm/')
+
+  return LaunchDescription([
+    Node(
+      package='tachimawari',
+      executable='main',
+      name='main',
+      output='screen',
+      arguments=['cm740', config_path],
+      respawn=True,
+      respawn_delay=1
+    )
+  ])

package.xml

@@ -8,6 +8,7 @@
   <license>MIT License</license>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>dynamixel_sdk</depend>
+  <depend>jitsuyo</depend>
   <depend>kansei_interfaces</depend>
   <depend>keisan</depend>
   <depend>rclcpp</depend>

src/tachimawari/joint/tf2/tf2_manager.cpp

@@ -21,47 +21,88 @@
 #include <algorithm>
 #include <fstream>
 #include <memory>
-#include <nlohmann/json.hpp>
 #include <string>
 #include <vector>
 
 #include "tachimawari/joint/tf2/tf2_manager.hpp"
 
+#include "jitsuyo/config.hpp"
+#include "nlohmann/json.hpp"
+
 namespace tachimawari::joint
 {
 
 Tf2Manager::Tf2Manager() {}
 
 bool Tf2Manager::load_configuration(const std::string & path)
 {
-  std::string ss = path + "frame_measurements.json";
-
-  std::ifstream input(ss, std::ifstream::in);
-  if (!input.is_open()) {
-    throw std::runtime_error("Unable to open `" + ss + "`!");
+  nlohmann::json config;
+  if (!jitsuyo::load_config(path, "frame_measurements.json", config)) {
+    return false;
   }
 
-  nlohmann::json config = nlohmann::json::parse(input);
+  bool valid_config = true;
 
   for (auto & item : config.items()) {
     // Get all config
-    try {
-      std::string name = item.key();
-      double translation_x = item.value().at("translation").at("x");
-      double translation_y = item.value().at("translation").at("y");
-      double translation_z = item.value().at("translation").at("z");
-      double const_roll = item.value().at("const_rpy").at("roll");
-      double const_pitch = item.value().at("const_rpy").at("pitch");
-      double const_yaw = item.value().at("const_rpy").at("yaw");
-      auto map_string_id = FrameId::frame_string_id.find(name);
-
-      uint8_t id = map_string_id->second;
-      frames.push_back(
-        Frame(id, translation_x, translation_y, translation_z, const_roll, const_pitch, const_yaw));
-    } catch (nlohmann::json::parse_error & ex) {
-      std::cerr << "parse error at byte " << ex.byte << std::endl;
-      throw ex;
+    double translation_x;
+    double translation_y;
+    double translation_z;
+    double const_roll;
+    double const_pitch;
+    double const_yaw;
+
+    std::string name = item.key();
+    
+    bool valid_section = true;
+    nlohmann::json section;
+    if (jitsuyo::assign_val(config, name, section)) {
+      nlohmann::json translation;
+      nlohmann::json const_rpy;
+
+      if (jitsuyo::assign_val(section, "translation", translation)) {
+        bool valid_translation = true;
+        valid_translation &= jitsuyo::assign_val(translation, "x", translation_x);
+        valid_translation &= jitsuyo::assign_val(translation, "y", translation_y);
+        valid_translation &= jitsuyo::assign_val(translation, "z", translation_z);
+        if (!valid_translation) {
+          std::cout << "Error found at section `translation`" << std::endl;
+          valid_section = false;
+        }
+      } else {
+        valid_section = false;
+      }
+
+      if (jitsuyo::assign_val(section, "const_rpy", const_rpy)) {
+        bool valid_const_rpy = true;
+        valid_const_rpy &= jitsuyo::assign_val(const_rpy, "roll", const_roll);
+        valid_const_rpy &= jitsuyo::assign_val(const_rpy, "pitch", const_pitch);
+        valid_const_rpy &= jitsuyo::assign_val(const_rpy, "yaw", const_yaw);
+        if (!valid_const_rpy) {
+          std::cout << "Error found at section `const_rpy`" << std::endl;
+          valid_section = false;
+        }
+      } else {
+        valid_section = false;
+      }
+    } else {
+      valid_section = false;
     }
+
+    if (!valid_section) {
+      valid_config = false;
+      continue;
+    }
+
+    auto map_string_id = FrameId::frame_string_id.find(name);
+
+    uint8_t id = map_string_id->second;
+    frames.push_back(
+      Frame(id, translation_x, translation_y, translation_z, const_roll, const_pitch, const_yaw));
+  }
+
+  if (!valid_config) {
+    throw std::runtime_error("Failed to load config file `" + path + "frame_measurements.json`");
   }
 
   return true;

src/tachimawari_main.cpp

@@ -27,17 +27,17 @@
 
 int main(int argc, char * argv[])
 {
-  rclcpp::init(argc, argv);
+  auto args = rclcpp::init_and_remove_ros_arguments(argc, argv);
 
-  if (argc < 2) {
+  if (args.size() < 2) {
     std::cerr << "Please specify the mode! [sdk / cm740]" << std::endl;
     return 0;
-  } else if (argc < 3) {
+  } else if (args.size() < 3) {
     std::cerr << "Please specify the tf configuration path" << std::endl;
   }
 
-  std::string mode = argv[1];
-  std::string path = argv[2];
+  std::string mode = args[1];
+  std::string path = args[2];
   std::shared_ptr<tachimawari::control::ControlManager> controller;
 
   if (mode == "sdk") {