-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #61 from ichiro-its/feature/create-tf2
[Sprint 22/23 / PI-412] - [Feature] Implement TF2 Tree
- Loading branch information
Showing
12 changed files
with
531 additions
and
11 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,77 @@ | ||
// Copyright (c) 2021-2023 Ichiro ITS | ||
// | ||
// Permission is hereby granted, free of charge, to any person obtaining a copy | ||
// of this software and associated documentation files (the "Software"), to deal | ||
// in the Software without restriction, including without limitation the rights | ||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
// copies of the Software, and to permit persons to whom the Software is | ||
// furnished to do so, subject to the following conditions: | ||
// | ||
// The above copyright notice and this permission notice shall be included in all | ||
// copies or substantial portions of the Software. | ||
// | ||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
// SOFTWARE. | ||
|
||
#ifndef TACHIMAWARI__JOINT__TF2__FRAME_HPP_ | ||
#define TACHIMAWARI__JOINT__TF2__FRAME_HPP_ | ||
|
||
#include <memory> | ||
#include <string> | ||
#include <vector> | ||
|
||
#include "geometry_msgs/msg/transform_stamped.hpp" | ||
#include "keisan/angle/angle.hpp" | ||
#include "nlohmann/json.hpp" | ||
#include "rclcpp/time.hpp" | ||
#include "tachimawari/joint/node/joint_manager.hpp" | ||
#include "tachimawari/joint/tf2/frame_id.hpp" | ||
#include "tf2/LinearMath/Quaternion.h" | ||
|
||
namespace tachimawari::joint | ||
{ | ||
|
||
class Frame | ||
{ | ||
public: | ||
Frame( | ||
const uint8_t id, const double translation_x, const double translation_y, | ||
const double translation_z, const double const_roll, const double const_pitch, | ||
const double const_yaw); | ||
|
||
void update_quaternion(const std::vector<Joint> & current_joints); | ||
void update_quaternion(const keisan::Angle<double> & imu_yaw); | ||
geometry_msgs::msg::TransformStamped get_transform_stamped(const rclcpp::Time & time_stamp); | ||
|
||
uint8_t id; | ||
|
||
double translation_x; | ||
double translation_y; | ||
double translation_z; | ||
|
||
double quaternion_x; | ||
double quaternion_y; | ||
double quaternion_z; | ||
double quaternion_w; | ||
|
||
double const_roll; | ||
double const_pitch; | ||
double const_yaw; | ||
|
||
private: | ||
enum : uint8_t { | ||
ROLL, | ||
PITCH, | ||
YAW, | ||
}; | ||
|
||
double get_joint_angle(const uint8_t & quaternion_axis, const std::vector<Joint> & current_joints); | ||
}; | ||
} // namespace tachimawari::joint | ||
|
||
#endif // TACHIMAWARI__JOINT__TF2__FRAME_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,58 @@ | ||
// Copyright (c) 2021-2023 Ichiro ITS | ||
// | ||
// Permission is hereby granted, free of charge, to any person obtaining a copy | ||
// of this software and associated documentation files (the "Software"), to deal | ||
// in the Software without restriction, including without limitation the rights | ||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
// copies of the Software, and to permit persons to whom the Software is | ||
// furnished to do so, subject to the following conditions: | ||
// | ||
// The above copyright notice and this permission notice shall be included in all | ||
// copies or substantial portions of the Software. | ||
// | ||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
// SOFTWARE. | ||
|
||
#ifndef TACHIMAWARI__JOINT_TF2__FRAME_ID_HPP_ | ||
#define TACHIMAWARI__JOINT_TF2__FRAME_ID_HPP_ | ||
|
||
#include <array> | ||
#include <map> | ||
#include <string> | ||
#include <vector> | ||
|
||
namespace tachimawari::joint | ||
{ | ||
|
||
class FrameId | ||
{ | ||
public: | ||
enum : uint8_t { | ||
ODOM, | ||
BASE_LINK, | ||
TORSO, | ||
HEAD, | ||
GAZE, | ||
RIGHT_THIGH, | ||
RIGHT_CALF, | ||
RIGHT_FOOT, | ||
LEFT_THIGH, | ||
LEFT_CALF, | ||
LEFT_FOOT | ||
}; | ||
|
||
static const std::array<uint8_t, 10> frame_ids; | ||
static const std::map<uint8_t, std::string> frame_id_string; | ||
static const std::map<std::string, uint8_t> frame_string_id; | ||
static const std::map<uint8_t, std::vector<uint8_t>> frame_joint_map; | ||
static const std::map<uint8_t, std::string> parent_frame; | ||
}; | ||
|
||
} // namespace tachimawari::joint | ||
|
||
#endif // TACHIMAWARI__JOINT_TF2__FRAME_ID_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,54 @@ | ||
// Copyright (c) 2021-2023 Ichiro ITS | ||
// | ||
// Permission is hereby granted, free of charge, to any person obtaining a copy | ||
// of this software and associated documentation files (the "Software"), to deal | ||
// in the Software without restriction, including without limitation the rights | ||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
// copies of the Software, and to permit persons to whom the Software is | ||
// furnished to do so, subject to the following conditions: | ||
// | ||
// The above copyright notice and this permission notice shall be included in all | ||
// copies or substantial portions of the Software. | ||
// | ||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
// SOFTWARE. | ||
|
||
#ifndef TACHIMAWARI__JOINT__TF2__TF2_MANAGER_HPP_ | ||
#define TACHIMAWARI__JOINT__TF2__TF2_MANAGER_HPP_ | ||
|
||
#include <memory> | ||
#include <string> | ||
#include <vector> | ||
|
||
#include "keisan/angle/angle.hpp" | ||
#include "rclcpp/time.hpp" | ||
#include "tachimawari/joint/model/joint.hpp" | ||
#include "tachimawari/joint/model/joint_id.hpp" | ||
#include "tachimawari/joint/node/joint_manager.hpp" | ||
#include "tachimawari/joint/tf2/frame.hpp" | ||
#include "tachimawari/joint/tf2/frame_id.hpp" | ||
#include "tf2_ros/transform_broadcaster.h" | ||
|
||
namespace tachimawari::joint | ||
{ | ||
|
||
class Tf2Manager | ||
{ | ||
public: | ||
explicit Tf2Manager(); | ||
|
||
bool load_configuration(const std::string & path); | ||
void update(const std::vector<Joint> & current_joints, const keisan::Angle<double> & imu_yaw); | ||
std::vector<Frame> get_frames() { return frames; } | ||
|
||
private: | ||
std::vector<Frame> frames; | ||
}; | ||
} // namespace tachimawari::joint | ||
|
||
#endif // TACHIMAWARI__JOINT__TF2__TF2_MANAGER_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.