[Sprint 22/23 / PI-412] - [Feature] Implement TF2 Tree

CMakeLists.txt

@@ -55,6 +55,9 @@ 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/tf2/frame_id.cpp"
+  "src/${PROJECT_NAME}/joint/tf2/frame.cpp"
+  "src/${PROJECT_NAME}/joint/tf2/tf2_manager.cpp"
   "src/${PROJECT_NAME}/node/tachimawari_node.cpp"
   # TODO: Enable me later.
   # "src/${PROJECT_NAME}/node/rviz_server_node.cpp"
@@ -170,6 +173,7 @@ ament_export_dependencies(
   kansei_interfaces
   keisan
   rclcpp
+  tf2_ros
   tachimawari_interfaces)
 ament_export_include_directories("include")
 ament_export_libraries(${PROJECT_NAME})

include/tachimawari/joint/node/joint_node.hpp

@@ -21,16 +21,23 @@
 #ifndef TACHIMAWARI__JOINT__NODE__JOINT_NODE_HPP_
 #define TACHIMAWARI__JOINT__NODE__JOINT_NODE_HPP_
 
+#include <tf2_ros/buffer.h>
+
 #include <memory>
 #include <string>
 
+#include "kansei_interfaces/msg/status.hpp"
+#include "keisan/angle/angle.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
 #include "tachimawari/joint/node/joint_manager.hpp"
+#include "tachimawari/joint/tf2/tf2_manager.hpp"
 #include "tachimawari/joint/utils/middleware.hpp"
 #include "tachimawari_interfaces/msg/control_joints.hpp"
 #include "tachimawari_interfaces/msg/current_joints.hpp"
 #include "tachimawari_interfaces/msg/set_joints.hpp"
 #include "tachimawari_interfaces/msg/set_torques.hpp"
+#include "tf2_ros/transform_broadcaster.h"
 
 namespace tachimawari::joint
 {
@@ -42,16 +49,21 @@ class JointNode
   using CurrentJoints = tachimawari_interfaces::msg::CurrentJoints;
   using SetJoints = tachimawari_interfaces::msg::SetJoints;
   using SetTorques = tachimawari_interfaces::msg::SetTorques;
+  using Status = kansei_interfaces::msg::Status;
 
   static std::string get_node_prefix();
   static std::string control_joints_topic();
   static std::string set_joints_topic();
   static std::string set_torques_topic();
   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();
+  void publish_frame_tree();
   void update();
 
 private:
@@ -63,8 +75,13 @@ class JointNode
 
   rclcpp::Subscription<SetTorques>::SharedPtr set_torques_subscriber;
 
+  rclcpp::Subscription<Status>::SharedPtr status_subscriber;
+
   Middleware middleware;
   rclcpp::Subscription<ControlJoints>::SharedPtr control_joints_subscriber;
+
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf2_broadcaster;
+  std::shared_ptr<Tf2Manager> tf2_manager;
 };
 
 }  // namespace tachimawari::joint

include/tachimawari/joint/tf2/frame.hpp

@@ -0,0 +1,77 @@
+// Copyright (c) 2021-2023 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.
+
+#ifndef TACHIMAWARI__JOINT__TF2__FRAME_HPP_
+#define TACHIMAWARI__JOINT__TF2__FRAME_HPP_
+
+#include <memory>
+#include <nlohmann/json.hpp>
+#include <string>
+#include <vector>
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "keisan/angle/angle.hpp"
+#include "rclcpp/time.hpp"
+#include "tachimawari/joint/node/joint_manager.hpp"
+#include "tachimawari/joint/tf2/frame_id.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+
+namespace tachimawari::joint
+{
+
+class Frame
+{
+public:
+  Frame(
+    const uint8_t id, const double translation_x, const double translation_y,
+    const double translation_z, const double const_roll, const double const_pitch,
+    const double const_yaw);
+
+  void update_quaternion(std::vector<Joint> current_joints);
+  void update_quaternion(keisan::Angle<double> imu_yaw);
+  geometry_msgs::msg::TransformStamped get_transform_stamped(rclcpp::Time time_stamp);
+
+  uint8_t id;
+
+  double translation_x;
+  double translation_y;
+  double translation_z;
+
+  double quaternion_x;
+  double quaternion_y;
+  double quaternion_z;
+  double quaternion_w;
+
+  double const_roll;
+  double const_pitch;
+  double const_yaw;
+
+private:
+  enum : uint8_t {
+    ROLL = 0,
+    PITCH = 1,
+    YAW = 2,
+  };
+
+  double get_joint_angle(uint8_t quaternion_axis, std::vector<Joint> current_joints);
+};
+}  // namespace tachimawari::joint
+
+#endif  // TACHIMAWARI__JOINT__TF2__FRAME_HPP_

