diff --git a/bitbots_motion/bitbots_dynamic_kick/src/stabilizer.cpp b/bitbots_motion/bitbots_dynamic_kick/src/stabilizer.cpp index 84320bfaf..d74688364 100644 --- a/bitbots_motion/bitbots_dynamic_kick/src/stabilizer.cpp +++ b/bitbots_motion/bitbots_dynamic_kick/src/stabilizer.cpp @@ -23,8 +23,8 @@ Stabilizer::Stabilizer(std::string ns) { pid_trunk_fused_pitch_ = std::make_shared(pitch_node_, ""); pid_trunk_fused_roll_ = std::make_shared(roll_node_, ""); - pid_trunk_fused_pitch_->initPid(); - pid_trunk_fused_roll_->initPid(); + pid_trunk_fused_pitch_->initialize_from_ros_parameters(); + pid_trunk_fused_roll_->initialize_from_ros_parameters(); reset(); } @@ -51,8 +51,8 @@ KickPositions Stabilizer::stabilize(const KickPositions &positions, const rclcpp cop_x_error = cop_x - positions.trunk_pose.translation().x(); cop_y_error = cop_y - positions.trunk_pose.translation().y(); - double x_correction = pid_trunk_fused_roll_->computeCommand(cop_x_error, dt); - double y_correction = pid_trunk_fused_pitch_->computeCommand(cop_y_error, dt); + double x_correction = pid_trunk_fused_roll_->compute_command(cop_x_error, dt); + double y_correction = pid_trunk_fused_pitch_->compute_command(cop_y_error, dt); stabilized_positions.trunk_pose.translation().x() += x_correction; stabilized_positions.trunk_pose.translation().y() += y_correction; diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp index a5a67e9af..5a04b90d7 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp @@ -1,13 +1,13 @@ #ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_ENGINE_H_ #define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_ENGINE_H_ +#include #include #include #include #include #include #include -#include #include #include #include diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp index 745c496dc..f14e95c86 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp @@ -2,8 +2,8 @@ #define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_IK_H_ #include +#include #include -#include #include #include #include diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp index ae214c2ec..028b35f43 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -13,7 +14,6 @@ #include #include #include -#include #include #include #include diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_stabilizer.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_stabilizer.hpp index 684474572..d4d2b861f 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_stabilizer.hpp +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_stabilizer.hpp @@ -4,9 +4,9 @@ #include #include +#include #include #include -#include #include #include #include diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.hpp index 571757923..35a8195dd 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.hpp +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.hpp @@ -1,13 +1,13 @@ #ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_VISUALIZER_H_ #define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_VISUALIZER_H_ +#include #include #include #include #include #include #include -#include #include #include #include diff --git a/bitbots_motion/bitbots_dynup/src/dynup_stabilizer.cpp b/bitbots_motion/bitbots_dynup/src/dynup_stabilizer.cpp index a44f34d15..92cb29805 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_stabilizer.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_stabilizer.cpp @@ -6,8 +6,8 @@ Stabilizer::Stabilizer(rclcpp::Node::SharedPtr node, bitbots_dynup::Params::Stab : params_(params), pid_trunk_pitch_(node, "stabilizer.trunk_pid.pitch"), pid_trunk_roll_(node, "stabilizer.trunk_pid.roll") { - pid_trunk_pitch_.initPid(); - pid_trunk_roll_.initPid(); + pid_trunk_pitch_.initialize_from_ros_parameters(); + pid_trunk_roll_.initialize_from_ros_parameters(); reset(); } @@ -43,8 +43,8 @@ DynupResponse Stabilizer::stabilize(const DynupResponse &ik_goals, const rclcpp: // Adapt trunk based on PID controller goal_fused.fusedPitch += - pid_trunk_pitch_.computeCommand(goal_fused.fusedPitch - current_orientation.fusedPitch, dt); - goal_fused.fusedRoll += pid_trunk_roll_.computeCommand(goal_fused.fusedRoll - current_orientation.fusedRoll, dt); + pid_trunk_pitch_.compute_command(goal_fused.fusedPitch - current_orientation.fusedPitch, dt); + goal_fused.fusedRoll += pid_trunk_roll_.compute_command(goal_fused.fusedRoll - current_orientation.fusedRoll, dt); // Check if the trunk is stable, meaning it isn't leaning too much // TODO it would be better to use the rotational velocity of the imu to determine stability diff --git a/bitbots_motion/bitbots_head_mover/src/move_head.cpp b/bitbots_motion/bitbots_head_mover/src/move_head.cpp index 47f24d05c..01758e2eb 100644 --- a/bitbots_motion/bitbots_head_mover/src/move_head.cpp +++ b/bitbots_motion/bitbots_head_mover/src/move_head.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -10,7 +11,6 @@ #include #include #include -#include #include #include #include diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp index afc46b373..5b9cf96cd 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp @@ -6,8 +6,8 @@ The original files can be found at: #ifndef BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_ENGINE_H_ #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_ENGINE_H_ +#include #include -#include #include #include #include diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp index c71d492e9..20da32c54 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp @@ -1,7 +1,7 @@ #ifndef BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ +#include #include -#include #include #include #include diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.hpp b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.hpp index 02945b294..9b847ba28 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.hpp +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.hpp @@ -14,11 +14,11 @@ The original files can be found at: #include #include #include +#include #include #include #include #include -#include #include #include #include diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp index 7a54551a5..abf539ae1 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp @@ -4,8 +4,8 @@ namespace bitbots_quintic_walk { WalkStabilizer::WalkStabilizer(rclcpp::Node::SharedPtr node) : pid_trunk_fused_pitch_(node, "node.trunk_pid.pitch"), pid_trunk_fused_roll_(node, "node.trunk_pid.roll") { - pid_trunk_fused_pitch_.initPid(); - pid_trunk_fused_roll_.initPid(); + pid_trunk_fused_pitch_.initialize_from_ros_parameters(); + pid_trunk_fused_roll_.initialize_from_ros_parameters(); reset(); } @@ -29,9 +29,9 @@ WalkResponse WalkStabilizer::stabilize(const WalkResponse& response, const rclcp // adapt trunk values based on PID controllers double fused_roll_correction = - pid_trunk_fused_roll_.computeCommand(goal_fused.fusedRoll - response.current_fused_roll, dt); + pid_trunk_fused_roll_.compute_command(goal_fused.fusedRoll - response.current_fused_roll, dt); double fused_pitch_correction = - pid_trunk_fused_pitch_.computeCommand(goal_fused.fusedPitch - response.current_fused_pitch, dt); + pid_trunk_fused_pitch_.compute_command(goal_fused.fusedPitch - response.current_fused_pitch, dt); // Change trunk x offset (in the trunks frame of reference) based on the PID controllers WalkResponse stabilized_response{response}; diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp index 30ecaf993..f6c53a97b 100644 --- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp +++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp @@ -8,9 +8,9 @@ #include #include +#include #include #include -#include #include #include #include diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp index 95f3521a1..b0309060b 100644 --- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp +++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -35,7 +36,6 @@ #include #include #include -#include #include #include #include