Feature/filter foot pressure

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,19 +36,27 @@ 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
   rclcpp::rclcpp
   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})
 

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]
+    }
+  }

bitbots_odometry/include/bitbots_odometry/motion_odometry.h

@@ -13,7 +13,7 @@
 #include <tf2_ros/transform_listener.h>
 #include <tf2/utils.h>
 #include <unistd.h>
-
+#include <bitbots_msgs/msg/foot_pressure.hpp>
 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_;
+
 };
 
 }

bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h

@@ -0,0 +1,42 @@
+#include <rclcpp/rclcpp.hpp>
+#include "odometry_parameters.hpp"
+#include <biped_interfaces/msg/phase.hpp>
+
+#include <bitbots_msgs/msg/foot_pressure.hpp>
+#include <std_msgs/msg/float64.hpp>
+using std::placeholders::_1;
+
+namespace bitbots_odometry {
+
+class WalkSupportStateDetector: public rclcpp::Node {
+ public:
+  WalkSupportStateDetector();
+  void loop();
+ private:
+  rclcpp::Subscription<bitbots_msgs::msg::FootPressure>::SharedPtr pressure_l_sub_;
+  rclcpp::Subscription<bitbots_msgs::msg::FootPressure>::SharedPtr pressure_r_sub_;
+  rclcpp::Publisher<biped_interfaces::msg::Phase>::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_;
+};
+
+}

bitbots_odometry/launch/odometry.launch

@@ -23,4 +23,8 @@
         <param name="cop_frame" value="$(var tf_prefix)cop"/>
         <param name="use_sim_time" value="$(var sim)"/>
     </node>
+
+    <node name="walk_support_state_detector" pkg="bitbots_odometry" exec="walk_support_state_detector" launch-prefix="$(var taskset)">
+            <param name="use_sim_time" value="$(var sim)"/>
+    </node>
 </launch>

bitbots_odometry/package.xml

@@ -16,6 +16,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>biped_interfaces</depend>
   <depend>bitbots_docs</depend>
+    <depend>bitbots_msgs</depend>
   <depend>bitbots_utils</depend>
   <depend>generate_parameter_library</depend>
   <depend>message_filters</depend>

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<biped_interfaces::msg::Phase>("walk_support_state",
+      this->create_subscription<biped_interfaces::msg::Phase>("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) {

bitbots_odometry/src/walk_support_state_detector.cpp

@@ -0,0 +1,106 @@
+#include <bitbots_odometry/walk_support_state_detector.h>
+namespace bitbots_odometry {
+
+WalkSupportStateDetector::WalkSupportStateDetector() : Node("WalkSupportStateDetector"),
+param_listener_(get_node_parameters_interface())
+{
+  config_ = param_listener_.get_params();
+
+  pressure_l_sub_ = this->create_subscription<bitbots_msgs::msg::FootPressure>(
+        "foot_pressure_left/raw", 1, std::bind(&WalkSupportStateDetector::pressure_l_callback, this, _1));
+  pressure_r_sub_ = this->create_subscription<bitbots_msgs::msg::FootPressure>(
+        "foot_pressure_right/raw", 1, std::bind(&WalkSupportStateDetector::pressure_r_callback, this, _1));
+  pub_foot_pressure_support_state_ = this->create_publisher<biped_interfaces::msg::Phase>("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<bitbots_odometry::WalkSupportStateDetector>();
+
+  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();
+}