move step_detector from old repo to mono

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,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_motion/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp

@@ -12,9 +12,12 @@
 #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 <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+#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 {
@@ -52,6 +55,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,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_motion/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_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);

bitbots_motion/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();
+}