Merge branch 'master' into enhancement/try-new-cm

Author: mbsaloka

CMakeLists.txt

@@ -56,9 +56,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/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"

include/tachimawari/joint/node/joint_node.hpp

@@ -29,14 +29,11 @@
 #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/buffer.h"
-#include "tf2_ros/transform_broadcaster.h"
 
 namespace tachimawari::joint
 {
@@ -57,12 +54,10 @@ class JointNode
   static std::string current_joints_topic();
   static std::string status_topic();
 
-  JointNode(
-    rclcpp::Node::SharedPtr node, std::shared_ptr<JointManager> joint_manager, const std::string & path);
+  JointNode(rclcpp::Node::SharedPtr node, std::shared_ptr<JointManager> joint_manager);
   keisan::Angle<double> imu_yaw;
 
   void publish_current_joints();
-  void publish_frame_tree();
   void update();
 
 private:
@@ -78,9 +73,6 @@ class JointNode
 
   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

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

include/tachimawari/joint/tf2/frame_id.hpp

@@ -34,28 +34,21 @@
 namespace tachimawari::joint
 {
 
-std::string JointNode::get_node_prefix() {return "joint";}
+std::string JointNode::get_node_prefix() { return "joint"; }
 
-std::string JointNode::control_joints_topic() {return get_node_prefix() + "/control_joints";}
+std::string JointNode::control_joints_topic() { return get_node_prefix() + "/control_joints"; }
 
-std::string JointNode::set_joints_topic() {return get_node_prefix() + "/set_joints";}
+std::string JointNode::set_joints_topic() { return get_node_prefix() + "/set_joints"; }
 
-std::string JointNode::set_torques_topic() {return get_node_prefix() + "/set_torques";}
+std::string JointNode::set_torques_topic() { return get_node_prefix() + "/set_torques"; }
 
-std::string JointNode::status_topic() {return "measurement/status";}
+std::string JointNode::status_topic() { return "measurement/status"; }
 
-std::string JointNode::current_joints_topic() {return get_node_prefix() + "/current_joints";}
+std::string JointNode::current_joints_topic() { return get_node_prefix() + "/current_joints"; }
 
-JointNode::JointNode(
-  rclcpp::Node::SharedPtr node, std::shared_ptr<JointManager> joint_manager, const 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))
+JointNode::JointNode(rclcpp::Node::SharedPtr node, std::shared_ptr<JointManager> joint_manager)
+: joint_manager(joint_manager), middleware(), 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);
@@ -74,7 +67,7 @@ JointNode::JointNode(
       std::vector<Joint> joints;
       std::transform(
         message->ids.begin(), message->ids.end(), std::back_inserter(joints),
-        [](uint8_t id) -> Joint {return Joint(id, 0);});
+        [](uint8_t id) -> Joint { return Joint(id, 0); });
 
       this->joint_manager->torque_enable(joints, message->torque_enable);
     });
@@ -87,7 +80,7 @@ JointNode::JointNode(
   current_joints_publisher = node->create_publisher<CurrentJoints>(current_joints_topic(), 10);
 }
 
-void JointNode::update() {middleware.update();}
+void JointNode::update() { middleware.update(); }
 
 void JointNode::publish_current_joints()
 {
@@ -104,15 +97,4 @@ 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