Skip to content

Commit

Permalink
Fix compile warnings
Browse files Browse the repository at this point in the history
  • Loading branch information
jaagut committed Jan 31, 2025
1 parent 4640f9e commit 2427bbc
Show file tree
Hide file tree
Showing 14 changed files with 23 additions and 23 deletions.
8 changes: 4 additions & 4 deletions bitbots_motion/bitbots_dynamic_kick/src/stabilizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,8 @@ Stabilizer::Stabilizer(std::string ns) {

pid_trunk_fused_pitch_ = std::make_shared<control_toolbox::PidROS>(pitch_node_, "");
pid_trunk_fused_roll_ = std::make_shared<control_toolbox::PidROS>(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();
}
Expand All @@ -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;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,13 +1,13 @@
#ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_ENGINE_H_
#define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_ENGINE_H_

#include <bitbots_dynup/dynup_parameters.hpp>
#include <bitbots_dynup/msg/dynup_engine_debug.hpp>
#include <bitbots_splines/abstract_engine.hpp>
#include <bitbots_splines/pose_spline.hpp>
#include <bitbots_splines/smooth_spline.hpp>
#include <bitbots_splines/spline_container.hpp>
#include <cmath>
#include <dynup_parameters.hpp>
#include <optional>
#include <rclcpp/rclcpp.hpp>
#include <string>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@
#define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_IK_H_

#include <bio_ik/bio_ik.hpp>
#include <bitbots_dynup/dynup_parameters.hpp>
#include <bitbots_splines/abstract_ik.hpp>
#include <dynup_parameters.hpp>
#include <moveit/robot_model_loader/robot_model_loader.hpp>
#include <moveit/robot_state/robot_state.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,14 @@

#include <bitbots_dynup/dynup_engine.hpp>
#include <bitbots_dynup/dynup_ik.hpp>
#include <bitbots_dynup/dynup_parameters.hpp>
#include <bitbots_dynup/dynup_stabilizer.hpp>
#include <bitbots_dynup/msg/dynup_poses.hpp>
#include <bitbots_dynup/visualizer.hpp>
#include <bitbots_msgs/action/dynup.hpp>
#include <bitbots_msgs/msg/joint_command.hpp>
#include <bitbots_utils/utils.hpp>
#include <cmath>
#include <dynup_parameters.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,9 @@
#include <rot_conv/rot_conv.h>

#include <Eigen/Geometry>
#include <bitbots_dynup/dynup_parameters.hpp>
#include <bitbots_splines/abstract_stabilizer.hpp>
#include <control_toolbox/pid_ros.hpp>
#include <dynup_parameters.hpp>
#include <optional>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,13 +1,13 @@
#ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_VISUALIZER_H_
#define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_VISUALIZER_H_

#include <bitbots_dynup/dynup_parameters.hpp>
#include <bitbots_dynup/dynup_utils.hpp>
#include <bitbots_dynup/msg/dynup_ik_offset.hpp>
#include <bitbots_splines/abstract_ik.hpp>
#include <bitbots_splines/abstract_visualizer.hpp>
#include <bitbots_splines/smooth_spline.hpp>
#include <bitbots_splines/spline_container.hpp>
#include <dynup_parameters.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit/robot_model/robot_model.hpp>
#include <moveit/robot_state/robot_state.hpp>
Expand Down
8 changes: 4 additions & 4 deletions bitbots_motion/bitbots_dynup/src/dynup_stabilizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand Down Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion bitbots_motion/bitbots_head_mover/src/move_head.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,14 +3,14 @@

#include <bio_ik/bio_ik.hpp>
#include <bio_ik_msgs/msg/ik_response.hpp>
#include <bitbots_head_mover/head_parameters.hpp>
#include <bitbots_msgs/action/look_at.hpp>
#include <bitbots_msgs/msg/head_mode.hpp>
#include <bitbots_msgs/msg/joint_command.hpp>
#include <chrono>
#include <cmath>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <head_parameters.hpp>
#include <iostream>
#include <memory>
#include <moveit/planning_scene/planning_scene.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 <bitbots_quintic_walk/bitbots_quintic_walk_parameters.hpp>
#include <bitbots_quintic_walk/walk_utils.hpp>
#include <bitbots_quintic_walk_parameters.hpp>
#include <bitbots_splines/abstract_engine.hpp>
#include <bitbots_splines/pose_spline.hpp>
#include <bitbots_splines/smooth_spline.hpp>
Expand Down
Original file line number Diff line number Diff line change
@@ -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 <bitbots_quintic_walk/bitbots_quintic_walk_parameters.hpp>
#include <bitbots_quintic_walk/walk_utils.hpp>
#include <bitbots_quintic_walk_parameters.hpp>
#include <bitbots_splines/abstract_ik.hpp>
#include <moveit/robot_state/robot_state.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,11 @@ The original files can be found at:
#include <bitbots_msgs/msg/foot_pressure.hpp>
#include <bitbots_msgs/msg/joint_command.hpp>
#include <bitbots_msgs/msg/robot_control_state.hpp>
#include <bitbots_quintic_walk/bitbots_quintic_walk_parameters.hpp>
#include <bitbots_quintic_walk/walk_engine.hpp>
#include <bitbots_quintic_walk/walk_ik.hpp>
#include <bitbots_quintic_walk/walk_stabilizer.hpp>
#include <bitbots_quintic_walk/walk_visualizer.hpp>
#include <bitbots_quintic_walk_parameters.hpp>
#include <bitbots_splines/abstract_ik.hpp>
#include <chrono>
#include <control_toolbox/pid_ros.hpp>
Expand Down
8 changes: 4 additions & 4 deletions bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand All @@ -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};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,9 @@
#include <tf2_ros/buffer.h>

#include <bitbots_localization/RobotState.hpp>
#include <bitbots_localization/localization_parameters.hpp>
#include <bitbots_localization/map.hpp>
#include <bitbots_localization/tools.hpp>
#include <localization_parameters.hpp>
#include <particle_filter/ParticleFilter.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include <bitbots_localization/Resampling.hpp>
#include <bitbots_localization/RobotState.hpp>
#include <bitbots_localization/StateDistribution.hpp>
#include <bitbots_localization/localization_parameters.hpp>
#include <bitbots_localization/map.hpp>
#include <bitbots_localization/srv/reset_filter.hpp>
#include <bitbots_localization/srv/set_paused.hpp>
Expand All @@ -35,7 +36,6 @@
#include <geometry_msgs/msg/twist.hpp>
#include <image_transport/image_transport.hpp>
#include <iterator>
#include <localization_parameters.hpp>
#include <memory>
#include <particle_filter/CRandomNumberGenerator.hpp>
#include <particle_filter/ParticleFilter.hpp>
Expand Down

0 comments on commit 2427bbc

Please sign in to comment.