Use foot sensor to detect steps / rework pressure converter.

bitbots_lowlevel/bitbots_ros_control/config/pressure_amy.yaml

@@ -1,22 +1,22 @@
 pressure_converter:
   ros__parameters:
     zero_r:
-      - -27455372.92
-      - 14459386.46
-      - 17952654.76
-      - -10605254.74
+      - -66830780.078431375
+      - 5538079.1372549022
+      - -24570754.156862747
+      - -168909925.33333334
     zero_l:
-      - -14063383.6
-      - 30331751.92
-      - -2034247.26
-      - 48130515.2
+      - -3166809.769230769
+      - -58675654.461538464
+      - 18054924.769230768
+      - -2147483648.0
     scale_r:
-      - 2.879445523352348e-06
-      - -2.252145560713656e-05
-      - -2.754225471994077e-06
-      - 2.763926644524643e-06
+      - 0.00044307157572154597
+      - -1.7351776459253373e-05
+      - -0.00047527688810231853
+      - -0.00079238401835424893
     scale_l:
-      - 2.879445523352348e-06
-      - -2.252145560713656e-05
-      - -2.754225471994077e-06
-      - 2.763926644524643e-06
+      - 0.0014725335760224815
+      - -3.8384549952827295e-06
+      - 0.00047995524568892787
+      - .inf

bitbots_lowlevel/bitbots_ros_control/config/wolfgang.yaml

@@ -158,6 +158,15 @@ wolfgang_hardware_interface:
         interface_type: LED
         number_of_LEDs: 3
         start_number: 3
+
+      LeftFoot:
+        id: 102
+        topic: /foot_pressure_left/raw
+        model_number: 0
+      RightFoot:
+        id: 101
+        topic: /foot_pressure_right/raw
+        model_number: 0 
       # Removed head imu at worldcup due to motorbus issues
       #IMU_head:
       #  id: 242

bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch

@@ -28,7 +28,7 @@
         <include file="$(find-pkg-share system_monitor)/launch/system_monitor.launch"/>
         <include file="$(find-pkg-share bitbots_buttons)/launch/buttons.launch"/>
         <include file="$(find-pkg-share humanoid_league_speaker)/launch/speaker.launch"/>
-        <!--include file="$(find-pkg-share bitbots_ros_control)/launch/pressure_converter.launch"/-->
+        <include file="$(find-pkg-share bitbots_ros_control)/launch/pressure_converter.launch"/>
     </group>
 
 </launch>

bitbots_motion/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
                            config/odometry_config_template.yaml)
@@ -32,15 +34,23 @@ 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)
 
-# Specify libraries to link a library or executable target against
+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
@@ -52,7 +62,17 @@ ament_target_dependencies(
   tf2
   tf2_eigen
   tf2_geometry_msgs
-  tf2_ros)
+  tf2_ros
+  std_msgs
+)
+
+ament_target_dependencies(
+  walk_support_state_detector
+  bitbots_msgs
+  biped_interfaces
+  generate_parameter_library
+  std_msgs
+)
 
 ament_target_dependencies(
   odometry_fuser
@@ -78,7 +98,11 @@ install(TARGETS motion_odometry DESTINATION lib/${PROJECT_NAME})
 
 install(TARGETS odometry_fuser DESTINATION lib/${PROJECT_NAME})
 
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(TARGETS walk_support_state_detector
+        DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch
+        DESTINATION share/${PROJECT_NAME})
 
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 

bitbots_motion/bitbots_odometry/config/odometry_config_template.yaml

@@ -40,3 +40,67 @@ motion_odometry:
     default_value: false,
     description:  "Should odom tf be published"
   }
+
+  k: {
+    type: double,
+    default_value: 0.97,
+    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. This only plays a role if temporal_step_offset is not 0.",
+    validation: {
+      bounds<>: [0.0, 1.0]
+    }
+  }
+
+  temporal_step_offset_factor: {
+    type: double,
+    default_value: 0.0,
+    description: "Offset factor for taking the time stamp when the robot really stands on one foot. 1 means waiting until the steps is finished, 0 means taking the first time stamp when robots start standing",
+    validation: {
+      bounds<>: [0.0, 1.0]
+    }
+  }
+
+  summed_pressure_threshold_right: {
+    type: int,
+    default_value: 2500,
+    description: "Threshold for detecting whether the robot really stands on right foot. If they overlap, the robot is considered to stand on both feet.",
+    validation: {
+      bounds<>: [0, 5000]
+    }
+  }
+
+  summed_pressure_threshold_left: {
+    type: int,
+    default_value: 2500,
+    description: "Threshold for detecting whether the robot really stands on left foot",
+    validation: {
+      bounds<>: [0, 5000]
+    }
+  }
+
+  foot_pressure_topic_left: {
+    type: string,
+    default_value: "cod",
+    description: "Topic for the left foot pressure sensor"
+  }
+
+  foot_pressure_topic_right: {
+    type: string,
+    default_value: "foot_pressure_right/filtered",
+    description: "Topic for the right foot pressure sensor"
+  }
+
+  debug: {
+    type: bool,
+    default_value: true,
+    description: "Whether debug information should be published"
+  }

bitbots_motion/bitbots_odometry/config/odometry_conifg_sim.yaml

