From ab5d62f8436fff40c7dabae43f15117c3a5f778a Mon Sep 17 00:00:00 2001 From: borednuna Date: Fri, 3 May 2024 22:48:59 +0700 Subject: [PATCH 01/19] feat: add tf2 implementation in joint node --- CMakeLists.txt | 4 + include/tachimawari/joint/node/joint_node.hpp | 9 ++ include/tachimawari/joint/tf2/frame.hpp | 70 +++++++++ include/tachimawari/joint/tf2/frame_id.hpp | 56 ++++++++ include/tachimawari/joint/tf2/tf2_manager.hpp | 58 ++++++++ include/tachimawari/joint/utils/utils.hpp | 43 ++++++ src/tachimawari/joint/node/joint_node.cpp | 14 ++ src/tachimawari/joint/tf2/frame.cpp | 93 ++++++++++++ src/tachimawari/joint/tf2/frame_id.cpp | 78 ++++++++++ src/tachimawari/joint/tf2/tf2_manager.cpp | 135 ++++++++++++++++++ src/tachimawari/joint/utils/utils.cpp | 135 ++++++++++++++++++ src/tachimawari/node/tachimawari_node.cpp | 1 + 12 files changed, 696 insertions(+) create mode 100644 include/tachimawari/joint/tf2/frame.hpp create mode 100644 include/tachimawari/joint/tf2/frame_id.hpp create mode 100644 include/tachimawari/joint/tf2/tf2_manager.hpp create mode 100644 include/tachimawari/joint/utils/utils.hpp create mode 100644 src/tachimawari/joint/tf2/frame.cpp create mode 100644 src/tachimawari/joint/tf2/frame_id.cpp create mode 100644 src/tachimawari/joint/tf2/tf2_manager.cpp create mode 100644 src/tachimawari/joint/utils/utils.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1ed3148..3573bf7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,6 +55,10 @@ 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" "src/${PROJECT_NAME}/node/tachimawari_node.cpp" # TODO: Enable me later. # "src/${PROJECT_NAME}/node/rviz_server_node.cpp" diff --git a/include/tachimawari/joint/node/joint_node.hpp b/include/tachimawari/joint/node/joint_node.hpp index e3fb094..ae6c84e 100644 --- a/include/tachimawari/joint/node/joint_node.hpp +++ b/include/tachimawari/joint/node/joint_node.hpp @@ -21,16 +21,21 @@ #ifndef TACHIMAWARI__JOINT__NODE__JOINT_NODE_HPP_ #define TACHIMAWARI__JOINT__NODE__JOINT_NODE_HPP_ +#include + #include #include #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 { @@ -52,6 +57,7 @@ class JointNode JointNode(rclcpp::Node::SharedPtr node, std::shared_ptr joint_manager); void publish_current_joints(); + void publish_frame_tree(); void update(); private: @@ -65,6 +71,9 @@ class JointNode 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..f067f4e --- /dev/null +++ b/include/tachimawari/joint/tf2/frame.hpp @@ -0,0 +1,70 @@ +// 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 + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rclcpp/time.hpp" +#include "tachimawari/joint/node/joint_manager.hpp" +#include "tachimawari/joint/tf2/frame_id.hpp" + +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 quaternion_x, const double quaternion_y, + const double quaternion_z, const double quaternion_w); + + void update_quaternion(std::vector current_joints); + geometry_msgs::msg::TransformStamped get_transform_stamped(rclcpp::Time time_stamp); + +private: + enum : uint8_t { + ROLL = 0, + PITCH = 1, + YAW = 2, + }; + + uint8_t id; + + double translation_x; + double translation_y; + double translation_z; + + double quaternion_x; // maybe change to keisan angle + double quaternion_y; + double quaternion_z; + double quaternion_w; + + double get_joint_angle(uint8_t quaternion_axis, 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..dca0cde --- /dev/null +++ b/include/tachimawari/joint/tf2/frame_id.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__FRAME_ID_HPP_ +#define TACHIMAWARI__JOINT_TF2__FRAME_ID_HPP_ + +#include +#include +#include +#include + +namespace tachimawari::joint +{ + +class FrameId +{ +public: + enum : uint8_t { + BASE_LINK = 0, + TORSO = 1, + GAZE = 2, + RIGHT_THIGH = 3, + RIGHT_CALF = 4, + RIGHT_FOOT = 5, + LEFT_THIGH = 6, + LEFT_CALF = 7, + LEFT_FOOT = 8, + }; + + 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_ \ No newline at end of file diff --git a/include/tachimawari/joint/tf2/tf2_manager.hpp b/include/tachimawari/joint/tf2/tf2_manager.hpp new file mode 100644 index 0000000..70186c2 --- /dev/null +++ b/include/tachimawari/joint/tf2/tf2_manager.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__TF2_MANAGER_HPP_ +#define TACHIMAWARI__JOINT__TF2__TF2_MANAGER_HPP_ + +#include +#include +#include + +#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 "tachimawari/joint/utils/utils.hpp" +#include "tf2_ros/transform_broadcaster.h" + +namespace tachimawari::joint +{ + +class Tf2Manager +{ +public: + explicit Tf2Manager(); + + bool load_configuration(); + bool save_configuration(); + bool sync_configuration(); + void update(std::vector current_joints); + std::vector get_frames() { return frames; } + +private: + static const std::vector> tf2_joint_pairs; + std::string config_path; + std::vector frames; +}; +} // namespace tachimawari::joint + +#endif // TACHIMAWARI__JOINT__TF2__TF2_MANAGER_HPP_ diff --git a/include/tachimawari/joint/utils/utils.hpp b/include/tachimawari/joint/utils/utils.hpp new file mode 100644 index 0000000..2123d7a --- /dev/null +++ b/include/tachimawari/joint/utils/utils.hpp @@ -0,0 +1,43 @@ +// 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__UTILS__UTILS_HPP +#define TACHIMAWARI_JOINT__UTILS__UTILS_HPP + +#include +#include + +namespace tachimawari::joint::utils +{ +std::string get_host_name(); +std::string get_env(std::string env); +bool is_root(); + +bool is_directory_exist(std::string path); +bool create_directory(std::string path); + +bool is_file_exist(std::string path); +bool create_file(std::string path); + +std::string split_string(std::string s, std::string del = " "); + +} // namespace tachimawari::joint::utils + +#endif // TACHIMAWARI__JOINT__UTILS__UTILS_HPP diff --git a/src/tachimawari/joint/node/joint_node.cpp b/src/tachimawari/joint/node/joint_node.cpp index 6cf45ed..4e3a0b3 100644 --- a/src/tachimawari/joint/node/joint_node.cpp +++ b/src/tachimawari/joint/node/joint_node.cpp @@ -71,6 +71,9 @@ JointNode::JointNode(rclcpp::Node::SharedPtr node, std::shared_ptr }); current_joints_publisher = node->create_publisher(current_joints_topic(), 10); + tf2_broadcaster = std::make_shared(node); + tf2_manager = std::make_shared(); + tf2_manager->load_configuration(); } void JointNode::update() {middleware.update();} @@ -87,7 +90,18 @@ void JointNode::publish_current_joints() joints[i].position = current_joints[i].get_position(); } + tf2_manager->update(current_joints); current_joints_publisher->publish(msg_joints); } +void JointNode::publish_frame_tree() +{ + // get time + 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..b9a2257 --- /dev/null +++ b/src/tachimawari/joint/tf2/frame.cpp @@ -0,0 +1,93 @@ +// 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 "tachimawari/joint/tf2/frame.hpp" + +#include +#include +#include +#include +#include +#include + +namespace tachimawari::joint +{ + +Frame::Frame( + const uint8_t id, const double translation_x, const double translation_y, + const double translation_z, const double quaternion_x, const double quaternion_y, + const double quaternion_z, const double quaternion_w) +: id(id), + translation_x(translation_x), + translation_y(translation_y), + translation_z(translation_z), + quaternion_x(quaternion_x), + quaternion_y(quaternion_y), + quaternion_z(quaternion_z), + quaternion_w(quaternion_w) +{ +} + +geometry_msgs::msg::TransformStamped Frame::get_transform_stamped(rclcpp::Time time_stamp) +{ + std::string frame_name = FrameId::frame_id_string.at(id); + std::string parent_frame_name = FrameId::frame_id_string.at(FrameId::parent_frame.at(id)); + + 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(std::vector current_joints) +{ + quaternion_x = get_joint_angle(ROLL, current_joints); + quaternion_y = get_joint_angle(PITCH, current_joints); + quaternion_z = get_joint_angle(YAW, current_joints); + quaternion_w = 1.0; +} + +double Frame::get_joint_angle(uint8_t quaternion_axis, std::vector current_joints) +{ + std::vector frame_joints = FrameId::frame_joint_map.at(id); + double joint_id = frame_joints[quaternion_axis]; + + if (joint_id == 0) { + return 0.0; + } + + for (auto joint : current_joints) { + if (joint.get_id() == joint_id) { + 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..69bfe19 --- /dev/null +++ b/src/tachimawari/joint/tf2/frame_id.cpp @@ -0,0 +1,78 @@ +// 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 "tachimawari/joint/tf2/frame_id.hpp" + +#include +#include +#include + +#include "tachimawari/joint/model/joint_id.hpp" + +namespace tachimawari::joint +{ + +const std::array FrameId::frame_ids = { + BASE_LINK, TORSO, GAZE, RIGHT_THIGH, RIGHT_CALF, RIGHT_FOOT, LEFT_THIGH, LEFT_CALF, LEFT_FOOT, +}; + +const std::map FrameId::frame_id_string = { + {FrameId::BASE_LINK, "BASE_LINK"}, + {FrameId::TORSO, "TORSO"}, + {FrameId::GAZE, "GAZE"}, + {FrameId::RIGHT_THIGH, "RIGHT_THIGH"}, + {FrameId::RIGHT_CALF, "RIGHT_CALF"}, + {FrameId::RIGHT_FOOT, "RIGHT_FOOT"}, + {FrameId::LEFT_THIGH, "LEFT_THIGH"}, + {FrameId::LEFT_CALF, "LEFT_CALF"}, + {FrameId::LEFT_FOOT, "LEFT_FOOT"}}; + +const std::map FrameId::frame_string_id = { + {"BASE_LINK", FrameId::BASE_LINK}, + {"TORSO", FrameId::TORSO}, + {"GAZE", FrameId::GAZE}, + {"RIGHT_THIGH", FrameId::RIGHT_THIGH}, + {"RIGHT_CALF", FrameId::RIGHT_CALF}, + {"RIGHT_FOOT", FrameId::RIGHT_FOOT}, + {"LEFT_THIGH", FrameId::LEFT_THIGH}, + {"LEFT_CALF", FrameId::LEFT_CALF}, + {"LEFT_FOOT", FrameId::LEFT_FOOT}}; + +const std::map> FrameId::frame_joint_map = { + {GAZE, {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 = { + {GAZE, TORSO}, + {RIGHT_THIGH, BASE_LINK}, + {RIGHT_CALF, RIGHT_THIGH}, + {RIGHT_FOOT, RIGHT_CALF}, + {LEFT_THIGH, BASE_LINK}, + {LEFT_CALF, LEFT_THIGH}, + {LEFT_FOOT, LEFT_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..e0b976b --- /dev/null +++ b/src/tachimawari/joint/tf2/tf2_manager.cpp @@ -0,0 +1,135 @@ +// 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 "tachimawari/joint/tf2/tf2_manager.hpp" + +#include +#include +#include +#include +#include +#include + +namespace tachimawari::joint +{ + +Tf2Manager::Tf2Manager() {} + +bool Tf2Manager::load_configuration() +{ + config_path = "/todo/"; + std::string ss = config_path + utils::get_host_name() + ".json"; + + if (utils::is_file_exist(ss) == false) { + if (save_configuration() == false) { + return false; + } + } + + std::ifstream input(ss, std::ifstream::in); + if (input.is_open() == false) { + return false; + } + + 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 quaternion_x = item.value().at("quaternion").at("x"); + double quaternion_y = item.value().at("quaternion").at("y"); + double quaternion_z = item.value().at("quaternion").at("z"); + double quaternion_w = item.value().at("quaternion").at("w"); + + auto iterator = FrameId::frame_string_id.find(name); + uint8_t id = iterator->second; + frames.push_back(Frame( + id, translation_x, translation_y, translation_z, quaternion_x, quaternion_y, quaternion_z, + quaternion_w)); + } catch (nlohmann::json::parse_error & ex) { + std::cerr << "parse error at byte " << ex.byte << std::endl; + } + } + + return true; +} + +bool Tf2Manager::save_configuration() +{ + 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) { + // nlohmann::json frame; + // frame["name"] = item.name; + // frame["translation"]["x"] = item.translation.x; + // frame["translation"]["y"] = item.translation.y; + // frame["translation"]["z"] = item.translation.z; + // frame["quaternion"]["x"] = item.quaternion.x; + // frame["quaternion"]["y"] = item.quaternion.y; + // frame["quaternion"]["z"] = item.quaternion.z; + // frame["quaternion"]["w"] = item.quaternion.w; + + // 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 current_joints) +{ + for (auto & item : frames) { + item.update_quaternion(current_joints); + } +} + +} // namespace tachimawari::joint diff --git a/src/tachimawari/joint/utils/utils.cpp b/src/tachimawari/joint/utils/utils.cpp new file mode 100644 index 0000000..ba651c0 --- /dev/null +++ b/src/tachimawari/joint/utils/utils.cpp @@ -0,0 +1,135 @@ +// 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 "tachimawari/joint/utils/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace tachimawari::joint::utils +{ + +std::string get_host_name() +{ + char hostname[32]; + if (gethostname(hostname, 32) != 0) { + return ""; + } + + return hostname; +} + +std::string get_env(std::string env) { return getenv(env.c_str()); } + +bool is_root() { return getuid() == 0; } + +bool is_directory_exist(std::string path) +{ + struct stat st; + if (stat(path.c_str(), &st) != 0) { + return false; + } + + return (st.st_mode & S_IFMT) == S_IFDIR; +} + +bool create_directory(std::string path) +{ + std::replace(path.begin(), path.end(), '/', ' '); + + std::vector directories; + std::stringstream ss(path); + + std::string str; + while (ss >> str) { + directories.push_back(str); + } + + ss.str(""); + ss.clear(); + for (std::vector::iterator it = directories.begin(); it != directories.end(); it++) { + ss << *it << "/"; + + if (!is_directory_exist(ss.str())) { + if (mkdir(ss.str().c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) != 0) { + return false; + } + } + } + + return true; +} + +bool is_file_exist(std::string path) +{ + struct stat st; + if (stat(path.c_str(), &st) != 0) { + return false; + } + + return (st.st_mode & S_IFMT) == S_IFREG; +} + +bool create_file(std::string path) +{ + std::size_t slash = path.find_last_of("/"); + if (slash < path.size()) { + std::string directory = path.substr(0, (slash == path.size()) ? 0 : slash); + + if (!is_directory_exist(directory)) { + if (!create_directory(directory)) { + return false; + } + } + } + + std::ofstream output; + output.open(path); + if (output.is_open() == false) { + return false; + } + + output.close(); + + return true; +} + +std::string split_string(std::string s, std::string del) +{ + int start = 0; + int end = s.find(del); + + while (end != -1) { + // cout << s.substr(start, end - start) << endl; + start = end + del.size(); + end = s.find(del, start); + } + + return s.substr(start, end - start); +} + +} // namespace tachimawari::joint::utils diff --git a/src/tachimawari/node/tachimawari_node.cpp b/src/tachimawari/node/tachimawari_node.cpp index 721c684..764c92b 100644 --- a/src/tachimawari/node/tachimawari_node.cpp +++ b/src/tachimawari/node/tachimawari_node.cpp @@ -62,6 +62,7 @@ TachimawariNode::TachimawariNode( this->joint_node->update(); this->joint_node->publish_current_joints(); + this->joint_node->publish_frame_tree(); } } }); From 67c7c15d1f0e139b380d3717bc5498afb92b3f02 Mon Sep 17 00:00:00 2001 From: borednuna Date: Sat, 4 May 2024 18:28:31 +0700 Subject: [PATCH 02/19] feat: add torso-base link to child parent mapping --- data/siber.json | 38 ++++++++++++++++++++++++++ src/tachimawari/joint/tf2/frame_id.cpp | 1 + 2 files changed, 39 insertions(+) create mode 100644 data/siber.json diff --git a/data/siber.json b/data/siber.json new file mode 100644 index 0000000..ccbb382 --- /dev/null +++ b/data/siber.json @@ -0,0 +1,38 @@ +{ + "BASE_LINK": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "TORSO": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "GAZE": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "RIGHT_THIGH": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "RIGHT_CALF": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "RIGHT_FOOT": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "LEFT_THIGH": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "LEFT_CALF": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "LEFT_FOOT": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + } +} diff --git a/src/tachimawari/joint/tf2/frame_id.cpp b/src/tachimawari/joint/tf2/frame_id.cpp index 69bfe19..b552742 100644 --- a/src/tachimawari/joint/tf2/frame_id.cpp +++ b/src/tachimawari/joint/tf2/frame_id.cpp @@ -67,6 +67,7 @@ const std::map> FrameId::frame_joint_map = { const std::map FrameId::parent_frame = { {GAZE, TORSO}, + {TORSO, BASE_LINK}, {RIGHT_THIGH, BASE_LINK}, {RIGHT_CALF, RIGHT_THIGH}, {RIGHT_FOOT, RIGHT_CALF}, From 699cdf510114f7f3d00e35e13159c9e5f54d7456 Mon Sep 17 00:00:00 2001 From: borednuna Date: Sat, 11 May 2024 20:05:14 +0700 Subject: [PATCH 03/19] fix: fix frame and joint mapping errors --- data/miru.json | 38 +++++++++++++++++++ include/tachimawari/joint/tf2/frame.hpp | 14 +++---- include/tachimawari/joint/tf2/frame_id.hpp | 2 +- src/tachimawari/data/miru.json | 38 +++++++++++++++++++ src/tachimawari/joint/node/joint_node.cpp | 1 + src/tachimawari/joint/tf2/frame.cpp | 13 +++++-- src/tachimawari/joint/tf2/frame_id.cpp | 20 +++++----- src/tachimawari/joint/tf2/tf2_manager.cpp | 44 ++++++++++++---------- 8 files changed, 129 insertions(+), 41 deletions(-) create mode 100644 data/miru.json create mode 100644 src/tachimawari/data/miru.json diff --git a/data/miru.json b/data/miru.json new file mode 100644 index 0000000..ccbb382 --- /dev/null +++ b/data/miru.json @@ -0,0 +1,38 @@ +{ + "BASE_LINK": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "TORSO": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "GAZE": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "RIGHT_THIGH": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "RIGHT_CALF": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "RIGHT_FOOT": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "LEFT_THIGH": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "LEFT_CALF": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "LEFT_FOOT": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + } +} diff --git a/include/tachimawari/joint/tf2/frame.hpp b/include/tachimawari/joint/tf2/frame.hpp index f067f4e..f85633c 100644 --- a/include/tachimawari/joint/tf2/frame.hpp +++ b/include/tachimawari/joint/tf2/frame.hpp @@ -45,13 +45,6 @@ class Frame void update_quaternion(std::vector current_joints); geometry_msgs::msg::TransformStamped get_transform_stamped(rclcpp::Time time_stamp); -private: - enum : uint8_t { - ROLL = 0, - PITCH = 1, - YAW = 2, - }; - uint8_t id; double translation_x; @@ -63,6 +56,13 @@ class Frame double quaternion_z; double quaternion_w; +private: + enum : uint8_t { + ROLL = 0, + PITCH = 1, + YAW = 2, + }; + double get_joint_angle(uint8_t quaternion_axis, std::vector current_joints); }; } // namespace tachimawari::joint diff --git a/include/tachimawari/joint/tf2/frame_id.hpp b/include/tachimawari/joint/tf2/frame_id.hpp index dca0cde..cbcc286 100644 --- a/include/tachimawari/joint/tf2/frame_id.hpp +++ b/include/tachimawari/joint/tf2/frame_id.hpp @@ -48,7 +48,7 @@ class FrameId 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; + static const std::map parent_frame; }; } // namespace tachimawari::joint diff --git a/src/tachimawari/data/miru.json b/src/tachimawari/data/miru.json new file mode 100644 index 0000000..e4e8faf --- /dev/null +++ b/src/tachimawari/data/miru.json @@ -0,0 +1,38 @@ +{ + "BASE_LINK": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "TORSO": { + "translation": {"x": 0.0, "y": 0.0, "z": 16.4124}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "GAZE": { + "translation": {"x": 2.9161, "y": 0.0, "z": 4.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "RIGHT_THIGH": { + "translation": {"x": 0.0, "y": 4.9346, "z": -4.8}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "RIGHT_CALF": { + "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "RIGHT_FOOT": { + "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "LEFT_THIGH": { + "translation": {"x": 0.0, "y": -4.9346, "z": -4.8}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "LEFT_CALF": { + "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, + "LEFT_FOOT": { + "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + } +} \ No newline at end of file diff --git a/src/tachimawari/joint/node/joint_node.cpp b/src/tachimawari/joint/node/joint_node.cpp index 4e3a0b3..94a20c9 100644 --- a/src/tachimawari/joint/node/joint_node.cpp +++ b/src/tachimawari/joint/node/joint_node.cpp @@ -100,6 +100,7 @@ void JointNode::publish_frame_tree() rclcpp::Time now = rclcpp::Clock().now(); for (auto & frame : tf2_manager->get_frames()) { + if (frame.id == 0) continue; tf2_broadcaster->sendTransform(frame.get_transform_stamped(now)); } } diff --git a/src/tachimawari/joint/tf2/frame.cpp b/src/tachimawari/joint/tf2/frame.cpp index b9a2257..ff202d9 100644 --- a/src/tachimawari/joint/tf2/frame.cpp +++ b/src/tachimawari/joint/tf2/frame.cpp @@ -47,8 +47,11 @@ Frame::Frame( geometry_msgs::msg::TransformStamped Frame::get_transform_stamped(rclcpp::Time time_stamp) { - std::string frame_name = FrameId::frame_id_string.at(id); - std::string parent_frame_name = FrameId::frame_id_string.at(FrameId::parent_frame.at(id)); + 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; @@ -76,8 +79,10 @@ void Frame::update_quaternion(std::vector current_joints) double Frame::get_joint_angle(uint8_t quaternion_axis, std::vector current_joints) { - std::vector frame_joints = FrameId::frame_joint_map.at(id); - double joint_id = frame_joints[quaternion_axis]; + 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; diff --git a/src/tachimawari/joint/tf2/frame_id.cpp b/src/tachimawari/joint/tf2/frame_id.cpp index b552742..5019325 100644 --- a/src/tachimawari/joint/tf2/frame_id.cpp +++ b/src/tachimawari/joint/tf2/frame_id.cpp @@ -56,6 +56,8 @@ const std::map FrameId::frame_string_id = { {"LEFT_FOOT", FrameId::LEFT_FOOT}}; const std::map> FrameId::frame_joint_map = { + {BASE_LINK, {0, 0, 0}}, + {TORSO, {0, 0, 0}}, {GAZE, {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}}, @@ -65,15 +67,15 @@ const std::map> FrameId::frame_joint_map = { {LEFT_FOOT, {JointId::LEFT_ANKLE_ROLL, JointId::LEFT_ANKLE_PITCH, 0}}, }; -const std::map FrameId::parent_frame = { - {GAZE, TORSO}, - {TORSO, BASE_LINK}, - {RIGHT_THIGH, BASE_LINK}, - {RIGHT_CALF, RIGHT_THIGH}, - {RIGHT_FOOT, RIGHT_CALF}, - {LEFT_THIGH, BASE_LINK}, - {LEFT_CALF, LEFT_THIGH}, - {LEFT_FOOT, LEFT_CALF}, +const std::map FrameId::parent_frame = { + {GAZE, "TORSO"}, + {TORSO, "BASE_LINK"}, + {RIGHT_THIGH, "BASE_LINK"}, + {RIGHT_CALF, "RIGHT_THIGH"}, + {RIGHT_FOOT, "RIGHT_CALF"}, + {LEFT_THIGH, "BASE_LINK"}, + {LEFT_CALF, "LEFT_THIGH"}, + {LEFT_FOOT, "LEFT_CALF"}, }; } // namespace tachimawari::joint diff --git a/src/tachimawari/joint/tf2/tf2_manager.cpp b/src/tachimawari/joint/tf2/tf2_manager.cpp index e0b976b..68982f0 100644 --- a/src/tachimawari/joint/tf2/tf2_manager.cpp +++ b/src/tachimawari/joint/tf2/tf2_manager.cpp @@ -34,7 +34,7 @@ Tf2Manager::Tf2Manager() {} bool Tf2Manager::load_configuration() { - config_path = "/todo/"; + 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) { @@ -77,6 +77,7 @@ bool Tf2Manager::load_configuration() 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) { @@ -87,19 +88,22 @@ bool Tf2Manager::save_configuration() nlohmann::json config = nlohmann::json::array(); - // for (auto & item : frames) { - // nlohmann::json frame; - // frame["name"] = item.name; - // frame["translation"]["x"] = item.translation.x; - // frame["translation"]["y"] = item.translation.y; - // frame["translation"]["z"] = item.translation.z; - // frame["quaternion"]["x"] = item.quaternion.x; - // frame["quaternion"]["y"] = item.quaternion.y; - // frame["quaternion"]["z"] = item.quaternion.z; - // frame["quaternion"]["w"] = item.quaternion.w; - - // config.push_back(frame); - // } + 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["quaternion"]["x"] = item.quaternion_x; + frame["quaternion"]["y"] = item.quaternion_y; + frame["quaternion"]["z"] = item.quaternion_z; + frame["quaternion"]["w"] = item.quaternion_w; + + config.push_back(frame); + } std::ofstream output(ss, std::ofstream::out); if (output.is_open() == false) { @@ -114,13 +118,13 @@ bool Tf2Manager::save_configuration() bool Tf2Manager::sync_configuration() { - // if (!load_configuration()) { - // return false; - // } + if (!load_configuration()) { + return false; + } - // if (!save_configuration()) { - // return false; - // } + if (!save_configuration()) { + return false; + } return true; } From c372bb54788b5581f2baa4d7e88b41779a874fab Mon Sep 17 00:00:00 2001 From: borednuna Date: Sat, 11 May 2024 21:58:42 +0700 Subject: [PATCH 04/19] feat: rename variable --- data/miru.json | 18 +++++++++--------- src/tachimawari/joint/tf2/tf2_manager.cpp | 4 ++-- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/data/miru.json b/data/miru.json index ccbb382..e4e8faf 100644 --- a/data/miru.json +++ b/data/miru.json @@ -4,35 +4,35 @@ "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "TORSO": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": 0.0, "z": 16.4124}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "GAZE": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 2.9161, "y": 0.0, "z": 4.0}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "RIGHT_THIGH": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": 4.9346, "z": -4.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "RIGHT_CALF": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "RIGHT_FOOT": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "LEFT_THIGH": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": -4.9346, "z": -4.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "LEFT_CALF": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "LEFT_FOOT": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} } -} +} \ No newline at end of file diff --git a/src/tachimawari/joint/tf2/tf2_manager.cpp b/src/tachimawari/joint/tf2/tf2_manager.cpp index 68982f0..825a812 100644 --- a/src/tachimawari/joint/tf2/tf2_manager.cpp +++ b/src/tachimawari/joint/tf2/tf2_manager.cpp @@ -61,9 +61,9 @@ bool Tf2Manager::load_configuration() double quaternion_y = item.value().at("quaternion").at("y"); double quaternion_z = item.value().at("quaternion").at("z"); double quaternion_w = item.value().at("quaternion").at("w"); + auto map_string_id = FrameId::frame_string_id.find(name); - auto iterator = FrameId::frame_string_id.find(name); - uint8_t id = iterator->second; + uint8_t id = map_string_id->second; frames.push_back(Frame( id, translation_x, translation_y, translation_z, quaternion_x, quaternion_y, quaternion_z, quaternion_w)); From 36daa518cbcfc8b74271928a05d6c02bdbad0a66 Mon Sep 17 00:00:00 2001 From: borednuna Date: Sat, 11 May 2024 23:03:17 +0700 Subject: [PATCH 05/19] feat: follow rep-120 for frames names --- data/miru.json | 18 ++++++------ src/tachimawari/joint/tf2/frame_id.cpp | 39 +++++++++----------------- 2 files changed, 22 insertions(+), 35 deletions(-) diff --git a/data/miru.json b/data/miru.json index e4e8faf..23acc87 100644 --- a/data/miru.json +++ b/data/miru.json @@ -1,37 +1,37 @@ { - "BASE_LINK": { + "base_link": { "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, - "TORSO": { + "torso": { "translation": {"x": 0.0, "y": 0.0, "z": 16.4124}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, - "GAZE": { + "gaze": { "translation": {"x": 2.9161, "y": 0.0, "z": 4.0}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, - "RIGHT_THIGH": { + "r_thigh": { "translation": {"x": 0.0, "y": 4.9346, "z": -4.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, - "RIGHT_CALF": { + "r_calf": { "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, - "RIGHT_FOOT": { + "r_sole": { "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, - "LEFT_THIGH": { + "l_thigh": { "translation": {"x": 0.0, "y": -4.9346, "z": -4.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, - "LEFT_CALF": { + "l_calf": { "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, - "LEFT_FOOT": { + "l_sole": { "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} } diff --git a/src/tachimawari/joint/tf2/frame_id.cpp b/src/tachimawari/joint/tf2/frame_id.cpp index 5019325..db0dde8 100644 --- a/src/tachimawari/joint/tf2/frame_id.cpp +++ b/src/tachimawari/joint/tf2/frame_id.cpp @@ -34,26 +34,18 @@ const std::array FrameId::frame_ids = { }; const std::map FrameId::frame_id_string = { - {FrameId::BASE_LINK, "BASE_LINK"}, - {FrameId::TORSO, "TORSO"}, - {FrameId::GAZE, "GAZE"}, - {FrameId::RIGHT_THIGH, "RIGHT_THIGH"}, - {FrameId::RIGHT_CALF, "RIGHT_CALF"}, - {FrameId::RIGHT_FOOT, "RIGHT_FOOT"}, - {FrameId::LEFT_THIGH, "LEFT_THIGH"}, - {FrameId::LEFT_CALF, "LEFT_CALF"}, - {FrameId::LEFT_FOOT, "LEFT_FOOT"}}; + {FrameId::BASE_LINK, "base_link"}, {FrameId::TORSO, "torso"}, + {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 = { - {"BASE_LINK", FrameId::BASE_LINK}, - {"TORSO", FrameId::TORSO}, - {"GAZE", FrameId::GAZE}, - {"RIGHT_THIGH", FrameId::RIGHT_THIGH}, - {"RIGHT_CALF", FrameId::RIGHT_CALF}, - {"RIGHT_FOOT", FrameId::RIGHT_FOOT}, - {"LEFT_THIGH", FrameId::LEFT_THIGH}, - {"LEFT_CALF", FrameId::LEFT_CALF}, - {"LEFT_FOOT", FrameId::LEFT_FOOT}}; + {"base_link", FrameId::BASE_LINK}, {"torso", FrameId::TORSO}, + {"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 = { {BASE_LINK, {0, 0, 0}}, @@ -68,14 +60,9 @@ const std::map> FrameId::frame_joint_map = { }; const std::map FrameId::parent_frame = { - {GAZE, "TORSO"}, - {TORSO, "BASE_LINK"}, - {RIGHT_THIGH, "BASE_LINK"}, - {RIGHT_CALF, "RIGHT_THIGH"}, - {RIGHT_FOOT, "RIGHT_CALF"}, - {LEFT_THIGH, "BASE_LINK"}, - {LEFT_CALF, "LEFT_THIGH"}, - {LEFT_FOOT, "LEFT_CALF"}, + {GAZE, "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 From f0b12c0497193a9aa00f3d05ba3e73d636e9be46 Mon Sep 17 00:00:00 2001 From: borednuna Date: Sun, 12 May 2024 01:50:51 +0700 Subject: [PATCH 06/19] fix: use quaternion from tf2 --- include/tachimawari/joint/tf2/frame.hpp | 1 + src/tachimawari/joint/tf2/frame.cpp | 19 +++++++++++++++---- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/include/tachimawari/joint/tf2/frame.hpp b/include/tachimawari/joint/tf2/frame.hpp index f85633c..1b2e2f5 100644 --- a/include/tachimawari/joint/tf2/frame.hpp +++ b/include/tachimawari/joint/tf2/frame.hpp @@ -29,6 +29,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp/time.hpp" #include "tachimawari/joint/node/joint_manager.hpp" +#include "tf2/LinearMath/Quaternion.h" #include "tachimawari/joint/tf2/frame_id.hpp" namespace tachimawari::joint diff --git a/src/tachimawari/joint/tf2/frame.cpp b/src/tachimawari/joint/tf2/frame.cpp index ff202d9..c5336e0 100644 --- a/src/tachimawari/joint/tf2/frame.cpp +++ b/src/tachimawari/joint/tf2/frame.cpp @@ -71,10 +71,21 @@ geometry_msgs::msg::TransformStamped Frame::get_transform_stamped(rclcpp::Time t void Frame::update_quaternion(std::vector current_joints) { - quaternion_x = get_joint_angle(ROLL, current_joints); - quaternion_y = get_joint_angle(PITCH, current_joints); - quaternion_z = get_joint_angle(YAW, current_joints); - quaternion_w = 1.0; + double roll_deg = get_joint_angle(ROLL, current_joints); + double pitch_deg = get_joint_angle(PITCH, current_joints); + double yaw_deg = get_joint_angle(YAW, current_joints); + + 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(); } double Frame::get_joint_angle(uint8_t quaternion_axis, std::vector current_joints) From 80eb33f0d6d3364e273c664093961fb0500fd453 Mon Sep 17 00:00:00 2001 From: borednuna Date: Sun, 12 May 2024 18:57:23 +0700 Subject: [PATCH 07/19] feat: create new walltimer for tf to avoid time disrepancy --- include/tachimawari/node/tachimawari_node.hpp | 1 + src/tachimawari/node/tachimawari_node.cpp | 38 ++++++++++--------- 2 files changed, 22 insertions(+), 17 deletions(-) diff --git a/include/tachimawari/node/tachimawari_node.hpp b/include/tachimawari/node/tachimawari_node.hpp index 7d33ad2..987b46a 100644 --- a/include/tachimawari/node/tachimawari_node.hpp +++ b/include/tachimawari/node/tachimawari_node.hpp @@ -46,6 +46,7 @@ class TachimawariNode private: rclcpp::Node::SharedPtr node; rclcpp::TimerBase::SharedPtr node_timer; + rclcpp::TimerBase::SharedPtr node_timer_tf; std::shared_ptr control_manager; diff --git a/src/tachimawari/node/tachimawari_node.cpp b/src/tachimawari/node/tachimawari_node.cpp index 764c92b..b265400 100644 --- a/src/tachimawari/node/tachimawari_node.cpp +++ b/src/tachimawari/node/tachimawari_node.cpp @@ -44,28 +44,32 @@ TachimawariNode::TachimawariNode( imu_node(nullptr), control_node(nullptr) { - node_timer = node->create_wall_timer( - 8ms, [this]() { - if (this->control_manager) { - this->control_manager->add_default_bulk_read_packet(); - this->control_manager->send_bulk_read_packet(); + node_timer = node->create_wall_timer(8ms, [this]() { + if (this->control_manager) { + this->control_manager->add_default_bulk_read_packet(); + this->control_manager->send_bulk_read_packet(); - if (this->imu_node) { - this->imu_node->update(); - } + if (this->imu_node) { + this->imu_node->update(); + } - if (this->control_node) { - this->control_node->update(); - } + if (this->control_node) { + this->control_node->update(); + } - if (this->joint_node) { - this->joint_node->update(); + if (this->joint_node) { + this->joint_node->update(); - this->joint_node->publish_current_joints(); - this->joint_node->publish_frame_tree(); - } + this->joint_node->publish_current_joints(); } - }); + } + }); + + node_timer_tf = node->create_wall_timer(30ms, [this]() { + if (this->joint_node) { + this->joint_node->publish_frame_tree(); + } + }); } void TachimawariNode::run_joint_manager() From b34838d090b6a11b345cfb374e16f92490367008 Mon Sep 17 00:00:00 2001 From: borednuna Date: Sun, 12 May 2024 23:47:16 +0700 Subject: [PATCH 08/19] feat: use imu from kansei for odom frame --- data/miru.json | 4 +++ include/tachimawari/joint/node/joint_node.hpp | 7 ++++ include/tachimawari/joint/tf2/frame.hpp | 4 ++- include/tachimawari/joint/tf2/frame_id.hpp | 19 +++++----- include/tachimawari/joint/tf2/tf2_manager.hpp | 3 +- src/tachimawari/joint/node/joint_node.cpp | 36 ++++++++++++------- src/tachimawari/joint/tf2/frame.cpp | 11 ++++++ src/tachimawari/joint/tf2/frame_id.cpp | 28 ++++++++------- src/tachimawari/joint/tf2/tf2_manager.cpp | 8 +++-- 9 files changed, 81 insertions(+), 39 deletions(-) diff --git a/data/miru.json b/data/miru.json index 23acc87..190fdef 100644 --- a/data/miru.json +++ b/data/miru.json @@ -1,4 +1,8 @@ { + "odom": { + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + }, "base_link": { "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} diff --git a/include/tachimawari/joint/node/joint_node.hpp b/include/tachimawari/joint/node/joint_node.hpp index ae6c84e..4324049 100644 --- a/include/tachimawari/joint/node/joint_node.hpp +++ b/include/tachimawari/joint/node/joint_node.hpp @@ -26,6 +26,8 @@ #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" @@ -47,14 +49,17 @@ 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); + keisan::Angle imu_yaw; void publish_current_joints(); void publish_frame_tree(); @@ -69,6 +74,8 @@ class JointNode rclcpp::Subscription::SharedPtr set_torques_subscriber; + rclcpp::Subscription::SharedPtr status_subscriber; + Middleware middleware; rclcpp::Subscription::SharedPtr control_joints_subscriber; diff --git a/include/tachimawari/joint/tf2/frame.hpp b/include/tachimawari/joint/tf2/frame.hpp index 1b2e2f5..7752d9b 100644 --- a/include/tachimawari/joint/tf2/frame.hpp +++ b/include/tachimawari/joint/tf2/frame.hpp @@ -27,10 +27,11 @@ #include #include "geometry_msgs/msg/transform_stamped.hpp" +#include "keisan/angle/angle.hpp" #include "rclcpp/time.hpp" #include "tachimawari/joint/node/joint_manager.hpp" -#include "tf2/LinearMath/Quaternion.h" #include "tachimawari/joint/tf2/frame_id.hpp" +#include "tf2/LinearMath/Quaternion.h" namespace tachimawari::joint { @@ -44,6 +45,7 @@ class Frame const double quaternion_z, const double quaternion_w); void update_quaternion(std::vector current_joints); + void update_quaternion(keisan::Angle imu_yaw); geometry_msgs::msg::TransformStamped get_transform_stamped(rclcpp::Time time_stamp); uint8_t id; diff --git a/include/tachimawari/joint/tf2/frame_id.hpp b/include/tachimawari/joint/tf2/frame_id.hpp index cbcc286..d62d99a 100644 --- a/include/tachimawari/joint/tf2/frame_id.hpp +++ b/include/tachimawari/joint/tf2/frame_id.hpp @@ -33,15 +33,16 @@ class FrameId { public: enum : uint8_t { - BASE_LINK = 0, - TORSO = 1, - GAZE = 2, - RIGHT_THIGH = 3, - RIGHT_CALF = 4, - RIGHT_FOOT = 5, - LEFT_THIGH = 6, - LEFT_CALF = 7, - LEFT_FOOT = 8, + ODOM = 0, + BASE_LINK = 1, + TORSO = 2, + GAZE = 3, + RIGHT_THIGH = 4, + RIGHT_CALF = 5, + RIGHT_FOOT = 6, + LEFT_THIGH = 7, + LEFT_CALF = 9, + LEFT_FOOT = 10, }; static const std::array frame_ids; diff --git a/include/tachimawari/joint/tf2/tf2_manager.hpp b/include/tachimawari/joint/tf2/tf2_manager.hpp index 70186c2..843a13f 100644 --- a/include/tachimawari/joint/tf2/tf2_manager.hpp +++ b/include/tachimawari/joint/tf2/tf2_manager.hpp @@ -25,6 +25,7 @@ #include #include +#include "keisan/angle/angle.hpp" #include "rclcpp/time.hpp" #include "tachimawari/joint/model/joint.hpp" #include "tachimawari/joint/model/joint_id.hpp" @@ -45,7 +46,7 @@ class Tf2Manager bool load_configuration(); bool save_configuration(); bool sync_configuration(); - void update(std::vector current_joints); + void update(std::vector current_joints, keisan::Angle imu_yaw); std::vector get_frames() { return frames; } private: diff --git a/src/tachimawari/joint/node/joint_node.cpp b/src/tachimawari/joint/node/joint_node.cpp index 94a20c9..74f81ba 100644 --- a/src/tachimawari/joint/node/joint_node.cpp +++ b/src/tachimawari/joint/node/joint_node.cpp @@ -34,19 +34,27 @@ 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::current_joints_topic() {return get_node_prefix() + "/current_joints";} +std::string JointNode::status_topic() { return get_node_prefix() + "/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() +: 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(); + control_joints_subscriber = node->create_subscription( control_joints_topic(), 10, [this](const ControlJoints::SharedPtr message) { this->middleware.set_rules(message->control_type, message->ids); @@ -65,18 +73,21 @@ JointNode::JointNode(rclcpp::Node::SharedPtr node, std::shared_ptr std::vector 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); }); + status_subscriber = + node->create_subscription(status_topic(), 10, [this](const Status::SharedPtr message) { + imu_yaw = keisan::make_degree(message->orientation.yaw); + std::cout << "Updated imu_yaw" << imu_yaw.radian() << std::endl; + }); + current_joints_publisher = node->create_publisher(current_joints_topic(), 10); - tf2_broadcaster = std::make_shared(node); - tf2_manager = std::make_shared(); - tf2_manager->load_configuration(); } -void JointNode::update() {middleware.update();} +void JointNode::update() { middleware.update(); } void JointNode::publish_current_joints() { @@ -90,7 +101,7 @@ void JointNode::publish_current_joints() joints[i].position = current_joints[i].get_position(); } - tf2_manager->update(current_joints); + tf2_manager->update(current_joints, imu_yaw); current_joints_publisher->publish(msg_joints); } @@ -100,7 +111,6 @@ void JointNode::publish_frame_tree() rclcpp::Time now = rclcpp::Clock().now(); for (auto & frame : tf2_manager->get_frames()) { - if (frame.id == 0) continue; tf2_broadcaster->sendTransform(frame.get_transform_stamped(now)); } } diff --git a/src/tachimawari/joint/tf2/frame.cpp b/src/tachimawari/joint/tf2/frame.cpp index c5336e0..dde5d72 100644 --- a/src/tachimawari/joint/tf2/frame.cpp +++ b/src/tachimawari/joint/tf2/frame.cpp @@ -88,6 +88,17 @@ void Frame::update_quaternion(std::vector current_joints) quaternion_w = q.w(); } +void Frame::update_quaternion(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(uint8_t quaternion_axis, std::vector current_joints) { auto map = FrameId::frame_joint_map.find(id); diff --git a/src/tachimawari/joint/tf2/frame_id.cpp b/src/tachimawari/joint/tf2/frame_id.cpp index db0dde8..d531da9 100644 --- a/src/tachimawari/joint/tf2/frame_id.cpp +++ b/src/tachimawari/joint/tf2/frame_id.cpp @@ -34,20 +34,21 @@ const std::array FrameId::frame_ids = { }; const std::map FrameId::frame_id_string = { - {FrameId::BASE_LINK, "base_link"}, {FrameId::TORSO, "torso"}, - {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"}}; + {FrameId::ODOM, "odom"}, {FrameId::BASE_LINK, "base_link"}, + {FrameId::TORSO, "torso"}, {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 = { - {"base_link", FrameId::BASE_LINK}, {"torso", FrameId::TORSO}, - {"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}}; + {"odom", FrameId::ODOM}, {"base_link", FrameId::BASE_LINK}, + {"torso", FrameId::TORSO}, {"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, JointId::NECK_PITCH, JointId::NECK_YAW}}, @@ -60,9 +61,10 @@ const std::map> FrameId::frame_joint_map = { }; const std::map FrameId::parent_frame = { - {GAZE, "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"}, + {ODOM, "world"}, {BASE_LINK, "odom"}, {GAZE, "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 index 825a812..aab42f8 100644 --- a/src/tachimawari/joint/tf2/tf2_manager.cpp +++ b/src/tachimawari/joint/tf2/tf2_manager.cpp @@ -129,10 +129,14 @@ bool Tf2Manager::sync_configuration() return true; } -void Tf2Manager::update(std::vector current_joints) +void Tf2Manager::update(std::vector current_joints, keisan::Angle imu_yaw) { for (auto & item : frames) { - item.update_quaternion(current_joints); + if (item.id == 0) { + item.update_quaternion(imu_yaw); + } else { + item.update_quaternion(current_joints); + } } } From f5a6aa7f5ae7d787d1f449c1c7ca9e329b05dd90 Mon Sep 17 00:00:00 2001 From: borednuna Date: Tue, 14 May 2024 00:45:43 +0700 Subject: [PATCH 09/19] fix: fix topic name for imu subscriber --- src/tachimawari/joint/node/joint_node.cpp | 3 +-- src/tachimawari/node/tachimawari_node.cpp | 7 +------ 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/src/tachimawari/joint/node/joint_node.cpp b/src/tachimawari/joint/node/joint_node.cpp index 74f81ba..2759067 100644 --- a/src/tachimawari/joint/node/joint_node.cpp +++ b/src/tachimawari/joint/node/joint_node.cpp @@ -42,7 +42,7 @@ std::string JointNode::set_joints_topic() { return get_node_prefix() + "/set_joi std::string JointNode::set_torques_topic() { return get_node_prefix() + "/set_torques"; } -std::string JointNode::status_topic() { return get_node_prefix() + "/status"; } +std::string JointNode::status_topic() { return "measurement/status"; } std::string JointNode::current_joints_topic() { return get_node_prefix() + "/current_joints"; } @@ -81,7 +81,6 @@ JointNode::JointNode(rclcpp::Node::SharedPtr node, std::shared_ptr status_subscriber = node->create_subscription(status_topic(), 10, [this](const Status::SharedPtr message) { imu_yaw = keisan::make_degree(message->orientation.yaw); - std::cout << "Updated imu_yaw" << imu_yaw.radian() << std::endl; }); current_joints_publisher = node->create_publisher(current_joints_topic(), 10); diff --git a/src/tachimawari/node/tachimawari_node.cpp b/src/tachimawari/node/tachimawari_node.cpp index b265400..1cf13ae 100644 --- a/src/tachimawari/node/tachimawari_node.cpp +++ b/src/tachimawari/node/tachimawari_node.cpp @@ -61,15 +61,10 @@ TachimawariNode::TachimawariNode( this->joint_node->update(); this->joint_node->publish_current_joints(); + this->joint_node->publish_frame_tree(); } } }); - - node_timer_tf = node->create_wall_timer(30ms, [this]() { - if (this->joint_node) { - this->joint_node->publish_frame_tree(); - } - }); } void TachimawariNode::run_joint_manager() From 60e5acc5ef812813a1afbdd1a65f81cea81b031d Mon Sep 17 00:00:00 2001 From: borednuna Date: Wed, 15 May 2024 00:59:12 +0700 Subject: [PATCH 10/19] fix: add tf2_ros to target install cmakelist --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3573bf7..0a7931d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -174,6 +174,7 @@ ament_export_dependencies( kansei_interfaces keisan rclcpp + tf2_ros tachimawari_interfaces) ament_export_include_directories("include") ament_export_libraries(${PROJECT_NAME}) From 3a6bea7dab4864dd2ebe06668086329fa38e3143 Mon Sep 17 00:00:00 2001 From: borednuna Date: Wed, 22 May 2024 23:49:32 +0700 Subject: [PATCH 11/19] fix: fix tf light calculation > > Co-authored-by: FaaizHaikal Co-authored-by: hiikariri --- data/miru.json | 4 ++-- src/tachimawari/joint/tf2/frame.cpp | 8 +++++++- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/data/miru.json b/data/miru.json index 190fdef..b508b3c 100644 --- a/data/miru.json +++ b/data/miru.json @@ -16,7 +16,7 @@ "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "r_thigh": { - "translation": {"x": 0.0, "y": 4.9346, "z": -4.8}, + "translation": {"x": 0.0, "y": -4.9346, "z": -4.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "r_calf": { @@ -28,7 +28,7 @@ "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "l_thigh": { - "translation": {"x": 0.0, "y": -4.9346, "z": -4.8}, + "translation": {"x": 0.0, "y": 4.9346, "z": -4.8}, "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} }, "l_calf": { diff --git a/src/tachimawari/joint/tf2/frame.cpp b/src/tachimawari/joint/tf2/frame.cpp index dde5d72..f64d29d 100644 --- a/src/tachimawari/joint/tf2/frame.cpp +++ b/src/tachimawari/joint/tf2/frame.cpp @@ -74,7 +74,7 @@ void Frame::update_quaternion(std::vector current_joints) double roll_deg = get_joint_angle(ROLL, current_joints); double pitch_deg = get_joint_angle(PITCH, current_joints); double yaw_deg = get_joint_angle(YAW, current_joints); - + 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; @@ -112,6 +112,12 @@ double Frame::get_joint_angle(uint8_t quaternion_axis, std::vector curren 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(); } } From e378c75702502de35b99821e7d80e99a86de6e14 Mon Sep 17 00:00:00 2001 From: borednuna Date: Sat, 25 May 2024 00:51:49 +0700 Subject: [PATCH 12/19] fix: add head and gaze rotation to optimize accuracy, fix error tf values Co-authored-by: hiikariri Co-authored-by: FaaizHaikal --- data/miru.json | 30 ++++++++++------- include/tachimawari/joint/tf2/frame.hpp | 8 +++-- include/tachimawari/joint/tf2/frame_id.hpp | 15 +++++---- src/tachimawari/data/miru.json | 38 ---------------------- src/tachimawari/joint/tf2/frame.cpp | 21 +++++++----- src/tachimawari/joint/tf2/frame_id.cpp | 38 ++++++++++++---------- src/tachimawari/joint/tf2/tf2_manager.cpp | 19 +++++------ 7 files changed, 74 insertions(+), 95 deletions(-) delete mode 100644 src/tachimawari/data/miru.json diff --git a/data/miru.json b/data/miru.json index b508b3c..583fc52 100644 --- a/data/miru.json +++ b/data/miru.json @@ -1,42 +1,50 @@ { "odom": { "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, "base_link": { "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, "torso": { "translation": {"x": 0.0, "y": 0.0, "z": 16.4124}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, - "gaze": { + "head": { "translation": {"x": 2.9161, "y": 0.0, "z": 4.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} + }, + "gaze": { + "translation": {"x": 0.0, "y": 0.0, "z": 6.22}, + "const_rpy": {"roll": 0.0, "pitch": 90.0, "yaw": 0.0} + }, + "camera": { + "translation": {"x": 0.0, "y": 0.0, "z": 4.0}, + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, "r_thigh": { "translation": {"x": 0.0, "y": -4.9346, "z": -4.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, "r_calf": { "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, "r_sole": { "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, "l_thigh": { "translation": {"x": 0.0, "y": 4.9346, "z": -4.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, "l_calf": { "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, "l_sole": { "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} + "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} } } \ No newline at end of file diff --git a/include/tachimawari/joint/tf2/frame.hpp b/include/tachimawari/joint/tf2/frame.hpp index 7752d9b..7e5e900 100644 --- a/include/tachimawari/joint/tf2/frame.hpp +++ b/include/tachimawari/joint/tf2/frame.hpp @@ -41,8 +41,8 @@ class Frame public: Frame( const uint8_t id, const double translation_x, const double translation_y, - const double translation_z, const double quaternion_x, const double quaternion_y, - const double quaternion_z, const double quaternion_w); + const double translation_z, const double const_roll, const double const_pitch, + const double const_yaw); void update_quaternion(std::vector current_joints); void update_quaternion(keisan::Angle imu_yaw); @@ -59,6 +59,10 @@ class Frame double quaternion_z; double quaternion_w; + double const_roll; + double const_pitch; + double const_yaw; + private: enum : uint8_t { ROLL = 0, diff --git a/include/tachimawari/joint/tf2/frame_id.hpp b/include/tachimawari/joint/tf2/frame_id.hpp index d62d99a..43029b7 100644 --- a/include/tachimawari/joint/tf2/frame_id.hpp +++ b/include/tachimawari/joint/tf2/frame_id.hpp @@ -36,16 +36,17 @@ class FrameId ODOM = 0, BASE_LINK = 1, TORSO = 2, - GAZE = 3, - RIGHT_THIGH = 4, - RIGHT_CALF = 5, - RIGHT_FOOT = 6, - LEFT_THIGH = 7, + HEAD = 3, + GAZE = 4, + RIGHT_THIGH = 5, + RIGHT_CALF = 6, + RIGHT_FOOT = 7, + LEFT_THIGH = 8, LEFT_CALF = 9, - LEFT_FOOT = 10, + LEFT_FOOT = 10 }; - static const std::array frame_ids; + 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; diff --git a/src/tachimawari/data/miru.json b/src/tachimawari/data/miru.json deleted file mode 100644 index e4e8faf..0000000 --- a/src/tachimawari/data/miru.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "BASE_LINK": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "TORSO": { - "translation": {"x": 0.0, "y": 0.0, "z": 16.4124}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "GAZE": { - "translation": {"x": 2.9161, "y": 0.0, "z": 4.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "RIGHT_THIGH": { - "translation": {"x": 0.0, "y": 4.9346, "z": -4.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "RIGHT_CALF": { - "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "RIGHT_FOOT": { - "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "LEFT_THIGH": { - "translation": {"x": 0.0, "y": -4.9346, "z": -4.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "LEFT_CALF": { - "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "LEFT_FOOT": { - "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - } -} \ No newline at end of file diff --git a/src/tachimawari/joint/tf2/frame.cpp b/src/tachimawari/joint/tf2/frame.cpp index f64d29d..3d50a60 100644 --- a/src/tachimawari/joint/tf2/frame.cpp +++ b/src/tachimawari/joint/tf2/frame.cpp @@ -32,16 +32,19 @@ namespace tachimawari::joint Frame::Frame( const uint8_t id, const double translation_x, const double translation_y, - const double translation_z, const double quaternion_x, const double quaternion_y, - const double quaternion_z, const double quaternion_w) + 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(quaternion_x), - quaternion_y(quaternion_y), - quaternion_z(quaternion_z), - quaternion_w(quaternion_w) + quaternion_x(0), + quaternion_y(0), + quaternion_z(0), + quaternion_w(0), + const_roll(const_roll), + const_pitch(const_pitch), + const_yaw(const_yaw) { } @@ -71,9 +74,9 @@ geometry_msgs::msg::TransformStamped Frame::get_transform_stamped(rclcpp::Time t void Frame::update_quaternion(std::vector current_joints) { - double roll_deg = get_joint_angle(ROLL, current_joints); - double pitch_deg = get_joint_angle(PITCH, current_joints); - double yaw_deg = get_joint_angle(YAW, 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; diff --git a/src/tachimawari/joint/tf2/frame_id.cpp b/src/tachimawari/joint/tf2/frame_id.cpp index d531da9..2e37930 100644 --- a/src/tachimawari/joint/tf2/frame_id.cpp +++ b/src/tachimawari/joint/tf2/frame_id.cpp @@ -29,29 +29,33 @@ namespace tachimawari::joint { -const std::array FrameId::frame_ids = { - BASE_LINK, TORSO, GAZE, RIGHT_THIGH, RIGHT_CALF, RIGHT_FOOT, LEFT_THIGH, LEFT_CALF, LEFT_FOOT, +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::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"}}; + {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}, {"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}}; + {"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, JointId::NECK_PITCH, JointId::NECK_YAW}}, + {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}}, @@ -61,10 +65,10 @@ const std::map> FrameId::frame_joint_map = { }; const std::map FrameId::parent_frame = { - {ODOM, "world"}, {BASE_LINK, "odom"}, {GAZE, "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"}, + {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 index aab42f8..1a6cef4 100644 --- a/src/tachimawari/joint/tf2/tf2_manager.cpp +++ b/src/tachimawari/joint/tf2/tf2_manager.cpp @@ -57,16 +57,14 @@ bool Tf2Manager::load_configuration() 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 quaternion_x = item.value().at("quaternion").at("x"); - double quaternion_y = item.value().at("quaternion").at("y"); - double quaternion_z = item.value().at("quaternion").at("z"); - double quaternion_w = item.value().at("quaternion").at("w"); + 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, quaternion_x, quaternion_y, quaternion_z, - quaternion_w)); + 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; } @@ -97,10 +95,9 @@ bool Tf2Manager::save_configuration() frame["translation"]["x"] = item.translation_x; frame["translation"]["y"] = item.translation_y; frame["translation"]["z"] = item.translation_z; - frame["quaternion"]["x"] = item.quaternion_x; - frame["quaternion"]["y"] = item.quaternion_y; - frame["quaternion"]["z"] = item.quaternion_z; - frame["quaternion"]["w"] = item.quaternion_w; + 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); } From df9aae29c04759d548664d8d09880ff14e6db238 Mon Sep 17 00:00:00 2001 From: borednuna Date: Sat, 25 May 2024 22:35:06 +0700 Subject: [PATCH 13/19] fix: fix wrong measurements Co-authored-by: hiikariri Co-authored-by: FaaizHaikal --- data/miru.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/data/miru.json b/data/miru.json index 583fc52..deccf48 100644 --- a/data/miru.json +++ b/data/miru.json @@ -12,11 +12,11 @@ "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, "head": { - "translation": {"x": 2.9161, "y": 0.0, "z": 4.0}, + "translation": {"x": 2.9161, "y": 0.0, "z": 6.22}, "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} }, "gaze": { - "translation": {"x": 0.0, "y": 0.0, "z": 6.22}, + "translation": {"x": 0.0, "y": 0.0, "z": 2.34}, "const_rpy": {"roll": 0.0, "pitch": 90.0, "yaw": 0.0} }, "camera": { From c590e5b0f478e6fbfc4696f121da3acb2e8887be Mon Sep 17 00:00:00 2001 From: borednuna Date: Sat, 25 May 2024 23:34:15 +0700 Subject: [PATCH 14/19] feat: change load configuration path, remove unused lines, tidy some lines --- CMakeLists.txt | 1 - data/miru.json | 50 ------- data/siber.json | 38 ----- include/tachimawari/joint/node/joint_node.hpp | 3 +- include/tachimawari/joint/tf2/tf2_manager.hpp | 5 +- include/tachimawari/joint/utils/utils.hpp | 43 ------ include/tachimawari/node/tachimawari_node.hpp | 2 +- src/tachimawari/joint/node/joint_node.cpp | 11 +- src/tachimawari/joint/tf2/tf2_manager.cpp | 70 +-------- src/tachimawari/joint/utils/utils.cpp | 135 ------------------ src/tachimawari/node/tachimawari_node.cpp | 4 +- src/tachimawari_main.cpp | 7 +- 12 files changed, 22 insertions(+), 347 deletions(-) delete mode 100644 data/miru.json delete mode 100644 data/siber.json delete mode 100644 include/tachimawari/joint/utils/utils.hpp delete mode 100644 src/tachimawari/joint/utils/utils.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0a7931d..babce85 100644 --- a/CMakeLists.txt +++ b/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" diff --git a/data/miru.json b/data/miru.json deleted file mode 100644 index deccf48..0000000 --- a/data/miru.json +++ /dev/null @@ -1,50 +0,0 @@ -{ - "odom": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - }, - "base_link": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - }, - "torso": { - "translation": {"x": 0.0, "y": 0.0, "z": 16.4124}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - }, - "head": { - "translation": {"x": 2.9161, "y": 0.0, "z": 6.22}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - }, - "gaze": { - "translation": {"x": 0.0, "y": 0.0, "z": 2.34}, - "const_rpy": {"roll": 0.0, "pitch": 90.0, "yaw": 0.0} - }, - "camera": { - "translation": {"x": 0.0, "y": 0.0, "z": 4.0}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - }, - "r_thigh": { - "translation": {"x": 0.0, "y": -4.9346, "z": -4.8}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - }, - "r_calf": { - "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - }, - "r_sole": { - "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - }, - "l_thigh": { - "translation": {"x": 0.0, "y": 4.9346, "z": -4.8}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - }, - "l_calf": { - "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - }, - "l_sole": { - "translation": {"x": 0.0, "y": 0.0, "z": -13.8}, - "const_rpy": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0} - } -} \ No newline at end of file diff --git a/data/siber.json b/data/siber.json deleted file mode 100644 index ccbb382..0000000 --- a/data/siber.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "BASE_LINK": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "TORSO": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "GAZE": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "RIGHT_THIGH": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "RIGHT_CALF": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "RIGHT_FOOT": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "LEFT_THIGH": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "LEFT_CALF": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - }, - "LEFT_FOOT": { - "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, - "quaternion": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 0.0} - } -} diff --git a/include/tachimawari/joint/node/joint_node.hpp b/include/tachimawari/joint/node/joint_node.hpp index 4324049..075fc51 100644 --- a/include/tachimawari/joint/node/joint_node.hpp +++ b/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 joint_manager); + JointNode( + rclcpp::Node::SharedPtr node, std::shared_ptr joint_manager, std::string path); keisan::Angle imu_yaw; void publish_current_joints(); diff --git a/include/tachimawari/joint/tf2/tf2_manager.hpp b/include/tachimawari/joint/tf2/tf2_manager.hpp index 843a13f..fbe346e 100644 --- a/include/tachimawari/joint/tf2/tf2_manager.hpp +++ b/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 current_joints, keisan::Angle imu_yaw); std::vector get_frames() { return frames; } diff --git a/include/tachimawari/joint/utils/utils.hpp b/include/tachimawari/joint/utils/utils.hpp deleted file mode 100644 index 2123d7a..0000000 --- a/include/tachimawari/joint/utils/utils.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// 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__UTILS__UTILS_HPP -#define TACHIMAWARI_JOINT__UTILS__UTILS_HPP - -#include -#include - -namespace tachimawari::joint::utils -{ -std::string get_host_name(); -std::string get_env(std::string env); -bool is_root(); - -bool is_directory_exist(std::string path); -bool create_directory(std::string path); - -bool is_file_exist(std::string path); -bool create_file(std::string path); - -std::string split_string(std::string s, std::string del = " "); - -} // namespace tachimawari::joint::utils - -#endif // TACHIMAWARI__JOINT__UTILS__UTILS_HPP diff --git a/include/tachimawari/node/tachimawari_node.hpp b/include/tachimawari/node/tachimawari_node.hpp index 987b46a..814b625 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(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 2759067..516f694 100644 --- a/src/tachimawari/joint/node/joint_node.cpp +++ b/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 joint_manager) +JointNode::JointNode( + rclcpp::Node::SharedPtr node, std::shared_ptr joint_manager, 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(); + tf2_manager->load_configuration(path); control_joints_subscriber = node->create_subscription( 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)); } diff --git a/src/tachimawari/joint/tf2/tf2_manager.cpp b/src/tachimawari/joint/tf2/tf2_manager.cpp index 1a6cef4..b15d402 100644 --- a/src/tachimawari/joint/tf2/tf2_manager.cpp +++ b/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 current_joints, keisan::Angle 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); diff --git a/src/tachimawari/joint/utils/utils.cpp b/src/tachimawari/joint/utils/utils.cpp deleted file mode 100644 index ba651c0..0000000 --- a/src/tachimawari/joint/utils/utils.cpp +++ /dev/null @@ -1,135 +0,0 @@ -// 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 "tachimawari/joint/utils/utils.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace tachimawari::joint::utils -{ - -std::string get_host_name() -{ - char hostname[32]; - if (gethostname(hostname, 32) != 0) { - return ""; - } - - return hostname; -} - -std::string get_env(std::string env) { return getenv(env.c_str()); } - -bool is_root() { return getuid() == 0; } - -bool is_directory_exist(std::string path) -{ - struct stat st; - if (stat(path.c_str(), &st) != 0) { - return false; - } - - return (st.st_mode & S_IFMT) == S_IFDIR; -} - -bool create_directory(std::string path) -{ - std::replace(path.begin(), path.end(), '/', ' '); - - std::vector directories; - std::stringstream ss(path); - - std::string str; - while (ss >> str) { - directories.push_back(str); - } - - ss.str(""); - ss.clear(); - for (std::vector::iterator it = directories.begin(); it != directories.end(); it++) { - ss << *it << "/"; - - if (!is_directory_exist(ss.str())) { - if (mkdir(ss.str().c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) != 0) { - return false; - } - } - } - - return true; -} - -bool is_file_exist(std::string path) -{ - struct stat st; - if (stat(path.c_str(), &st) != 0) { - return false; - } - - return (st.st_mode & S_IFMT) == S_IFREG; -} - -bool create_file(std::string path) -{ - std::size_t slash = path.find_last_of("/"); - if (slash < path.size()) { - std::string directory = path.substr(0, (slash == path.size()) ? 0 : slash); - - if (!is_directory_exist(directory)) { - if (!create_directory(directory)) { - return false; - } - } - } - - std::ofstream output; - output.open(path); - if (output.is_open() == false) { - return false; - } - - output.close(); - - return true; -} - -std::string split_string(std::string s, std::string del) -{ - int start = 0; - int end = s.find(del); - - while (end != -1) { - // cout << s.substr(start, end - start) << endl; - start = end + del.size(); - end = s.find(del, start); - } - - return s.substr(start, end - start); -} - -} // namespace tachimawari::joint::utils diff --git a/src/tachimawari/node/tachimawari_node.cpp b/src/tachimawari/node/tachimawari_node.cpp index 1cf13ae..c08adb2 100644 --- a/src/tachimawari/node/tachimawari_node.cpp +++ b/src/tachimawari/node/tachimawari_node.cpp @@ -67,10 +67,10 @@ TachimawariNode::TachimawariNode( }); } -void TachimawariNode::run_joint_manager() +void TachimawariNode::run_joint_manager(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); From f07ebdaececaf9a089ab228419394ca8e48a21e2 Mon Sep 17 00:00:00 2001 From: borednuna Date: Sun, 26 May 2024 03:00:29 +0700 Subject: [PATCH 15/19] feat: tidy codes --- include/tachimawari/joint/tf2/frame.hpp | 2 +- include/tachimawari/joint/tf2/frame_id.hpp | 2 +- src/tachimawari/joint/node/joint_node.cpp | 16 +++++------ src/tachimawari/node/tachimawari_node.cpp | 31 +++++++++++----------- 4 files changed, 26 insertions(+), 25 deletions(-) diff --git a/include/tachimawari/joint/tf2/frame.hpp b/include/tachimawari/joint/tf2/frame.hpp index 7e5e900..685edbe 100644 --- a/include/tachimawari/joint/tf2/frame.hpp +++ b/include/tachimawari/joint/tf2/frame.hpp @@ -54,7 +54,7 @@ class Frame double translation_y; double translation_z; - double quaternion_x; // maybe change to keisan angle + double quaternion_x; double quaternion_y; double quaternion_z; double quaternion_w; diff --git a/include/tachimawari/joint/tf2/frame_id.hpp b/include/tachimawari/joint/tf2/frame_id.hpp index 43029b7..813c5e1 100644 --- a/include/tachimawari/joint/tf2/frame_id.hpp +++ b/include/tachimawari/joint/tf2/frame_id.hpp @@ -55,4 +55,4 @@ class FrameId } // namespace tachimawari::joint -#endif // TACHIMAWARI__JOINT_TF2__FRAME_ID_HPP_ \ No newline at end of file +#endif // TACHIMAWARI__JOINT_TF2__FRAME_ID_HPP_ diff --git a/src/tachimawari/joint/node/joint_node.cpp b/src/tachimawari/joint/node/joint_node.cpp index 516f694..cb7090f 100644 --- a/src/tachimawari/joint/node/joint_node.cpp +++ b/src/tachimawari/joint/node/joint_node.cpp @@ -34,17 +34,17 @@ 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 joint_manager, std::string path) @@ -74,7 +74,7 @@ JointNode::JointNode( std::vector 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 +87,7 @@ JointNode::JointNode( current_joints_publisher = node->create_publisher(current_joints_topic(), 10); } -void JointNode::update() { middleware.update(); } +void JointNode::update() {middleware.update();} void JointNode::publish_current_joints() { diff --git a/src/tachimawari/node/tachimawari_node.cpp b/src/tachimawari/node/tachimawari_node.cpp index c08adb2..5e162b0 100644 --- a/src/tachimawari/node/tachimawari_node.cpp +++ b/src/tachimawari/node/tachimawari_node.cpp @@ -44,26 +44,27 @@ TachimawariNode::TachimawariNode( imu_node(nullptr), control_node(nullptr) { - node_timer = node->create_wall_timer(8ms, [this]() { - if (this->control_manager) { - this->control_manager->add_default_bulk_read_packet(); - this->control_manager->send_bulk_read_packet(); + node_timer = node->create_wall_timer( + 8ms, [this]() { + if (this->control_manager) { + this->control_manager->add_default_bulk_read_packet(); + this->control_manager->send_bulk_read_packet(); - if (this->imu_node) { - this->imu_node->update(); - } + if (this->imu_node) { + this->imu_node->update(); + } - if (this->control_node) { - this->control_node->update(); - } + if (this->control_node) { + this->control_node->update(); + } - if (this->joint_node) { - this->joint_node->update(); + if (this->joint_node) { + this->joint_node->update(); - this->joint_node->publish_current_joints(); - this->joint_node->publish_frame_tree(); + this->joint_node->publish_current_joints(); + this->joint_node->publish_frame_tree(); + } } - } }); } From 212a9828a2c89601fa246a28486ef7025b04e2a5 Mon Sep 17 00:00:00 2001 From: borednuna Date: Wed, 29 May 2024 21:20:35 +0700 Subject: [PATCH 16/19] feat: remove unused codes, simplify enums, and const parameters --- include/tachimawari/joint/tf2/frame.hpp | 6 ++--- include/tachimawari/joint/tf2/frame_id.hpp | 22 +++++++++---------- include/tachimawari/joint/tf2/tf2_manager.hpp | 4 +--- include/tachimawari/node/tachimawari_node.hpp | 3 +-- src/tachimawari/joint/tf2/tf2_manager.cpp | 3 ++- src/tachimawari/node/tachimawari_node.cpp | 2 +- 6 files changed, 19 insertions(+), 21 deletions(-) diff --git a/include/tachimawari/joint/tf2/frame.hpp b/include/tachimawari/joint/tf2/frame.hpp index 685edbe..a1ab892 100644 --- a/include/tachimawari/joint/tf2/frame.hpp +++ b/include/tachimawari/joint/tf2/frame.hpp @@ -65,9 +65,9 @@ class Frame private: enum : uint8_t { - ROLL = 0, - PITCH = 1, - YAW = 2, + ROLL, + PITCH, + YAW, }; double get_joint_angle(uint8_t quaternion_axis, std::vector current_joints); diff --git a/include/tachimawari/joint/tf2/frame_id.hpp b/include/tachimawari/joint/tf2/frame_id.hpp index 813c5e1..67254e8 100644 --- a/include/tachimawari/joint/tf2/frame_id.hpp +++ b/include/tachimawari/joint/tf2/frame_id.hpp @@ -33,17 +33,17 @@ 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 + ODOM, + BASE_LINK, + TORSO, + HEAD, + GAZE, + RIGHT_THIGH, + RIGHT_CALF, + RIGHT_FOOT, + LEFT_THIGH, + LEFT_CALF, + LEFT_FOOT }; static const std::array frame_ids; diff --git a/include/tachimawari/joint/tf2/tf2_manager.hpp b/include/tachimawari/joint/tf2/tf2_manager.hpp index fbe346e..e6f3c44 100644 --- a/include/tachimawari/joint/tf2/tf2_manager.hpp +++ b/include/tachimawari/joint/tf2/tf2_manager.hpp @@ -42,13 +42,11 @@ class Tf2Manager public: explicit Tf2Manager(); - bool load_configuration(std::string path); + bool load_configuration(const std::string & path); void update(std::vector current_joints, keisan::Angle imu_yaw); std::vector get_frames() { return frames; } private: - static const std::vector> tf2_joint_pairs; - std::string config_path; std::vector frames; }; } // namespace tachimawari::joint diff --git a/include/tachimawari/node/tachimawari_node.hpp b/include/tachimawari/node/tachimawari_node.hpp index 814b625..e14af96 100644 --- a/include/tachimawari/node/tachimawari_node.hpp +++ b/include/tachimawari/node/tachimawari_node.hpp @@ -39,14 +39,13 @@ class TachimawariNode explicit TachimawariNode( rclcpp::Node::SharedPtr node, std::shared_ptr control_manager); - void run_joint_manager(std::string path); + void run_joint_manager(const 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_manager; diff --git a/src/tachimawari/joint/tf2/tf2_manager.cpp b/src/tachimawari/joint/tf2/tf2_manager.cpp index b15d402..d646952 100644 --- a/src/tachimawari/joint/tf2/tf2_manager.cpp +++ b/src/tachimawari/joint/tf2/tf2_manager.cpp @@ -32,7 +32,7 @@ namespace tachimawari::joint Tf2Manager::Tf2Manager() {} -bool Tf2Manager::load_configuration(std::string path) +bool Tf2Manager::load_configuration(const std::string & path) { std::string ss = path + "frame_measurements.json"; @@ -60,6 +60,7 @@ bool Tf2Manager::load_configuration(std::string path) 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; } } diff --git a/src/tachimawari/node/tachimawari_node.cpp b/src/tachimawari/node/tachimawari_node.cpp index 5e162b0..c3e3a1f 100644 --- a/src/tachimawari/node/tachimawari_node.cpp +++ b/src/tachimawari/node/tachimawari_node.cpp @@ -68,7 +68,7 @@ TachimawariNode::TachimawariNode( }); } -void TachimawariNode::run_joint_manager(std::string path) +void TachimawariNode::run_joint_manager(const std::string & path) { joint_node = std::make_shared( node, std::make_shared(control_manager), path); From 07e9af8f1686f9d35db750ca5167be9bdc41f351 Mon Sep 17 00:00:00 2001 From: borednuna Date: Fri, 7 Jun 2024 00:28:05 +0700 Subject: [PATCH 17/19] feat: use const for path variable --- include/tachimawari/joint/node/joint_node.hpp | 2 +- src/tachimawari/joint/node/joint_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/tachimawari/joint/node/joint_node.hpp b/include/tachimawari/joint/node/joint_node.hpp index 075fc51..c5cb6b5 100644 --- a/include/tachimawari/joint/node/joint_node.hpp +++ b/include/tachimawari/joint/node/joint_node.hpp @@ -59,7 +59,7 @@ class JointNode static std::string status_topic(); JointNode( - rclcpp::Node::SharedPtr node, std::shared_ptr joint_manager, std::string path); + rclcpp::Node::SharedPtr node, std::shared_ptr joint_manager, const std::string & path); keisan::Angle imu_yaw; void publish_current_joints(); diff --git a/src/tachimawari/joint/node/joint_node.cpp b/src/tachimawari/joint/node/joint_node.cpp index cb7090f..b14f4cb 100644 --- a/src/tachimawari/joint/node/joint_node.cpp +++ b/src/tachimawari/joint/node/joint_node.cpp @@ -47,7 +47,7 @@ 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, std::string path) + rclcpp::Node::SharedPtr node, std::shared_ptr joint_manager, const std::string & path) : joint_manager(joint_manager), middleware(), tf2_broadcaster(std::make_shared(node)), From 9df35fdd948a951a68a371aec3baf4d2024c0582 Mon Sep 17 00:00:00 2001 From: borednuna Date: Fri, 7 Jun 2024 02:50:37 +0700 Subject: [PATCH 18/19] feat: use const for path variable --- include/tachimawari/joint/tf2/frame.hpp | 8 ++++---- include/tachimawari/joint/tf2/tf2_manager.hpp | 2 +- src/tachimawari/joint/tf2/frame.cpp | 8 ++++---- src/tachimawari/joint/tf2/tf2_manager.cpp | 2 +- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/tachimawari/joint/tf2/frame.hpp b/include/tachimawari/joint/tf2/frame.hpp index a1ab892..da13ab3 100644 --- a/include/tachimawari/joint/tf2/frame.hpp +++ b/include/tachimawari/joint/tf2/frame.hpp @@ -44,9 +44,9 @@ class Frame const double translation_z, const double const_roll, const double const_pitch, const double const_yaw); - void update_quaternion(std::vector current_joints); - void update_quaternion(keisan::Angle imu_yaw); - geometry_msgs::msg::TransformStamped get_transform_stamped(rclcpp::Time time_stamp); + 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; @@ -70,7 +70,7 @@ class Frame YAW, }; - double get_joint_angle(uint8_t quaternion_axis, std::vector current_joints); + double get_joint_angle(const uint8_t & quaternion_axis, const std::vector & current_joints); }; } // namespace tachimawari::joint diff --git a/include/tachimawari/joint/tf2/tf2_manager.hpp b/include/tachimawari/joint/tf2/tf2_manager.hpp index e6f3c44..61e512b 100644 --- a/include/tachimawari/joint/tf2/tf2_manager.hpp +++ b/include/tachimawari/joint/tf2/tf2_manager.hpp @@ -43,7 +43,7 @@ class Tf2Manager explicit Tf2Manager(); bool load_configuration(const std::string & path); - void update(std::vector current_joints, keisan::Angle imu_yaw); + void update(const std::vector & current_joints, const keisan::Angle & imu_yaw); std::vector get_frames() { return frames; } private: diff --git a/src/tachimawari/joint/tf2/frame.cpp b/src/tachimawari/joint/tf2/frame.cpp index 3d50a60..27f1bc6 100644 --- a/src/tachimawari/joint/tf2/frame.cpp +++ b/src/tachimawari/joint/tf2/frame.cpp @@ -48,7 +48,7 @@ Frame::Frame( { } -geometry_msgs::msg::TransformStamped Frame::get_transform_stamped(rclcpp::Time time_stamp) +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; @@ -72,7 +72,7 @@ geometry_msgs::msg::TransformStamped Frame::get_transform_stamped(rclcpp::Time t return t; } -void Frame::update_quaternion(std::vector current_joints) +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; @@ -91,7 +91,7 @@ void Frame::update_quaternion(std::vector current_joints) quaternion_w = q.w(); } -void Frame::update_quaternion(keisan::Angle imu_yaw) +void Frame::update_quaternion(const keisan::Angle & imu_yaw) { tf2::Quaternion q; q.setRPY(0, 0, imu_yaw.radian()); @@ -102,7 +102,7 @@ void Frame::update_quaternion(keisan::Angle imu_yaw) quaternion_w = q.w(); } -double Frame::get_joint_angle(uint8_t quaternion_axis, std::vector current_joints) +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; diff --git a/src/tachimawari/joint/tf2/tf2_manager.cpp b/src/tachimawari/joint/tf2/tf2_manager.cpp index d646952..b51ab07 100644 --- a/src/tachimawari/joint/tf2/tf2_manager.cpp +++ b/src/tachimawari/joint/tf2/tf2_manager.cpp @@ -67,7 +67,7 @@ bool Tf2Manager::load_configuration(const std::string & path) return true; } -void Tf2Manager::update(std::vector current_joints, keisan::Angle imu_yaw) +void Tf2Manager::update(const std::vector & current_joints, const keisan::Angle & imu_yaw) { for (auto & item : frames) { if (item.id == FrameId::ODOM) { From acc7ef5225df2778f1fa8e3a5ceabdc87345f64a Mon Sep 17 00:00:00 2001 From: borednuna Date: Sat, 8 Jun 2024 00:56:31 +0700 Subject: [PATCH 19/19] feat: library order --- include/tachimawari/joint/node/joint_node.hpp | 3 +-- include/tachimawari/joint/tf2/frame.hpp | 2 +- src/tachimawari/joint/tf2/frame.cpp | 4 ++-- src/tachimawari/joint/tf2/frame_id.cpp | 3 +-- src/tachimawari/joint/tf2/tf2_manager.cpp | 4 ++-- src/tachimawari/node/tachimawari_node.cpp | 3 +-- 6 files changed, 8 insertions(+), 11 deletions(-) diff --git a/include/tachimawari/joint/node/joint_node.hpp b/include/tachimawari/joint/node/joint_node.hpp index c5cb6b5..c883e03 100644 --- a/include/tachimawari/joint/node/joint_node.hpp +++ b/include/tachimawari/joint/node/joint_node.hpp @@ -21,8 +21,6 @@ #ifndef TACHIMAWARI__JOINT__NODE__JOINT_NODE_HPP_ #define TACHIMAWARI__JOINT__NODE__JOINT_NODE_HPP_ -#include - #include #include @@ -37,6 +35,7 @@ #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 diff --git a/include/tachimawari/joint/tf2/frame.hpp b/include/tachimawari/joint/tf2/frame.hpp index da13ab3..36ab993 100644 --- a/include/tachimawari/joint/tf2/frame.hpp +++ b/include/tachimawari/joint/tf2/frame.hpp @@ -22,12 +22,12 @@ #define TACHIMAWARI__JOINT__TF2__FRAME_HPP_ #include -#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" diff --git a/src/tachimawari/joint/tf2/frame.cpp b/src/tachimawari/joint/tf2/frame.cpp index 27f1bc6..1111331 100644 --- a/src/tachimawari/joint/tf2/frame.cpp +++ b/src/tachimawari/joint/tf2/frame.cpp @@ -18,8 +18,6 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include "tachimawari/joint/tf2/frame.hpp" - #include #include #include @@ -27,6 +25,8 @@ #include #include +#include "tachimawari/joint/tf2/frame.hpp" + namespace tachimawari::joint { diff --git a/src/tachimawari/joint/tf2/frame_id.cpp b/src/tachimawari/joint/tf2/frame_id.cpp index 2e37930..586946f 100644 --- a/src/tachimawari/joint/tf2/frame_id.cpp +++ b/src/tachimawari/joint/tf2/frame_id.cpp @@ -18,13 +18,12 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. -#include "tachimawari/joint/tf2/frame_id.hpp" - #include #include #include #include "tachimawari/joint/model/joint_id.hpp" +#include "tachimawari/joint/tf2/frame_id.hpp" namespace tachimawari::joint { diff --git a/src/tachimawari/joint/tf2/tf2_manager.cpp b/src/tachimawari/joint/tf2/tf2_manager.cpp index b51ab07..6888f25 100644 --- a/src/tachimawari/joint/tf2/tf2_manager.cpp +++ b/src/tachimawari/joint/tf2/tf2_manager.cpp @@ -18,8 +18,6 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include "tachimawari/joint/tf2/tf2_manager.hpp" - #include #include #include @@ -27,6 +25,8 @@ #include #include +#include "tachimawari/joint/tf2/tf2_manager.hpp" + namespace tachimawari::joint { diff --git a/src/tachimawari/node/tachimawari_node.cpp b/src/tachimawari/node/tachimawari_node.cpp index c3e3a1f..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;