From b58555a6cd791566caf964883ad8c5df69654730 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Mon, 20 Nov 2023 15:59:56 +0100 Subject: [PATCH 1/9] add pressure filter --- bitbots_odometry/CMakeLists.txt | 2 ++ .../bitbots_odometry/motion_odometry.h | 9 +++++- bitbots_odometry/package.xml | 1 + bitbots_odometry/src/motion_odometry.cpp | 30 +++++++++++++++++++ 4 files changed, 41 insertions(+), 1 deletion(-) diff --git a/bitbots_odometry/CMakeLists.txt b/bitbots_odometry/CMakeLists.txt index 52e3a64f..3188a842 100644 --- a/bitbots_odometry/CMakeLists.txt +++ b/bitbots_odometry/CMakeLists.txt @@ -9,6 +9,7 @@ endif () find_package(ament_cmake REQUIRED) find_package(biped_interfaces REQUIRED) find_package(bitbots_docs REQUIRED) +find_package(bitbots_msgs REQUIRED) find_package(bitbots_utils REQUIRED) find_package(Eigen3 REQUIRED) find_package(generate_parameter_library REQUIRED) @@ -47,6 +48,7 @@ ament_target_dependencies( ament_cmake biped_interfaces bitbots_docs + bitbots_msgs bitbots_utils generate_parameter_library geometry_msgs diff --git a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h index 9de61911..4a8d87b8 100644 --- a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h +++ b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h @@ -13,7 +13,7 @@ #include #include #include - +#include using std::placeholders::_1; namespace bitbots_odometry { @@ -50,6 +50,13 @@ class MotionOdometry : public rclcpp::Node { std::string previous_support_link_; std::string current_support_link_; rclcpp::Time start_time_; + + // put this in other package later + rclcpp::Subscription::SharedPtr pressure_l_sub_; + rclcpp::Subscription::SharedPtr pressure_r_sub_; + rclcpp::Publisher::SharedPtr pub_foot_pressure_support_state_; + void pressure_l_callback(bitbots_msgs::msg::FootPressure msg); + void pressure_r_callback(bitbots_msgs::msg::FootPressure msg); }; } diff --git a/bitbots_odometry/package.xml b/bitbots_odometry/package.xml index c3db876b..adafd099 100644 --- a/bitbots_odometry/package.xml +++ b/bitbots_odometry/package.xml @@ -16,6 +16,7 @@ ament_cmake biped_interfaces bitbots_docs + bitbots_msgs bitbots_utils generate_parameter_library message_filters diff --git a/bitbots_odometry/src/motion_odometry.cpp b/bitbots_odometry/src/motion_odometry.cpp index 26231793..22a2d29c 100644 --- a/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_odometry/src/motion_odometry.cpp @@ -46,6 +46,15 @@ MotionOdometry::MotionOdometry() : Node("MotionOdometry"), foot_change_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); previous_support_link_ = r_sole_frame_; start_time_ = this->now(); + + // use foot pressure information, put into different package later + pressure_l_sub_ = this->create_subscription( + "foot_pressure_left/raw", 1, std::bind(&MotionOdometry::pressure_l_callback, this, _1)); + pressure_r_sub_ = this->create_subscription( + "foot_pressure_right/raw", 1, std::bind(&MotionOdometry::pressure_r_callback, this, _1)); + pub_foot_pressure_support_state_ = this->create_publisher("foot_pressure/walk_support_state", 1); + + } void MotionOdometry::loop() { @@ -206,6 +215,27 @@ void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) current_odom_msg_ = *msg; } + void MotionOdometry::pressure_l_callback(bitbots_msgs::msg::FootPressure msg) { + float_t summed_pressure = msg.left_back +msg.left_front + msg.right_front + msg.right_back; + if (summed_pressure > 30){ + curr_stand_left_ = true; + } + else{ + curr_stand_left_ = false; + } + + } + + void MotionOdometry::pressure_r_callback(bitbots_msgs::msg::FootPressure msg) { + float_t summed_pressure = msg.left_back +msg.left_front + msg.right_front + msg.right_back; + if (summed_pressure > 30){ + curr_stand_right_ = true; + } + else{ + curr_stand_right_ = false; + } + } + } int main(int argc, char **argv) { From 31bdc89cde36d430f14a431b862f366d427ec4c4 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Mon, 20 Nov 2023 17:22:39 +0100 Subject: [PATCH 2/9] make it work --- .../bitbots_odometry/motion_odometry.h | 9 +++++- bitbots_odometry/src/motion_odometry.cpp | 30 +++++++++++++++++-- 2 files changed, 35 insertions(+), 4 deletions(-) diff --git a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h index 4a8d87b8..6bdbeccf 100644 --- a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h +++ b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h @@ -55,8 +55,15 @@ class MotionOdometry : public rclcpp::Node { rclcpp::Subscription::SharedPtr pressure_l_sub_; rclcpp::Subscription::SharedPtr pressure_r_sub_; rclcpp::Publisher::SharedPtr pub_foot_pressure_support_state_; - void pressure_l_callback(bitbots_msgs::msg::FootPressure msg); + + void pressure_l_callback(bitbots_msgs::msg::FootPressure msg); void pressure_r_callback(bitbots_msgs::msg::FootPressure msg); + int curr_stand_left_; + int curr_stand_right_; + int prev_stand_right_; + int prev_stand_left_; + biped_interfaces::msg::Phase curr_stance_; + }; } diff --git a/bitbots_odometry/src/motion_odometry.cpp b/bitbots_odometry/src/motion_odometry.cpp index 22a2d29c..ff8aa34a 100644 --- a/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_odometry/src/motion_odometry.cpp @@ -53,7 +53,7 @@ MotionOdometry::MotionOdometry() : Node("MotionOdometry"), pressure_r_sub_ = this->create_subscription( "foot_pressure_right/raw", 1, std::bind(&MotionOdometry::pressure_r_callback, this, _1)); pub_foot_pressure_support_state_ = this->create_publisher("foot_pressure/walk_support_state", 1); - + curr_stance_.phase = 2; } @@ -70,6 +70,28 @@ void MotionOdometry::loop() { rclcpp::Time cycle_start_time = this->now(); config_ = param_listener_.get_params(); + int curr_stand_left = curr_stand_left_; + int prev_stand_left = prev_stand_left_; + + int curr_stand_right = curr_stand_right_; + int prev_stand_right = prev_stand_right_; + + biped_interfaces::msg::Phase phase; + if ( curr_stand_left_ && curr_stand_right_){ + phase.phase = 2; + } + else if (curr_stand_left_ && ! curr_stand_right_){ + phase.phase = 1; + } + else if (!curr_stand_left_ && curr_stand_right_){ + phase.phase = 0; + } + if (phase.phase != curr_stance_.phase){ + curr_stance_.phase = phase.phase; + phase.header.stamp = this->now(); + + pub_foot_pressure_support_state_->publish(phase); + } // check if step finished, meaning left->right or right->left support. double support is skipped // the support foot change is published when the joint goals for the last movements are published. // it takes some time till the joints actually reach this position, this can create some offset @@ -217,7 +239,8 @@ void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) void MotionOdometry::pressure_l_callback(bitbots_msgs::msg::FootPressure msg) { float_t summed_pressure = msg.left_back +msg.left_front + msg.right_front + msg.right_back; - if (summed_pressure > 30){ + prev_stand_left_ = curr_stand_left_; + if (summed_pressure > 20){ curr_stand_left_ = true; } else{ @@ -228,7 +251,8 @@ void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) void MotionOdometry::pressure_r_callback(bitbots_msgs::msg::FootPressure msg) { float_t summed_pressure = msg.left_back +msg.left_front + msg.right_front + msg.right_back; - if (summed_pressure > 30){ + prev_stand_right_ = curr_stand_right_; + if (summed_pressure > 20){ curr_stand_right_ = true; } else{ From 9a2ee553236dfb9178dbe6e007d5cacd33179250 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Mon, 20 Nov 2023 18:13:50 +0100 Subject: [PATCH 3/9] move support state detector to own node --- bitbots_odometry/CMakeLists.txt | 15 ++++++ .../bitbots_odometry/motion_odometry.h | 13 ----- bitbots_odometry/src/motion_odometry.cpp | 54 +------------------ 3 files changed, 17 insertions(+), 65 deletions(-) diff --git a/bitbots_odometry/CMakeLists.txt b/bitbots_odometry/CMakeLists.txt index 3188a842..24acd15f 100644 --- a/bitbots_odometry/CMakeLists.txt +++ b/bitbots_odometry/CMakeLists.txt @@ -35,6 +35,7 @@ add_compile_options(-Wall -Werror -Wno-unused) add_executable(odometry_fuser src/odometry_fuser.cpp) add_executable(motion_odometry src/motion_odometry.cpp) +add_executable(walk_support_state_detector src/walk_support_state_detector.cpp) target_link_libraries( motion_odometry @@ -42,6 +43,11 @@ target_link_libraries( odometry_parameters ) +target_link_libraries( + walk_support_state_detector + rclcpp::rclcpp +) + ## Specify libraries to link a library or executable target against ament_target_dependencies( motion_odometry @@ -63,6 +69,12 @@ ament_target_dependencies( tf2_ros ) +ament_target_dependencies( + walk_support_state_detector + bitbots_msgs + biped_interfaces +) + ament_target_dependencies( odometry_fuser ament_cmake @@ -90,6 +102,9 @@ install(TARGETS motion_odometry install(TARGETS odometry_fuser DESTINATION lib/${PROJECT_NAME}) +install(TARGETS walk_support_state_detector + DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) diff --git a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h index 6bdbeccf..c91ab6d4 100644 --- a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h +++ b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h @@ -51,19 +51,6 @@ class MotionOdometry : public rclcpp::Node { std::string current_support_link_; rclcpp::Time start_time_; - // put this in other package later - rclcpp::Subscription::SharedPtr pressure_l_sub_; - rclcpp::Subscription::SharedPtr pressure_r_sub_; - rclcpp::Publisher::SharedPtr pub_foot_pressure_support_state_; - - void pressure_l_callback(bitbots_msgs::msg::FootPressure msg); - void pressure_r_callback(bitbots_msgs::msg::FootPressure msg); - int curr_stand_left_; - int curr_stand_right_; - int prev_stand_right_; - int prev_stand_left_; - biped_interfaces::msg::Phase curr_stance_; - }; } diff --git a/bitbots_odometry/src/motion_odometry.cpp b/bitbots_odometry/src/motion_odometry.cpp index ff8aa34a..a230a6c1 100644 --- a/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_odometry/src/motion_odometry.cpp @@ -47,13 +47,7 @@ MotionOdometry::MotionOdometry() : Node("MotionOdometry"), previous_support_link_ = r_sole_frame_; start_time_ = this->now(); - // use foot pressure information, put into different package later - pressure_l_sub_ = this->create_subscription( - "foot_pressure_left/raw", 1, std::bind(&MotionOdometry::pressure_l_callback, this, _1)); - pressure_r_sub_ = this->create_subscription( - "foot_pressure_right/raw", 1, std::bind(&MotionOdometry::pressure_r_callback, this, _1)); - pub_foot_pressure_support_state_ = this->create_publisher("foot_pressure/walk_support_state", 1); - curr_stance_.phase = 2; + } @@ -70,28 +64,6 @@ void MotionOdometry::loop() { rclcpp::Time cycle_start_time = this->now(); config_ = param_listener_.get_params(); - int curr_stand_left = curr_stand_left_; - int prev_stand_left = prev_stand_left_; - - int curr_stand_right = curr_stand_right_; - int prev_stand_right = prev_stand_right_; - - biped_interfaces::msg::Phase phase; - if ( curr_stand_left_ && curr_stand_right_){ - phase.phase = 2; - } - else if (curr_stand_left_ && ! curr_stand_right_){ - phase.phase = 1; - } - else if (!curr_stand_left_ && curr_stand_right_){ - phase.phase = 0; - } - if (phase.phase != curr_stance_.phase){ - curr_stance_.phase = phase.phase; - phase.header.stamp = this->now(); - - pub_foot_pressure_support_state_->publish(phase); - } // check if step finished, meaning left->right or right->left support. double support is skipped // the support foot change is published when the joint goals for the last movements are published. // it takes some time till the joints actually reach this position, this can create some offset @@ -204,7 +176,7 @@ void MotionOdometry::loop() { void MotionOdometry::supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg) { current_support_state_ = msg->phase; current_support_state_time_ = msg->header.stamp; - + // remember if we received first support state, only remember left or right if (previous_support_state_ == -1 && current_support_state_ != biped_interfaces::msg::Phase::DOUBLE_STANCE) { std::string current_support_link; @@ -237,28 +209,6 @@ void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) current_odom_msg_ = *msg; } - void MotionOdometry::pressure_l_callback(bitbots_msgs::msg::FootPressure msg) { - float_t summed_pressure = msg.left_back +msg.left_front + msg.right_front + msg.right_back; - prev_stand_left_ = curr_stand_left_; - if (summed_pressure > 20){ - curr_stand_left_ = true; - } - else{ - curr_stand_left_ = false; - } - - } - - void MotionOdometry::pressure_r_callback(bitbots_msgs::msg::FootPressure msg) { - float_t summed_pressure = msg.left_back +msg.left_front + msg.right_front + msg.right_back; - prev_stand_right_ = curr_stand_right_; - if (summed_pressure > 20){ - curr_stand_right_ = true; - } - else{ - curr_stand_right_ = false; - } - } } From 3db2077f2a976759487c7626088c991a05bc6523 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Wed, 22 Nov 2023 10:29:47 +0100 Subject: [PATCH 4/9] add node to launch --- bitbots_odometry/launch/odometry.launch | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/bitbots_odometry/launch/odometry.launch b/bitbots_odometry/launch/odometry.launch index df13a281..278df807 100644 --- a/bitbots_odometry/launch/odometry.launch +++ b/bitbots_odometry/launch/odometry.launch @@ -23,4 +23,8 @@ + + + + From 769eb1cf72282fcad5906a30e81bf230d86f4c60 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Wed, 22 Nov 2023 11:54:26 +0100 Subject: [PATCH 5/9] add debug and low pass --- bitbots_odometry/CMakeLists.txt | 3 +++ bitbots_odometry/src/motion_odometry.cpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/bitbots_odometry/CMakeLists.txt b/bitbots_odometry/CMakeLists.txt index 24acd15f..cf6254a0 100644 --- a/bitbots_odometry/CMakeLists.txt +++ b/bitbots_odometry/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(tf2 REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(std_msgs REQUIRED) generate_parameter_library( odometry_parameters @@ -67,12 +68,14 @@ ament_target_dependencies( tf2_eigen tf2_geometry_msgs tf2_ros + std_msgs ) ament_target_dependencies( walk_support_state_detector bitbots_msgs biped_interfaces + std_msgs ) ament_target_dependencies( diff --git a/bitbots_odometry/src/motion_odometry.cpp b/bitbots_odometry/src/motion_odometry.cpp index a230a6c1..0d5fd9d6 100644 --- a/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_odometry/src/motion_odometry.cpp @@ -24,7 +24,7 @@ MotionOdometry::MotionOdometry() : Node("MotionOdometry"), previous_support_state_ = -1; walk_support_state_sub_ = - this->create_subscription("walk_support_state", + this->create_subscription("foot_pressure/walk_support_state", 1, std::bind(&MotionOdometry::supportCallback, this, _1)); From aaff146b16cb03faf102157d1d162f06d3a09c10 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Wed, 22 Nov 2023 13:53:06 +0100 Subject: [PATCH 6/9] add step duration threshold --- .../walk_support_state_detector.h | 40 +++++++ .../src/walk_support_state_detector.cpp | 110 ++++++++++++++++++ 2 files changed, 150 insertions(+) create mode 100644 bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h create mode 100644 bitbots_odometry/src/walk_support_state_detector.cpp diff --git a/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h b/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h new file mode 100644 index 00000000..2d1b9fd7 --- /dev/null +++ b/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h @@ -0,0 +1,40 @@ +#include + +#include + +#include +#include +using std::placeholders::_1; + +namespace bitbots_odometry { + +class WalkSupportStateDetector: public rclcpp::Node { + public: + WalkSupportStateDetector(); + void loop(); + private: + // put this in other package later + rclcpp::Subscription::SharedPtr pressure_l_sub_; + rclcpp::Subscription::SharedPtr pressure_r_sub_; + rclcpp::Publisher::SharedPtr pub_foot_pressure_support_state_; + rclcpp::Publisher::SharedPtr pub_foot_pressure_debug_l_; +rclcpp::Publisher::SharedPtr pub_foot_pressure_debug_r_; + void pressure_l_callback(bitbots_msgs::msg::FootPressure msg); + void pressure_r_callback(bitbots_msgs::msg::FootPressure msg); + int curr_stand_left_; + int curr_stand_right_; + int prev_stand_right_; + int prev_stand_left_; + float_t pressure_filtered_left_; + float_t pressure_filtered_right_; + float_t k; + long step_duration_r_; + rclcpp::Time up_r_; + + long step_duration_l_; + rclcpp::Time up_l_; + biped_interfaces::msg::Phase curr_stance_; + +}; + +} diff --git a/bitbots_odometry/src/walk_support_state_detector.cpp b/bitbots_odometry/src/walk_support_state_detector.cpp new file mode 100644 index 00000000..d5c8ff7a --- /dev/null +++ b/bitbots_odometry/src/walk_support_state_detector.cpp @@ -0,0 +1,110 @@ +#include +namespace bitbots_odometry { + +WalkSupportStateDetector::WalkSupportStateDetector() : Node("WalkSupportStateDetector") +{ + //debug + pub_foot_pressure_debug_l_ = this->create_publisher("foot_pressure/filtered_l", 1); + pub_foot_pressure_debug_r_ = this->create_publisher("foot_pressure/filtered_r", 1); + // use foot pressure information, put into different package later + pressure_l_sub_ = this->create_subscription( + "foot_pressure_left/raw", 1, std::bind(&WalkSupportStateDetector::pressure_l_callback, this, _1)); + pressure_r_sub_ = this->create_subscription( + "foot_pressure_right/raw", 1, std::bind(&WalkSupportStateDetector::pressure_r_callback, this, _1)); + pub_foot_pressure_support_state_ = this->create_publisher("foot_pressure/walk_support_state", 1); + curr_stance_.phase = 2; + pressure_filtered_right_ = 0; + pressure_filtered_left_ = 0; + k = 0.85; + step_duration_r_ = 0; + up_r_ = this->now(); // this is not true + step_duration_l_ = 0; + up_l_ = this->now(); // this is not true + +} + +void WalkSupportStateDetector::loop() { + int curr_stand_left = curr_stand_left_; + int prev_stand_left = prev_stand_left_; + + int curr_stand_right = curr_stand_right_; + int prev_stand_right = prev_stand_right_; + + biped_interfaces::msg::Phase phase; + if ( curr_stand_left_ && curr_stand_right_){ + phase.phase = 2; + } + else if (curr_stand_left_ && ! curr_stand_right_){//} && (up_r_ + rclcpp::Duration::from_nanoseconds(int(0.1*step_duration_r_))) < this->now()){ + phase.phase = 0; + } + else if (!curr_stand_left_ && curr_stand_right_){//} &&(up_r_ + rclcpp::Duration::from_nanoseconds(int(0.1*step_duration_r_))) < this->now()){ + phase.phase = 1; + } + if (phase.phase != curr_stance_.phase){ + curr_stance_.phase = phase.phase; + phase.header.stamp = this->now(); + + pub_foot_pressure_support_state_->publish(phase); + } + +} + + void WalkSupportStateDetector::pressure_l_callback(bitbots_msgs::msg::FootPressure msg) { + float_t summed_pressure = msg.left_back +msg.left_front + msg.right_front + msg.right_back; + pressure_filtered_left_ = (1-k)*summed_pressure + k*pressure_filtered_left_; + prev_stand_left_ = curr_stand_left_; + std_msgs::msg::Float64 pressure_msg; + pressure_msg.data = pressure_filtered_left_; + pub_foot_pressure_debug_l_->publish(pressure_msg); + if (pressure_filtered_left_ > 50){ + if (curr_stand_left_ != true){ + up_l_ = this->now(); + + curr_stand_left_ = true;} + } + else{ + if (curr_stand_left_ != false){ + step_duration_l_ = (1-k)*(this->now().nanoseconds() - up_l_.nanoseconds()) + k*step_duration_l_; + curr_stand_left_ = false; + } + } + + } + + void WalkSupportStateDetector::pressure_r_callback(bitbots_msgs::msg::FootPressure msg) { + float_t summed_pressure = msg.left_back +msg.left_front + msg.right_front + msg.right_back; + pressure_filtered_right_ = (1-k)*summed_pressure + k*pressure_filtered_right_; + prev_stand_right_ = curr_stand_right_; + std_msgs::msg::Float64 pressure_msg; + pressure_msg.data = pressure_filtered_right_; + pub_foot_pressure_debug_r_->publish(pressure_msg); + + if (pressure_filtered_right_ > 50){ + if (curr_stand_right_ != true){ + up_r_ = this->now(); + + curr_stand_right_ = true;} + } + else{ + if (curr_stand_right_ != false){ + step_duration_r_ = (1-k)*(this->now().nanoseconds() - up_r_.nanoseconds()) + k*step_duration_r_; + curr_stand_right_ = false; + } + } + } + +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + rclcpp::Duration timer_duration = rclcpp::Duration::from_seconds(1.0 / 600.0); + rclcpp::experimental::executors::EventsExecutor exec; + exec.add_node(node); + + rclcpp::TimerBase::SharedPtr timer = rclcpp::create_timer(node, node->get_clock(), timer_duration, [node]() -> void {node->loop();}); + + exec.spin(); + rclcpp::shutdown(); +} From 3c9cf6c47e417247474dbe5f7a50c6c645ff439d Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 24 Nov 2023 12:00:40 +0100 Subject: [PATCH 7/9] add parameter library --- bitbots_odometry/CMakeLists.txt | 2 ++ .../config/odometry_config_template.yaml | 19 +++++++++++++++++++ .../walk_support_state_detector.h | 8 ++++++-- .../src/walk_support_state_detector.cpp | 17 ++++++++++------- 4 files changed, 37 insertions(+), 9 deletions(-) diff --git a/bitbots_odometry/CMakeLists.txt b/bitbots_odometry/CMakeLists.txt index cf6254a0..32c045ff 100644 --- a/bitbots_odometry/CMakeLists.txt +++ b/bitbots_odometry/CMakeLists.txt @@ -47,6 +47,7 @@ target_link_libraries( target_link_libraries( walk_support_state_detector rclcpp::rclcpp + odometry_parameters ) ## Specify libraries to link a library or executable target against @@ -75,6 +76,7 @@ ament_target_dependencies( walk_support_state_detector bitbots_msgs biped_interfaces + generate_parameter_library std_msgs ) diff --git a/bitbots_odometry/config/odometry_config_template.yaml b/bitbots_odometry/config/odometry_config_template.yaml index 03d46145..18bfec34 100644 --- a/bitbots_odometry/config/odometry_config_template.yaml +++ b/bitbots_odometry/config/odometry_config_template.yaml @@ -40,3 +40,22 @@ motion_odometry: default_value: false, description: "Should odom tf be published" } + + k: { + type: double, + default_value: 0.95, + description: "Factor for lowpass filtering the foot pressure", + validation: { + bounds<>: [0.0, 1.0] + } + } + + + m: { + type: double, + default_value: 0.95, + description: "Factor for lowpass filtering the step duration", + validation: { + bounds<>: [0.0, 1.0] + } + } diff --git a/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h b/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h index 2d1b9fd7..9b5e5899 100644 --- a/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h +++ b/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h @@ -1,5 +1,5 @@ #include - +#include "odometry_parameters.hpp" #include #include @@ -13,7 +13,6 @@ class WalkSupportStateDetector: public rclcpp::Node { WalkSupportStateDetector(); void loop(); private: - // put this in other package later rclcpp::Subscription::SharedPtr pressure_l_sub_; rclcpp::Subscription::SharedPtr pressure_r_sub_; rclcpp::Publisher::SharedPtr pub_foot_pressure_support_state_; @@ -28,6 +27,7 @@ rclcpp::Publisher::SharedPtr pub_foot_pressure_debug_r_; float_t pressure_filtered_left_; float_t pressure_filtered_right_; float_t k; + float_t m; long step_duration_r_; rclcpp::Time up_r_; @@ -35,6 +35,10 @@ rclcpp::Publisher::SharedPtr pub_foot_pressure_debug_r_; rclcpp::Time up_l_; biped_interfaces::msg::Phase curr_stance_; + // 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_; }; } diff --git a/bitbots_odometry/src/walk_support_state_detector.cpp b/bitbots_odometry/src/walk_support_state_detector.cpp index d5c8ff7a..dbc388d8 100644 --- a/bitbots_odometry/src/walk_support_state_detector.cpp +++ b/bitbots_odometry/src/walk_support_state_detector.cpp @@ -1,8 +1,10 @@ #include namespace bitbots_odometry { -WalkSupportStateDetector::WalkSupportStateDetector() : Node("WalkSupportStateDetector") +WalkSupportStateDetector::WalkSupportStateDetector() : Node("WalkSupportStateDetector"), +param_listener_(get_node_parameters_interface()) { + config_ = param_listener_.get_params(); //debug pub_foot_pressure_debug_l_ = this->create_publisher("foot_pressure/filtered_l", 1); pub_foot_pressure_debug_r_ = this->create_publisher("foot_pressure/filtered_r", 1); @@ -24,6 +26,7 @@ WalkSupportStateDetector::WalkSupportStateDetector() : Node("WalkSupportStateDet } void WalkSupportStateDetector::loop() { + config_ = param_listener_.get_params(); int curr_stand_left = curr_stand_left_; int prev_stand_left = prev_stand_left_; @@ -34,10 +37,10 @@ void WalkSupportStateDetector::loop() { if ( curr_stand_left_ && curr_stand_right_){ phase.phase = 2; } - else if (curr_stand_left_ && ! curr_stand_right_){//} && (up_r_ + rclcpp::Duration::from_nanoseconds(int(0.1*step_duration_r_))) < this->now()){ + else if (curr_stand_left_ && ! curr_stand_right_ && (up_r_ + rclcpp::Duration::from_nanoseconds(int(0.1*step_duration_r_))) < this->now()){ phase.phase = 0; } - else if (!curr_stand_left_ && curr_stand_right_){//} &&(up_r_ + rclcpp::Duration::from_nanoseconds(int(0.1*step_duration_r_))) < this->now()){ + else if (!curr_stand_left_ && curr_stand_right_ &&(up_r_ + rclcpp::Duration::from_nanoseconds(int(0.1*step_duration_r_))) < this->now()){ phase.phase = 1; } if (phase.phase != curr_stance_.phase){ @@ -51,7 +54,7 @@ void WalkSupportStateDetector::loop() { void WalkSupportStateDetector::pressure_l_callback(bitbots_msgs::msg::FootPressure msg) { float_t summed_pressure = msg.left_back +msg.left_front + msg.right_front + msg.right_back; - pressure_filtered_left_ = (1-k)*summed_pressure + k*pressure_filtered_left_; + pressure_filtered_left_ = (1-config_.k)*summed_pressure + config_.k*pressure_filtered_left_; prev_stand_left_ = curr_stand_left_; std_msgs::msg::Float64 pressure_msg; pressure_msg.data = pressure_filtered_left_; @@ -64,7 +67,7 @@ void WalkSupportStateDetector::loop() { } else{ if (curr_stand_left_ != false){ - step_duration_l_ = (1-k)*(this->now().nanoseconds() - up_l_.nanoseconds()) + k*step_duration_l_; + step_duration_l_ = (1-config_.m)*(this->now().nanoseconds() - up_l_.nanoseconds()) + config_.m*step_duration_l_; curr_stand_left_ = false; } } @@ -73,7 +76,7 @@ void WalkSupportStateDetector::loop() { void WalkSupportStateDetector::pressure_r_callback(bitbots_msgs::msg::FootPressure msg) { float_t summed_pressure = msg.left_back +msg.left_front + msg.right_front + msg.right_back; - pressure_filtered_right_ = (1-k)*summed_pressure + k*pressure_filtered_right_; + pressure_filtered_right_ = (1-config_.k)*summed_pressure + config_.k*pressure_filtered_right_; prev_stand_right_ = curr_stand_right_; std_msgs::msg::Float64 pressure_msg; pressure_msg.data = pressure_filtered_right_; @@ -87,7 +90,7 @@ void WalkSupportStateDetector::loop() { } else{ if (curr_stand_right_ != false){ - step_duration_r_ = (1-k)*(this->now().nanoseconds() - up_r_.nanoseconds()) + k*step_duration_r_; + step_duration_r_ = (1-config_.m)*(this->now().nanoseconds() - up_r_.nanoseconds()) + config_.m*step_duration_r_; curr_stand_right_ = false; } } From 25b00c5a6146afc3af82524ef8a1d148b9e16ccf Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 24 Nov 2023 12:32:58 +0100 Subject: [PATCH 8/9] add parameter tuning --- .../config/odometry_config_template.yaml | 20 ++++++++++++++++++- .../src/walk_support_state_detector.cpp | 8 ++++---- 2 files changed, 23 insertions(+), 5 deletions(-) diff --git a/bitbots_odometry/config/odometry_config_template.yaml b/bitbots_odometry/config/odometry_config_template.yaml index 18bfec34..5de4c85b 100644 --- a/bitbots_odometry/config/odometry_config_template.yaml +++ b/bitbots_odometry/config/odometry_config_template.yaml @@ -43,7 +43,7 @@ motion_odometry: k: { type: double, - default_value: 0.95, + default_value: 0.7, description: "Factor for lowpass filtering the foot pressure", validation: { bounds<>: [0.0, 1.0] @@ -59,3 +59,21 @@ motion_odometry: bounds<>: [0.0, 1.0] } } + + temporal_step_offset: { + type: double, + default_value: 0.0, + description: "Factor for detecting when the robot really stands on one foot", + validation: { + bounds<>: [0.0, 1.0] + } + } + + summed_pressure_threshold: { + type: int, + default_value: 40, + description: "Factor for detecting when the robot really stands on one foot", + validation: { + bounds<>: [25, 200] + } + } diff --git a/bitbots_odometry/src/walk_support_state_detector.cpp b/bitbots_odometry/src/walk_support_state_detector.cpp index dbc388d8..63f3a9df 100644 --- a/bitbots_odometry/src/walk_support_state_detector.cpp +++ b/bitbots_odometry/src/walk_support_state_detector.cpp @@ -37,10 +37,10 @@ void WalkSupportStateDetector::loop() { if ( curr_stand_left_ && curr_stand_right_){ phase.phase = 2; } - else if (curr_stand_left_ && ! curr_stand_right_ && (up_r_ + rclcpp::Duration::from_nanoseconds(int(0.1*step_duration_r_))) < this->now()){ + else if (curr_stand_left_ && ! curr_stand_right_ && (up_r_ + rclcpp::Duration::from_nanoseconds(int(config_.temporal_step_offset*step_duration_r_))) < this->now()){ phase.phase = 0; } - else if (!curr_stand_left_ && curr_stand_right_ &&(up_r_ + rclcpp::Duration::from_nanoseconds(int(0.1*step_duration_r_))) < this->now()){ + else if (!curr_stand_left_ && curr_stand_right_ &&(up_r_ + rclcpp::Duration::from_nanoseconds(int(config_.temporal_step_offset*step_duration_r_))) < this->now()){ phase.phase = 1; } if (phase.phase != curr_stance_.phase){ @@ -59,7 +59,7 @@ void WalkSupportStateDetector::loop() { std_msgs::msg::Float64 pressure_msg; pressure_msg.data = pressure_filtered_left_; pub_foot_pressure_debug_l_->publish(pressure_msg); - if (pressure_filtered_left_ > 50){ + if (pressure_filtered_left_ > config_.summed_pressure_threshold){ if (curr_stand_left_ != true){ up_l_ = this->now(); @@ -82,7 +82,7 @@ void WalkSupportStateDetector::loop() { pressure_msg.data = pressure_filtered_right_; pub_foot_pressure_debug_r_->publish(pressure_msg); - if (pressure_filtered_right_ > 50){ + if (pressure_filtered_right_ > config_.summed_pressure_threshold){ if (curr_stand_right_ != true){ up_r_ = this->now(); From ab44063a43936656808e96ca534f9d53afa04b32 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 24 Nov 2023 12:35:55 +0100 Subject: [PATCH 9/9] remove unused variables and comments --- .../bitbots_odometry/walk_support_state_detector.h | 6 ++---- .../src/walk_support_state_detector.cpp | 13 +++---------- 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h b/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h index 9b5e5899..0175be6a 100644 --- a/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h +++ b/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h @@ -16,8 +16,7 @@ class WalkSupportStateDetector: public rclcpp::Node { rclcpp::Subscription::SharedPtr pressure_l_sub_; rclcpp::Subscription::SharedPtr pressure_r_sub_; rclcpp::Publisher::SharedPtr pub_foot_pressure_support_state_; - rclcpp::Publisher::SharedPtr pub_foot_pressure_debug_l_; -rclcpp::Publisher::SharedPtr pub_foot_pressure_debug_r_; + void pressure_l_callback(bitbots_msgs::msg::FootPressure msg); void pressure_r_callback(bitbots_msgs::msg::FootPressure msg); int curr_stand_left_; @@ -26,8 +25,7 @@ rclcpp::Publisher::SharedPtr pub_foot_pressure_debug_r_; int prev_stand_left_; float_t pressure_filtered_left_; float_t pressure_filtered_right_; - float_t k; - float_t m; + long step_duration_r_; rclcpp::Time up_r_; diff --git a/bitbots_odometry/src/walk_support_state_detector.cpp b/bitbots_odometry/src/walk_support_state_detector.cpp index 63f3a9df..31371852 100644 --- a/bitbots_odometry/src/walk_support_state_detector.cpp +++ b/bitbots_odometry/src/walk_support_state_detector.cpp @@ -5,10 +5,7 @@ WalkSupportStateDetector::WalkSupportStateDetector() : Node("WalkSupportStateDet param_listener_(get_node_parameters_interface()) { config_ = param_listener_.get_params(); - //debug - pub_foot_pressure_debug_l_ = this->create_publisher("foot_pressure/filtered_l", 1); - pub_foot_pressure_debug_r_ = this->create_publisher("foot_pressure/filtered_r", 1); - // use foot pressure information, put into different package later + pressure_l_sub_ = this->create_subscription( "foot_pressure_left/raw", 1, std::bind(&WalkSupportStateDetector::pressure_l_callback, this, _1)); pressure_r_sub_ = this->create_subscription( @@ -17,11 +14,10 @@ param_listener_(get_node_parameters_interface()) curr_stance_.phase = 2; pressure_filtered_right_ = 0; pressure_filtered_left_ = 0; - k = 0.85; step_duration_r_ = 0; - up_r_ = this->now(); // this is not true + up_r_ = this->now(); step_duration_l_ = 0; - up_l_ = this->now(); // this is not true + up_l_ = this->now(); } @@ -58,7 +54,6 @@ void WalkSupportStateDetector::loop() { prev_stand_left_ = curr_stand_left_; std_msgs::msg::Float64 pressure_msg; pressure_msg.data = pressure_filtered_left_; - pub_foot_pressure_debug_l_->publish(pressure_msg); if (pressure_filtered_left_ > config_.summed_pressure_threshold){ if (curr_stand_left_ != true){ up_l_ = this->now(); @@ -80,8 +75,6 @@ void WalkSupportStateDetector::loop() { prev_stand_right_ = curr_stand_right_; std_msgs::msg::Float64 pressure_msg; pressure_msg.data = pressure_filtered_right_; - pub_foot_pressure_debug_r_->publish(pressure_msg); - if (pressure_filtered_right_ > config_.summed_pressure_threshold){ if (curr_stand_right_ != true){ up_r_ = this->now();