@@ -0,0 +1,16 @@
+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
+    k: 0.7
+    m: 0.95
+    temporal_step_offset_factor: 0.0
+    summed_pressure_threshold_right: 30
+    summed_pressure_threshold_left: 30
+    foot_pressure_topic_left: "foot_pressure_left/raw"
+    foot_pressure_topic_right: "foot_pressure_right/raw"
+    debug: true
+

bitbots_motion/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp

@@ -3,7 +3,7 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 #include <unistd.h>
-
+#include "odometry_parameters.hpp"
 #include <biped_interfaces/msg/phase.hpp>
 #include <bitbots_utils/utils.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -12,9 +12,8 @@
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <std_msgs/msg/char.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-
-#include "odometry_parameters.hpp"
-
+#include <unistd.h>
+#include <bitbots_msgs/msg/foot_pressure.hpp>
 using std::placeholders::_1;
 
 namespace bitbots_odometry {
@@ -52,6 +51,7 @@ class MotionOdometry : public rclcpp::Node {
   std::string previous_support_link_;
   std::string current_support_link_;
   rclcpp::Time start_time_;
+
 };
 
 }  // namespace bitbots_odometry

bitbots_motion/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h

@@ -0,0 +1,46 @@
+#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_left_sub_;
+  rclcpp::Subscription<bitbots_msgs::msg::FootPressure>::SharedPtr pressure_right_sub_;
+  rclcpp::Publisher<biped_interfaces::msg::Phase>::SharedPtr pub_foot_pressure_support_state_;
+
+  void pressure_left_callback(bitbots_msgs::msg::FootPressure msg);
+  void pressure_right_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_right_;
+  rclcpp::Time up_right_;
+
+  long step_duration_left_;
+  rclcpp::Time up_left_;
+  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_;
+
+  // if debug is true, publish a debug for summed pressure
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_summed_pressure_left_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_summed_pressure_right_;
+};
+
+}

bitbots_motion/bitbots_odometry/launch/odometry.launch

@@ -23,4 +23,19 @@
         <param name="cop_frame" value="$(var tf_prefix)cop"/>
         <param name="use_sim_time" value="$(var sim)"/>
     </node>
+
+    <group unless="$(var sim)">
+        <node name="walk_support_state_detector" pkg="bitbots_odometry" exec="walk_support_state_detector" launch-prefix="$(var taskset)">
+        <!-- for the normal walk_support_state_detector_default values are set and do not need to be loaded-->
+
+        </node>
+    </group>
+    <group if="$(var sim)">
+        <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)"/>
+            <param from="$(find-pkg-share bitbots_odometry)/config/odometry_config_sim.yaml"/>
+    </node>
+    </group>
+
+
 </launch>

bitbots_motion/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_motion/bitbots_odometry/src/motion_odometry.cpp

@@ -21,12 +21,20 @@ MotionOdometry::MotionOdometry() : Node("MotionOdometry"), param_listener_(get_n
   current_support_state_ = -1;
   previous_support_state_ = -1;
 
-  walk_support_state_sub_ = this->create_subscription<biped_interfaces::msg::Phase>(
-      "walk_support_state", 1, std::bind(&MotionOdometry::supportCallback, this, _1));
-  kick_support_state_sub_ = this->create_subscription<biped_interfaces::msg::Phase>(
-      "dynamic_kick_support_state", 1, std::bind(&MotionOdometry::supportCallback, this, _1));
-  odom_subscriber_ = this->create_subscription<nav_msgs::msg::Odometry>(
-      "walk_engine_odometry", 1, std::bind(&MotionOdometry::odomCallback, this, _1));
+  walk_support_state_sub_ =
+      this->create_subscription<biped_interfaces::msg::Phase>("foot_pressure/walk_support_state",
+                                                              1,
+                                                              std::bind(&MotionOdometry::supportCallback,
+                                                                        this, _1));
+  kick_support_state_sub_ =
+      this->create_subscription<biped_interfaces::msg::Phase>("dynamic_kick_support_state",
+                                                              1,
+                                                              std::bind(&MotionOdometry::supportCallback,
+                                                                        this, _1));
+  odom_subscriber_ =
+      this->create_subscription<nav_msgs::msg::Odometry>("walk_engine_odometry",
+                                                         1,
+                                                         std::bind(&MotionOdometry::odomCallback, this, _1));
 
   pub_odometry_ = this->create_publisher<nav_msgs::msg::Odometry>("motion_odometry", 1);
   // set the origin to 0. will be set correctly on recieving first support state
@@ -36,6 +44,9 @@ MotionOdometry::MotionOdometry() : Node("MotionOdometry"), param_listener_(get_n
   foot_change_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
   previous_support_link_ = r_sole_frame_;
   start_time_ = this->now();
+
+
+
 }
 
 void MotionOdometry::loop() {
@@ -154,7 +165,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;
@@ -183,7 +194,8 @@ void MotionOdometry::supportCallback(const biped_interfaces::msg::Phase::SharedP
 
 void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { current_odom_msg_ = *msg; }
 
-}  // namespace bitbots_odometry
+
+}
 
 int main(int argc, char **argv) {
   rclcpp::init(argc, argv);