diff --git a/bitbots_odometry/CMakeLists.txt b/bitbots_odometry/CMakeLists.txt index 52e3a64f..32c045ff 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) @@ -22,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 @@ -34,6 +36,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 @@ -41,12 +44,19 @@ target_link_libraries( odometry_parameters ) +target_link_libraries( + walk_support_state_detector + rclcpp::rclcpp + odometry_parameters +) + ## Specify libraries to link a library or executable target against ament_target_dependencies( motion_odometry ament_cmake biped_interfaces bitbots_docs + bitbots_msgs bitbots_utils generate_parameter_library geometry_msgs @@ -59,6 +69,15 @@ ament_target_dependencies( tf2_eigen tf2_geometry_msgs tf2_ros + std_msgs +) + +ament_target_dependencies( + walk_support_state_detector + bitbots_msgs + biped_interfaces + generate_parameter_library + std_msgs ) ament_target_dependencies( @@ -88,6 +107,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/config/odometry_config_template.yaml b/bitbots_odometry/config/odometry_config_template.yaml index 03d46145..5de4c85b 100644 --- a/bitbots_odometry/config/odometry_config_template.yaml +++ b/bitbots_odometry/config/odometry_config_template.yaml @@ -40,3 +40,40 @@ motion_odometry: default_value: false, description: "Should odom tf be published" } + + k: { + type: double, + default_value: 0.7, + 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] + } + } + + 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/include/bitbots_odometry/motion_odometry.h b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h index 9de61911..c91ab6d4 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,7 @@ class MotionOdometry : public rclcpp::Node { std::string previous_support_link_; std::string current_support_link_; rclcpp::Time start_time_; + }; } 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..0175be6a --- /dev/null +++ b/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h @@ -0,0 +1,42 @@ +#include +#include "odometry_parameters.hpp" +#include + +#include +#include +using std::placeholders::_1; + +namespace bitbots_odometry { + +class WalkSupportStateDetector: public rclcpp::Node { + public: + WalkSupportStateDetector(); + void loop(); + private: + 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_; + float_t pressure_filtered_left_; + float_t pressure_filtered_right_; + + long step_duration_r_; + rclcpp::Time up_r_; + + long step_duration_l_; + 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/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 @@ + + + + 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..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)); @@ -46,6 +46,9 @@ MotionOdometry::MotionOdometry() : Node("MotionOdometry"), foot_change_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); previous_support_link_ = r_sole_frame_; start_time_ = this->now(); + + + } void MotionOdometry::loop() { @@ -173,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; @@ -206,6 +209,7 @@ void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) current_odom_msg_ = *msg; } + } int main(int argc, char **argv) { 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..31371852 --- /dev/null +++ b/bitbots_odometry/src/walk_support_state_detector.cpp @@ -0,0 +1,106 @@ +#include +namespace bitbots_odometry { + +WalkSupportStateDetector::WalkSupportStateDetector() : Node("WalkSupportStateDetector"), +param_listener_(get_node_parameters_interface()) +{ + config_ = param_listener_.get_params(); + + 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; + step_duration_r_ = 0; + up_r_ = this->now(); + step_duration_l_ = 0; + up_l_ = this->now(); + +} + +void WalkSupportStateDetector::loop() { + 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_ && (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(config_.temporal_step_offset*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-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_; + if (pressure_filtered_left_ > config_.summed_pressure_threshold){ + if (curr_stand_left_ != true){ + up_l_ = this->now(); + + curr_stand_left_ = true;} + } + else{ + if (curr_stand_left_ != false){ + step_duration_l_ = (1-config_.m)*(this->now().nanoseconds() - up_l_.nanoseconds()) + config_.m*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-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_; + if (pressure_filtered_right_ > config_.summed_pressure_threshold){ + if (curr_stand_right_ != true){ + up_r_ = this->now(); + + curr_stand_right_ = true;} + } + else{ + if (curr_stand_right_ != false){ + step_duration_r_ = (1-config_.m)*(this->now().nanoseconds() - up_r_.nanoseconds()) + config_.m*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(); +}