feat: change load configuration path, remove unused lines, tidy some …

…lines

CMakeLists.txt

@@ -55,7 +55,6 @@ add_library(${PROJECT_NAME} SHARED
   "src/${PROJECT_NAME}/joint/node/joint_node.cpp"
   "src/${PROJECT_NAME}/joint/utils/middleware.cpp"
   "src/${PROJECT_NAME}/joint/utils/node_control.cpp"
-  "src/${PROJECT_NAME}/joint/utils/utils.cpp"
   "src/${PROJECT_NAME}/joint/tf2/frame_id.cpp"
   "src/${PROJECT_NAME}/joint/tf2/frame.cpp"
   "src/${PROJECT_NAME}/joint/tf2/tf2_manager.cpp"

include/tachimawari/joint/node/joint_node.hpp

@@ -58,7 +58,8 @@ class JointNode
   static std::string current_joints_topic();
   static std::string status_topic();
 
-  JointNode(rclcpp::Node::SharedPtr node, std::shared_ptr<JointManager> joint_manager);
+  JointNode(
+    rclcpp::Node::SharedPtr node, std::shared_ptr<JointManager> joint_manager, std::string path);
   keisan::Angle<double> imu_yaw;
 
   void publish_current_joints();

include/tachimawari/joint/tf2/tf2_manager.hpp

@@ -32,7 +32,6 @@
 #include "tachimawari/joint/node/joint_manager.hpp"
 #include "tachimawari/joint/tf2/frame.hpp"
 #include "tachimawari/joint/tf2/frame_id.hpp"
-#include "tachimawari/joint/utils/utils.hpp"
 #include "tf2_ros/transform_broadcaster.h"
 
 namespace tachimawari::joint
@@ -43,9 +42,7 @@ class Tf2Manager
 public:
   explicit Tf2Manager();
 
-  bool load_configuration();
-  bool save_configuration();
-  bool sync_configuration();
+  bool load_configuration(std::string path);
   void update(std::vector<Joint> current_joints, keisan::Angle<double> imu_yaw);
   std::vector<Frame> get_frames() { return frames; }
 

include/tachimawari/node/tachimawari_node.hpp

@@ -39,7 +39,7 @@ class TachimawariNode
   explicit TachimawariNode(
     rclcpp::Node::SharedPtr node, std::shared_ptr<control::ControlManager> control_manager);
 
-  void run_joint_manager();
+  void run_joint_manager(std::string path);
 
   void run_imu_provider();
 

src/tachimawari/joint/node/joint_node.cpp

@@ -46,14 +46,15 @@ std::string JointNode::status_topic() { return "measurement/status"; }
 
 std::string JointNode::current_joints_topic() { return get_node_prefix() + "/current_joints"; }
 
-JointNode::JointNode(rclcpp::Node::SharedPtr node, std::shared_ptr<JointManager> joint_manager)
+JointNode::JointNode(
+  rclcpp::Node::SharedPtr node, std::shared_ptr<JointManager> joint_manager, std::string path)
 : joint_manager(joint_manager),
   middleware(),
   tf2_broadcaster(std::make_shared<tf2_ros::TransformBroadcaster>(node)),
   tf2_manager(std::make_shared<Tf2Manager>()),
   imu_yaw(keisan::make_degree(0))
 {
-  tf2_manager->load_configuration();
+  tf2_manager->load_configuration(path);
 
   control_joints_subscriber = node->create_subscription<ControlJoints>(
     control_joints_topic(), 10, [this](const ControlJoints::SharedPtr message) {
@@ -100,15 +101,15 @@ void JointNode::publish_current_joints()
     joints[i].position = current_joints[i].get_position();
   }
 
-  tf2_manager->update(current_joints, imu_yaw);
   current_joints_publisher->publish(msg_joints);
 }
 
 void JointNode::publish_frame_tree()
 {
-  // get time
-  rclcpp::Time now = rclcpp::Clock().now();
+  const auto & current_joints = this->joint_manager->get_current_joints();
+  tf2_manager->update(current_joints, imu_yaw);
 
+  rclcpp::Time now = rclcpp::Clock().now();
   for (auto & frame : tf2_manager->get_frames()) {
     tf2_broadcaster->sendTransform(frame.get_transform_stamped(now));
   }

src/tachimawari/joint/tf2/tf2_manager.cpp

@@ -32,20 +32,13 @@ namespace tachimawari::joint
 
 Tf2Manager::Tf2Manager() {}
 
-bool Tf2Manager::load_configuration()
+bool Tf2Manager::load_configuration(std::string path)
 {
-  config_path = "/home/ichiro/ros2-ws-cp/src/tachimawari/data/";
-  std::string ss = config_path + utils::get_host_name() + ".json";
-
-  if (utils::is_file_exist(ss) == false) {
-    if (save_configuration() == false) {
-      return false;
-    }
-  }
+  std::string ss = path + "frame_measurements.json";
 
   std::ifstream input(ss, std::ifstream::in);
-  if (input.is_open() == false) {
-    return false;
+  if (!input.is_open()) {
+    throw std::runtime_error("Unable to open `" + ss + "`!");
   }
 
   nlohmann::json config = nlohmann::json::parse(input);
@@ -73,63 +66,10 @@ bool Tf2Manager::load_configuration()
   return true;
 }
 
-bool Tf2Manager::save_configuration()
-{
-  config_path = "/home/ichiro/ros2-ws-cp/src/tachimawari/data/";
-  std::string ss = config_path + utils::get_host_name() + ".json";
-
-  if (utils::is_file_exist(ss) == false) {
-    if (utils::create_file(ss) == false) {
-      return false;
-    }
-  }
-
-  nlohmann::json config = nlohmann::json::array();
-
-  for (auto & item : frames) {
-    auto iterator = FrameId::frame_id_string.find(item.id);
-    std::string name = iterator->second;
-
-    nlohmann::json frame;
-    frame["name"] = name;
-    frame["translation"]["x"] = item.translation_x;
-    frame["translation"]["y"] = item.translation_y;
-    frame["translation"]["z"] = item.translation_z;
-    frame["const_rpy"]["roll"] = item.const_roll;
-    frame["const_rpy"]["pitch"] = item.const_pitch;
-    frame["const_rpy"]["yaw"] = item.const_yaw;
-
-    config.push_back(frame);
-  }
-
-  std::ofstream output(ss, std::ofstream::out);
-  if (output.is_open() == false) {
-    return false;
-  }
-
-  output << config.dump(2);
-  output.close();
-
-  return true;
-}
-
-bool Tf2Manager::sync_configuration()
-{
-  if (!load_configuration()) {
-    return false;
-  }
-
-  if (!save_configuration()) {
-    return false;
-  }
-
-  return true;
-}
-
 void Tf2Manager::update(std::vector<Joint> current_joints, keisan::Angle<double> imu_yaw)
 {
   for (auto & item : frames) {
-    if (item.id == 0) {
+    if (item.id == FrameId::ODOM) {
       item.update_quaternion(imu_yaw);
     } else {
       item.update_quaternion(current_joints);