include/tachimawari/joint/tf2/frame_id.hpp

@@ -0,0 +1,58 @@
+// Copyright (c) 2021-2023 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.
+
+#ifndef TACHIMAWARI__JOINT_TF2__FRAME_ID_HPP_
+#define TACHIMAWARI__JOINT_TF2__FRAME_ID_HPP_
+
+#include <array>
+#include <map>
+#include <string>
+#include <vector>
+
+namespace tachimawari::joint
+{
+
+class FrameId
+{
+public:
+  enum : uint8_t {
+    ODOM = 0,
+    BASE_LINK = 1,
+    TORSO = 2,
+    HEAD = 3,
+    GAZE = 4,
+    RIGHT_THIGH = 5,
+    RIGHT_CALF = 6,
+    RIGHT_FOOT = 7,
+    LEFT_THIGH = 8,
+    LEFT_CALF = 9,
+    LEFT_FOOT = 10
+  };
+
+  static const std::array<uint8_t, 10> frame_ids;
+  static const std::map<uint8_t, std::string> frame_id_string;
+  static const std::map<std::string, uint8_t> frame_string_id;
+  static const std::map<uint8_t, std::vector<uint8_t>> frame_joint_map;
+  static const std::map<uint8_t, std::string> parent_frame;
+};
+
+}  // namespace tachimawari::joint
+
+#endif  // TACHIMAWARI__JOINT_TF2__FRAME_ID_HPP_

include/tachimawari/joint/tf2/tf2_manager.hpp

@@ -0,0 +1,56 @@
+// Copyright (c) 2021-2023 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.
+
+#ifndef TACHIMAWARI__JOINT__TF2__TF2_MANAGER_HPP_
+#define TACHIMAWARI__JOINT__TF2__TF2_MANAGER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "keisan/angle/angle.hpp"
+#include "rclcpp/time.hpp"
+#include "tachimawari/joint/model/joint.hpp"
+#include "tachimawari/joint/model/joint_id.hpp"
+#include "tachimawari/joint/node/joint_manager.hpp"
+#include "tachimawari/joint/tf2/frame.hpp"
+#include "tachimawari/joint/tf2/frame_id.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+
+namespace tachimawari::joint
+{
+
+class Tf2Manager
+{
+public:
+  explicit Tf2Manager();
+
+  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; }
+
+private:
+  static const std::vector<std::pair<int, int>> tf2_joint_pairs;
+  std::string config_path;
+  std::vector<Frame> frames;
+};
+}  // namespace tachimawari::joint
+
+#endif  // TACHIMAWARI__JOINT__TF2__TF2_MANAGER_HPP_

include/tachimawari/node/tachimawari_node.hpp

@@ -39,13 +39,14 @@ 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();
 
 private:
   rclcpp::Node::SharedPtr node;
   rclcpp::TimerBase::SharedPtr node_timer;
+  rclcpp::TimerBase::SharedPtr node_timer_tf;
 
   std::shared_ptr<control::ControlManager> control_manager;
 

src/tachimawari/joint/node/joint_node.cpp

@@ -42,11 +42,20 @@ std::string JointNode::set_joints_topic() {return get_node_prefix() + "/set_join
 
 std::string JointNode::set_torques_topic() {return get_node_prefix() + "/set_torques";}
 
+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)
-: joint_manager(joint_manager), middleware()
+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(path);
+
   control_joints_subscriber = node->create_subscription<ControlJoints>(
     control_joints_topic(), 10, [this](const ControlJoints::SharedPtr message) {
       this->middleware.set_rules(message->control_type, message->ids);
@@ -70,6 +79,11 @@ JointNode::JointNode(rclcpp::Node::SharedPtr node, std::shared_ptr<JointManager>
       this->joint_manager->torque_enable(joints, message->torque_enable);
     });
 
+  status_subscriber =
+    node->create_subscription<Status>(status_topic(), 10, [this](const Status::SharedPtr message) {
+      imu_yaw = keisan::make_degree(message->orientation.yaw);
+    });
+
   current_joints_publisher = node->create_publisher<CurrentJoints>(current_joints_topic(), 10);
 }
 
@@ -90,4 +104,15 @@ void JointNode::publish_current_joints()
   current_joints_publisher->publish(msg_joints);
 }
 
+void JointNode::publish_frame_tree()
+{
+  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));
+  }
+}
+
 }  // namespace tachimawari::joint