diff --git a/bitbots_odometry/CMakeLists.txt b/bitbots_odometry/CMakeLists.txt index db10c2cb..4bd17893 100644 --- a/bitbots_odometry/CMakeLists.txt +++ b/bitbots_odometry/CMakeLists.txt @@ -6,20 +6,26 @@ if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif () -find_package(tf2_eigen REQUIRED) -find_package(tf2 REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(message_filters REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(ament_cmake REQUIRED) find_package(biped_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(bitbots_docs REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) find_package(rot_conv REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(Eigen3 REQUIRED) + +generate_parameter_library( + odometry_parameters + config/odometry_config_template.yaml +) include_directories(include) @@ -28,37 +34,48 @@ add_compile_options(-Wall -Werror -Wno-unused) add_executable(odometry_fuser src/odometry_fuser.cpp) add_executable(motion_odometry src/motion_odometry.cpp) +target_link_libraries( + motion_odometry + rclcpp::rclcpp + odometry_parameters +) + ## Specify libraries to link a library or executable target against -ament_target_dependencies(motion_odometry - tf2_eigen - tf2 - nav_msgs - message_filters - tf2_geometry_msgs - biped_interfaces - geometry_msgs - bitbots_docs - rot_conv - sensor_msgs - tf2_ros - ament_cmake - rclcpp) +ament_target_dependencies( + motion_odometry + tf2_eigen + tf2 + nav_msgs + message_filters + tf2_geometry_msgs + biped_interfaces + geometry_msgs + bitbots_docs + rot_conv + sensor_msgs + tf2_ros + ament_cmake + generate_parameter_library + rclcpp +) -ament_target_dependencies(odometry_fuser - tf2_eigen - tf2 - nav_msgs - message_filters - tf2_geometry_msgs - biped_interfaces - geometry_msgs - bitbots_docs - rot_conv - sensor_msgs - tf2_ros - ament_cmake - rclcpp - Eigen3) +ament_target_dependencies( + odometry_fuser + tf2_eigen + tf2 + nav_msgs + message_filters + tf2_geometry_msgs + biped_interfaces + geometry_msgs + bitbots_docs + rot_conv + sensor_msgs + tf2_ros + ament_cmake + rclcpp + Eigen3 +) enable_bitbots_docs() @@ -71,5 +88,4 @@ install(TARGETS odometry_fuser install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) - ament_package() diff --git a/bitbots_odometry/config/odometry_config_amy.yaml b/bitbots_odometry/config/odometry_config_amy.yaml new file mode 100644 index 00000000..55021f9a --- /dev/null +++ b/bitbots_odometry/config/odometry_config_amy.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 0.6 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_donna.yaml b/bitbots_odometry/config/odometry_config_donna.yaml new file mode 100644 index 00000000..55021f9a --- /dev/null +++ b/bitbots_odometry/config/odometry_config_donna.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 0.6 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_jack.yaml b/bitbots_odometry/config/odometry_config_jack.yaml new file mode 100644 index 00000000..55021f9a --- /dev/null +++ b/bitbots_odometry/config/odometry_config_jack.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 0.6 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_melody.yaml b/bitbots_odometry/config/odometry_config_melody.yaml new file mode 100644 index 00000000..55021f9a --- /dev/null +++ b/bitbots_odometry/config/odometry_config_melody.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 0.6 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_rory.yaml b/bitbots_odometry/config/odometry_config_rory.yaml new file mode 100644 index 00000000..55021f9a --- /dev/null +++ b/bitbots_odometry/config/odometry_config_rory.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 0.6 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_template.yaml b/bitbots_odometry/config/odometry_config_template.yaml new file mode 100644 index 00000000..03d46145 --- /dev/null +++ b/bitbots_odometry/config/odometry_config_template.yaml @@ -0,0 +1,42 @@ +motion_odometry: + x_forward_scaling: { + type: double, + default_value: 1.25, + description: "Scaling factor in forward x direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + x_backward_scaling: { + type: double, + default_value: 0.95, + description: "Scaling factor in backward x direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + y_scaling: { + type: double, + default_value: 1.0, + description: "Scaling factor in y direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + yaw_scaling: { + type: double, + default_value: 0.6, + description: "Scaling factor for rotation in yaw direction", + validation: { + bounds<>: [0.2, 2.5] + } + } + + publish_walk_odom_tf: { + type: bool, + default_value: false, + description: "Should odom tf be published" + } diff --git a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h index 7b44cc1e..2456dfd3 100644 --- a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h +++ b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h @@ -10,9 +10,12 @@ #include #include #include +#include "odometry_parameters.hpp" using std::placeholders::_1; +namespace bitbots_odometry { + class MotionOdometry : public rclcpp::Node { public: MotionOdometry(); @@ -33,11 +36,10 @@ class MotionOdometry : public rclcpp::Node { rclcpp::Subscription::SharedPtr joint_state_sub_; rclcpp::Subscription::SharedPtr odom_subscriber_; - double x_forward_scaling_; - double x_backward_scaling_; - double y_scaling_; - double yaw_scaling_; - bool publish_walk_odom_tf_; + // Declare parameter listener and struct from the generate_parameter_library + motion_odometry::ParamListener param_listener_; + // Datastructure to hold all parameters, which is build from the schema in the 'parameters.yaml' + motion_odometry::Params config_; void supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg); void jointStateCb(const sensor_msgs::msg::JointState::SharedPtr msg); @@ -50,3 +52,5 @@ class MotionOdometry : public rclcpp::Node { std::string current_support_link_; rclcpp::Time start_time_; }; + +} diff --git a/bitbots_odometry/launch/odometry.launch b/bitbots_odometry/launch/odometry.launch index b1d5d95c..545c0731 100644 --- a/bitbots_odometry/launch/odometry.launch +++ b/bitbots_odometry/launch/odometry.launch @@ -6,16 +6,12 @@ + - - - - - diff --git a/bitbots_odometry/package.xml b/bitbots_odometry/package.xml index 8d09bd8d..3a93cbe7 100644 --- a/bitbots_odometry/package.xml +++ b/bitbots_odometry/package.xml @@ -23,6 +23,7 @@ rot_conv bitbots_docs biped_interfaces + generate_parameter_library diff --git a/bitbots_odometry/src/motion_odometry.cpp b/bitbots_odometry/src/motion_odometry.cpp index b28efc02..9c469bab 100644 --- a/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_odometry/src/motion_odometry.cpp @@ -1,12 +1,15 @@ #include -MotionOdometry::MotionOdometry() - : Node("MotionOdometry"), - tf_buffer_(std::make_unique(this->get_clock())), - br_(std::make_unique(this)), - tf_listener_(std::make_shared(*tf_buffer_, this)) { - this->declare_parameter("publish_walk_odom_tf", false); - this->get_parameter("publish_walk_odom_tf", publish_walk_odom_tf_); +namespace bitbots_odometry { + +MotionOdometry::MotionOdometry() : Node("MotionOdometry"), + param_listener_(get_node_parameters_interface()) { + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_, this); + br_ = std::make_unique(this); + config_ = param_listener_.get_params(); + this->declare_parameter("base_link_frame", "base_link"); this->get_parameter("base_link_frame", base_link_frame_); this->declare_parameter("r_sole_frame", "r_sole"); @@ -15,27 +18,20 @@ MotionOdometry::MotionOdometry() this->get_parameter("l_sole_frame", l_sole_frame_); this->declare_parameter("odom_frame", "odom"); this->get_parameter("odom_frame", odom_frame_); - this->declare_parameter("x_forward_scaling", 1.0); - this->get_parameter("x_forward_scaling", x_forward_scaling_); - this->declare_parameter("x_backward_scaling", 1.0); - this->get_parameter("x_backward_scaling", x_backward_scaling_); - this->declare_parameter("y_scaling", 1.0); - this->get_parameter("y_scaling", y_scaling_); - this->declare_parameter("yaw_scaling", 1.0); - this->get_parameter("yaw_scaling", yaw_scaling_); - joint_update_time_= rclcpp::Time(0, 0, RCL_ROS_TIME); + + joint_update_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); current_support_state_ = -1; previous_support_state_ = -1; walk_support_state_sub_ = this->create_subscription("walk_support_state", - 1, - std::bind(&MotionOdometry::supportCallback, - this, _1)); + 1, + std::bind(&MotionOdometry::supportCallback, + this, _1)); kick_support_state_sub_ = this->create_subscription("dynamic_kick_support_state", - 1, - std::bind(&MotionOdometry::supportCallback, - this, _1)); + 1, + std::bind(&MotionOdometry::supportCallback, + this, _1)); joint_state_sub_ = this->create_subscription("joint_states", 1, @@ -57,12 +53,13 @@ MotionOdometry::MotionOdometry() void MotionOdometry::loop() { rclcpp::Time cycle_start_time = this->now(); + config_ = param_listener_.get_params(); //check if joint states were received, otherwise we can't provide odometry rclcpp::Duration joints_delta_t = this->now() - joint_update_time_; if (joints_delta_t.seconds() > 0.1) { // only warn if we did not just start as this results in unecessary warnings - if ((this->now() - start_time_).seconds() > 10){ + if ((this->now() - start_time_).seconds() > 10) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 30000, "No joint states received. Will not provide odometry."); } @@ -73,9 +70,9 @@ void MotionOdometry::loop() { // but since we skip the double support phase, we basically take the timepoint when the double support phase is // finished. This means both feet did not move and this should create no offset. if ((current_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE - && previous_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE) || + && previous_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE) || (current_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE - && previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE)) { + && previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE)) { foot_change_time_ = current_support_state_time_; if (previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE) { previous_support_link_ = l_sole_frame_; @@ -99,12 +96,12 @@ void MotionOdometry::loop() { // scale odometry based on parameters double x = previous_to_current_support.getOrigin().x(); if (x > 0) { - x = x * x_forward_scaling_; + x = x * config_.x_forward_scaling; } else { - x = x * x_backward_scaling_; + x = x * config_.x_backward_scaling; } - double y = previous_to_current_support.getOrigin().y() * y_scaling_; - double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * yaw_scaling_; + double y = previous_to_current_support.getOrigin().y() * config_.y_scaling; + double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * config_.yaw_scaling; previous_to_current_support.setOrigin({x, y, 0}); tf2::Quaternion q; q.setRPY(0, 0, yaw); @@ -134,12 +131,12 @@ void MotionOdometry::loop() { tf2::fromMsg(current_support_to_base_msg.transform, current_support_to_base); double x = current_support_to_base.getOrigin().x(); if (current_odom_msg_.twist.twist.linear.x > 0) { - x = x * x_forward_scaling_; - }else{ - x = x * x_backward_scaling_; + x = x * config_.x_forward_scaling; + } else { + x = x * config_.x_backward_scaling; } - double y = current_support_to_base.getOrigin().y() * y_scaling_; - double yaw = tf2::getYaw(current_support_to_base.getRotation()) * yaw_scaling_; + double y = current_support_to_base.getOrigin().y() * config_.y_scaling; + double yaw = tf2::getYaw(current_support_to_base.getRotation()) * config_.yaw_scaling; current_support_to_base.setOrigin({x, y, current_support_to_base.getOrigin().z()}); tf2::Quaternion q; q.setRPY(0, 0, yaw); @@ -151,7 +148,7 @@ void MotionOdometry::loop() { odom_to_base_link_msg.header.stamp = current_support_to_base_msg.header.stamp; odom_to_base_link_msg.header.frame_id = odom_frame_; odom_to_base_link_msg.child_frame_id = base_link_frame_; - if (publish_walk_odom_tf_) { + if (config_.publish_walk_odom_tf) { RCLCPP_WARN_ONCE(this->get_logger(), "Sending Tf from walk odometry directly"); br_->sendTransform(odom_to_base_link_msg); } @@ -217,9 +214,11 @@ void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) current_odom_msg_ = *msg; } +} + int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::Duration timer_duration = rclcpp::Duration::from_seconds(1.0 / 200.0); rclcpp::TimerBase::SharedPtr timer = rclcpp::create_timer(node, node->get_clock(), timer_duration, [node]() -> void {node->loop();}); diff --git a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h index f9bdca41..17f4c035 100644 --- a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h +++ b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h @@ -148,6 +148,11 @@ class WalkNode : public rclcpp::Node { double last_ros_update_time_; + double pitch_error_avg_ = 0; + double pitch_error_derivative_ = 0; + double pitch_error_integral_ = 0; + std::vector> pitch_error_buffer_; + int robot_state_; int current_support_foot_; diff --git a/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_quintic_walk/src/walk_node.cpp index 8b27651b..f306f4d5 100644 --- a/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_quintic_walk/src/walk_node.cpp @@ -229,11 +229,10 @@ void WalkNode::publish_debug(){ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) { WalkRequest request(current_request_); - // update walk engine response - if (got_new_goals_) { - got_new_goals_ = false; - walk_engine_.setGoals(request); - } + request.linear_orders[0] += pitch_error_avg_ * 0.16 + pitch_error_derivative_ * 5; + + walk_engine_.setGoals(request); + checkPhaseRestAndReset(); current_response_ = walk_engine_.update(dt); @@ -471,10 +470,45 @@ void WalkNode::imuCb(const sensor_msgs::msg::Imu::SharedPtr msg) { current_trunk_fused_pitch_ = imu_fused.fusedPitch; current_trunk_fused_roll_ = imu_fused.fusedRoll; - // get angular velocities + // Get angular velocities roll_vel_ = msg->angular_velocity.x; pitch_vel_ = msg->angular_velocity.y; + // Calculate average pitch angle error over a given ros duration + pitch_error_buffer_.push_back(std::make_pair( + current_trunk_fused_pitch_ - walk_engine_.getWantedTrunkPitch(), + msg->header.stamp)); + + + // Filter the buffer to only contain values within the last duration + pitch_error_buffer_.erase( + std::remove_if( + pitch_error_buffer_.begin(), + pitch_error_buffer_.end(), + [this, &msg](const std::pair &pair) { + auto pitch_error_duration = rclcpp::Duration::from_seconds(0.2); + return rclcpp::Time(msg->header.stamp) - pair.second > pitch_error_duration; + }), + pitch_error_buffer_.end()); + + // Save the last average pitch error + auto last_pitch_error_avg = pitch_error_avg_; + + // Calculate the average pitch error + pitch_error_avg_ = std::accumulate( + pitch_error_buffer_.begin(), + pitch_error_buffer_.end(), + 0.0, + [](double sum, const std::pair &pair) { + return sum + pair.first; + }) / pitch_error_buffer_.size(); + + // Calculate the derivative of the pitch error + pitch_error_derivative_ = (pitch_error_avg_ - last_pitch_error_avg) / 0.2; //TODO correct timing + + // Calculate the integral of the pitch error + pitch_error_integral_ += pitch_error_avg_ * 0.2; //TODO correct timing + if (config_.node.imu_active) { // compute the pitch offset to the currently wanted pitch of the engine double wanted_pitch = walk_engine_.getWantedTrunkPitch();