diff --git a/CMakeLists.txt b/CMakeLists.txt index 1ed3148..babce85 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/include/tachimawari/joint/node/joint_node.hpp b/include/tachimawari/joint/node/joint_node.hpp index e3fb094..c883e03 100644 --- a/include/tachimawari/joint/node/joint_node.hpp +++ b/include/tachimawari/joint/node/joint_node.hpp @@ -24,13 +24,19 @@ #include #include +#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/buffer.h" +#include "tf2_ros/transform_broadcaster.h" namespace tachimawari::joint { @@ -42,16 +48,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 joint_manager); + JointNode( + rclcpp::Node::SharedPtr node, std::shared_ptr joint_manager, const std::string & path); + keisan::Angle imu_yaw; void publish_current_joints(); + void publish_frame_tree(); void update(); private: @@ -63,8 +74,13 @@ class JointNode rclcpp::Subscription::SharedPtr set_torques_subscriber; + rclcpp::Subscription::SharedPtr status_subscriber; + Middleware middleware; rclcpp::Subscription::SharedPtr control_joints_subscriber; + + std::shared_ptr tf2_broadcaster; + std::shared_ptr tf2_manager; }; } // namespace tachimawari::joint diff --git a/include/tachimawari/joint/tf2/frame.hpp b/include/tachimawari/joint/tf2/frame.hpp new file mode 100644 index 0000000..36ab993 --- /dev/null +++ b/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 +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "keisan/angle/angle.hpp" +#include "nlohmann/json.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(const std::vector & current_joints); + void update_quaternion(const keisan::Angle & imu_yaw); + geometry_msgs::msg::TransformStamped get_transform_stamped(const 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, + PITCH, + YAW, + }; + + double get_joint_angle(const uint8_t & quaternion_axis, const std::vector & current_joints); +}; +} // namespace tachimawari::joint + +#endif // TACHIMAWARI__JOINT__TF2__FRAME_HPP_ diff --git a/include/tachimawari/joint/tf2/frame_id.hpp b/include/tachimawari/joint/tf2/frame_id.hpp new file mode 100644 index 0000000..67254e8 --- /dev/null +++ b/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 +#include +#include +#include + +namespace tachimawari::joint +{ + +class FrameId +{ +public: + enum : uint8_t { + ODOM, + BASE_LINK, + TORSO, + HEAD, + GAZE, + RIGHT_THIGH, + RIGHT_CALF, + RIGHT_FOOT, + LEFT_THIGH, + LEFT_CALF, + LEFT_FOOT + }; + + static const std::array frame_ids; + static const std::map frame_id_string; + static const std::map frame_string_id; + static const std::map> frame_joint_map; + static const std::map parent_frame; +}; + +} // namespace tachimawari::joint + +#endif // TACHIMAWARI__JOINT_TF2__FRAME_ID_HPP_ diff --git a/include/tachimawari/joint/tf2/tf2_manager.hpp b/include/tachimawari/joint/tf2/tf2_manager.hpp new file mode 100644 index 0000000..61e512b --- /dev/null +++ b/include/tachimawari/joint/tf2/tf2_manager.hpp @@ -0,0 +1,54 @@ +// 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 +#include +#include + +#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(const std::string & path); + void update(const std::vector & current_joints, const keisan::Angle & imu_yaw); + std::vector get_frames() { return frames; } + +private: + std::vector frames; +}; +} // namespace tachimawari::joint + +#endif // TACHIMAWARI__JOINT__TF2__TF2_MANAGER_HPP_ diff --git a/include/tachimawari/node/tachimawari_node.hpp b/include/tachimawari/node/tachimawari_node.hpp index 7d33ad2..e14af96 100644 --- a/include/tachimawari/node/tachimawari_node.hpp +++ b/include/tachimawari/node/tachimawari_node.hpp @@ -39,7 +39,7 @@ class TachimawariNode explicit TachimawariNode( rclcpp::Node::SharedPtr node, std::shared_ptr control_manager); - void run_joint_manager(); + void run_joint_manager(const std::string & path); void run_imu_provider(); diff --git a/src/tachimawari/joint/node/joint_node.cpp b/src/tachimawari/joint/node/joint_node.cpp index 6cf45ed..b14f4cb 100644 --- a/src/tachimawari/joint/node/joint_node.cpp +++ b/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 joint_manager) -: joint_manager(joint_manager), middleware() +JointNode::JointNode( + rclcpp::Node::SharedPtr node, std::shared_ptr joint_manager, const std::string & path) +: joint_manager(joint_manager), + middleware(), + tf2_broadcaster(std::make_shared(node)), + tf2_manager(std::make_shared()), + imu_yaw(keisan::make_degree(0)) { + tf2_manager->load_configuration(path); + control_joints_subscriber = node->create_subscription( 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 this->joint_manager->torque_enable(joints, message->torque_enable); }); + status_subscriber = + node->create_subscription(status_topic(), 10, [this](const Status::SharedPtr message) { + imu_yaw = keisan::make_degree(message->orientation.yaw); + }); + current_joints_publisher = node->create_publisher(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 diff --git a/src/tachimawari/joint/tf2/frame.cpp b/src/tachimawari/joint/tf2/frame.cpp new file mode 100644 index 0000000..1111331 --- /dev/null +++ b/src/tachimawari/joint/tf2/frame.cpp @@ -0,0 +1,129 @@ +// 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. + +#include +#include +#include +#include +#include +#include + +#include "tachimawari/joint/tf2/frame.hpp" + +namespace tachimawari::joint +{ + +Frame::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) +: id(id), + translation_x(translation_x), + translation_y(translation_y), + translation_z(translation_z), + quaternion_x(0), + quaternion_y(0), + quaternion_z(0), + quaternion_w(0), + const_roll(const_roll), + const_pitch(const_pitch), + const_yaw(const_yaw) +{ +} + +geometry_msgs::msg::TransformStamped Frame::get_transform_stamped(const rclcpp::Time & time_stamp) +{ + auto map_string_name = FrameId::frame_id_string.find(id); + std::string frame_name = map_string_name->second; + + auto map_parent_name = FrameId::parent_frame.find(id); + std::string parent_frame_name = map_parent_name->second; + + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = parent_frame_name; + t.header.stamp = time_stamp; + t.child_frame_id = frame_name; + + t.transform.translation.x = translation_x; + t.transform.translation.y = translation_y; + t.transform.translation.z = translation_z; + + t.transform.rotation.x = quaternion_x; + t.transform.rotation.y = quaternion_y; + t.transform.rotation.z = quaternion_z; + t.transform.rotation.w = quaternion_w; + return t; +} + +void Frame::update_quaternion(const std::vector & current_joints) +{ + double roll_deg = get_joint_angle(ROLL, current_joints) + const_roll; + double pitch_deg = get_joint_angle(PITCH, current_joints) + const_pitch; + double yaw_deg = get_joint_angle(YAW, current_joints) + const_yaw; + + double roll_rad = roll_deg * M_PI / 180.0; + double pitch_rad = pitch_deg * M_PI / 180.0; + double yaw_rad = yaw_deg * M_PI / 180.0; + + tf2::Quaternion q; + q.setRPY(roll_rad, pitch_rad, yaw_rad); + + quaternion_x = q.x(); + quaternion_y = q.y(); + quaternion_z = q.z(); + quaternion_w = q.w(); +} + +void Frame::update_quaternion(const keisan::Angle & imu_yaw) +{ + tf2::Quaternion q; + q.setRPY(0, 0, imu_yaw.radian()); + + quaternion_x = q.x(); + quaternion_y = q.y(); + quaternion_z = q.z(); + quaternion_w = q.w(); +} + +double Frame::get_joint_angle(const uint8_t & quaternion_axis, const std::vector & current_joints) +{ + auto map = FrameId::frame_joint_map.find(id); + std::vector frame_joints = map->second; + uint8_t id_map = map->first; + uint8_t joint_id = frame_joints[quaternion_axis]; + + if (joint_id == 0) { + return 0.0; + } + + for (auto joint : current_joints) { + if (joint.get_id() == joint_id) { + if ( + joint_id == JointId::LEFT_HIP_PITCH || joint_id == JointId::LEFT_KNEE || + joint_id == JointId::NECK_PITCH) { + return -joint.get_position(); + } + + return joint.get_position(); + } + } +} + +} // namespace tachimawari::joint diff --git a/src/tachimawari/joint/tf2/frame_id.cpp b/src/tachimawari/joint/tf2/frame_id.cpp new file mode 100644 index 0000000..586946f --- /dev/null +++ b/src/tachimawari/joint/tf2/frame_id.cpp @@ -0,0 +1,73 @@ +// 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. + +#include +#include +#include + +#include "tachimawari/joint/model/joint_id.hpp" +#include "tachimawari/joint/tf2/frame_id.hpp" + +namespace tachimawari::joint +{ + +const std::array FrameId::frame_ids = { + BASE_LINK, TORSO, HEAD, GAZE, RIGHT_THIGH, + RIGHT_CALF, RIGHT_FOOT, LEFT_THIGH, LEFT_CALF, LEFT_FOOT, +}; + +const std::map FrameId::frame_id_string = { + {FrameId::ODOM, "odom"}, {FrameId::BASE_LINK, "base_link"}, + {FrameId::TORSO, "torso"}, {FrameId::HEAD, "head"}, + {FrameId::GAZE, "gaze"}, {FrameId::RIGHT_THIGH, "r_thigh"}, + {FrameId::RIGHT_CALF, "r_calf"}, {FrameId::RIGHT_FOOT, "r_sole"}, + {FrameId::LEFT_THIGH, "l_thigh"}, {FrameId::LEFT_CALF, "l_calf"}, + {FrameId::LEFT_FOOT, "l_sole"}}; + +const std::map FrameId::frame_string_id = { + {"odom", FrameId::ODOM}, {"base_link", FrameId::BASE_LINK}, + {"torso", FrameId::TORSO}, {"head", FrameId::HEAD}, + {"gaze", FrameId::GAZE}, {"r_thigh", FrameId::RIGHT_THIGH}, + {"r_calf", FrameId::RIGHT_CALF}, {"r_sole", FrameId::RIGHT_FOOT}, + {"l_thigh", FrameId::LEFT_THIGH}, {"l_calf", FrameId::LEFT_CALF}, + {"l_sole", FrameId::LEFT_FOOT}}; + +const std::map> FrameId::frame_joint_map = { + {ODOM, {0, 0, 0}}, + {BASE_LINK, {0, 0, 0}}, + {TORSO, {0, 0, 0}}, + {GAZE, {0, 0, 0}}, + {HEAD, {0, JointId::NECK_PITCH, JointId::NECK_YAW}}, + {RIGHT_THIGH, {JointId::RIGHT_HIP_ROLL, JointId::RIGHT_HIP_PITCH, JointId::RIGHT_HIP_YAW}}, + {RIGHT_CALF, {0, JointId::RIGHT_KNEE, 0}}, + {RIGHT_FOOT, {JointId::RIGHT_ANKLE_ROLL, JointId::RIGHT_ANKLE_PITCH, 0}}, + {LEFT_THIGH, {JointId::LEFT_HIP_ROLL, JointId::LEFT_HIP_PITCH, JointId::LEFT_HIP_YAW}}, + {LEFT_CALF, {0, JointId::LEFT_KNEE, 0}}, + {LEFT_FOOT, {JointId::LEFT_ANKLE_ROLL, JointId::LEFT_ANKLE_PITCH, 0}}, +}; + +const std::map FrameId::parent_frame = { + {ODOM, "world"}, {BASE_LINK, "odom"}, {GAZE, "head"}, + {HEAD, "torso"}, {TORSO, "base_link"}, {RIGHT_THIGH, "base_link"}, + {RIGHT_CALF, "r_thigh"}, {RIGHT_FOOT, "r_calf"}, {LEFT_THIGH, "base_link"}, + {LEFT_CALF, "l_thigh"}, {LEFT_FOOT, "l_calf"}, +}; + +} // namespace tachimawari::joint diff --git a/src/tachimawari/joint/tf2/tf2_manager.cpp b/src/tachimawari/joint/tf2/tf2_manager.cpp new file mode 100644 index 0000000..6888f25 --- /dev/null +++ b/src/tachimawari/joint/tf2/tf2_manager.cpp @@ -0,0 +1,81 @@ +// 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. + +#include +#include +#include +#include +#include +#include + +#include "tachimawari/joint/tf2/tf2_manager.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 = nlohmann::json::parse(input); + + 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; + } + } + + return true; +} + +void Tf2Manager::update(const std::vector & current_joints, const keisan::Angle & imu_yaw) +{ + for (auto & item : frames) { + if (item.id == FrameId::ODOM) { + item.update_quaternion(imu_yaw); + } else { + item.update_quaternion(current_joints); + } + } +} + +} // namespace tachimawari::joint diff --git a/src/tachimawari/node/tachimawari_node.cpp b/src/tachimawari/node/tachimawari_node.cpp index 721c684..ea75346 100644 --- a/src/tachimawari/node/tachimawari_node.cpp +++ b/src/tachimawari/node/tachimawari_node.cpp @@ -18,8 +18,6 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include "tachimawari/node/tachimawari_node.hpp" - #include #include #include @@ -30,6 +28,7 @@ #include "tachimawari/imu/node/imu_provider.hpp" #include "tachimawari/joint/node/joint_manager.hpp" #include "tachimawari/joint/node/joint_node.hpp" +#include "tachimawari/node/tachimawari_node.hpp" using namespace std::chrono_literals; @@ -62,15 +61,16 @@ TachimawariNode::TachimawariNode( this->joint_node->update(); this->joint_node->publish_current_joints(); + this->joint_node->publish_frame_tree(); } } - }); + }); } -void TachimawariNode::run_joint_manager() +void TachimawariNode::run_joint_manager(const std::string & path) { joint_node = std::make_shared( - node, std::make_shared(control_manager)); + node, std::make_shared(control_manager), path); control_node = std::make_shared(node, control_manager); } diff --git a/src/tachimawari_main.cpp b/src/tachimawari_main.cpp index 8ceb1f9..4138be8 100644 --- a/src/tachimawari_main.cpp +++ b/src/tachimawari_main.cpp @@ -21,9 +21,9 @@ #include #include +#include "rclcpp/rclcpp.hpp" #include "tachimawari/control/control.hpp" #include "tachimawari/node/tachimawari_node.hpp" -#include "rclcpp/rclcpp.hpp" int main(int argc, char * argv[]) { @@ -32,9 +32,12 @@ int main(int argc, char * argv[]) if (argc < 2) { std::cerr << "Please specify the mode! [sdk / cm740]" << std::endl; return 0; + } else if (argc < 3) { + std::cerr << "Please specify the tf configuration path" << std::endl; } std::string mode = argv[1]; + std::string path = argv[2]; std::shared_ptr controller; if (mode == "sdk") { @@ -58,7 +61,7 @@ int main(int argc, char * argv[]) auto node = std::make_shared("tachimawari_node"); auto tachimawari_node = std::make_shared(node, controller); - tachimawari_node->run_joint_manager(); + tachimawari_node->run_joint_manager(path); tachimawari_node->run_imu_provider(); rclcpp::spin(node);