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) {