Use foot sensor to detect steps / rework pressure converter.

bitbots_lowlevel/bitbots_ros_control/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(controller_interface REQUIRED)
 find_package(controller_manager REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(dynamixel_workbench_toolbox REQUIRED)
+find_package(generate_parameter_library REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -26,8 +27,7 @@ find_package(tf2_ros REQUIRED)
 find_package(transmission_interface REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
-set(INCLUDE_DIRS include)
-include_directories(${INCLUDE_DIRS})
+include_directories(include)
 
 set(CMAKE_CXX_STANDARD 17)
 add_compile_options(-Wall -Werror -Wno-unused -pedantic -Wextra)
@@ -70,6 +70,8 @@ ament_target_dependencies(
   transmission_interface
   yaml-cpp)
 
+generate_parameter_library(pressure_converter_parameters
+                           config/pressure_template.yaml)
 add_executable(pressure_converter src/pressure_converter.cpp)
 ament_target_dependencies(
   pressure_converter
@@ -90,9 +92,9 @@ ament_target_dependencies(
   std_msgs
   std_srvs
   tf2_ros
-  transmission_interface
-  yaml-cpp)
-target_link_libraries(pressure_converter yaml-cpp)
+  transmission_interface)
+target_link_libraries(pressure_converter rclcpp::rclcpp
+                      pressure_converter_parameters)
 
 ament_export_dependencies(ament_cmake)
 ament_export_dependencies(bitbots_buttons)

bitbots_lowlevel/bitbots_ros_control/config/pressure_amy.yaml

@@ -1,22 +1,8 @@
 pressure_converter:
   ros__parameters:
-    zero_r:
-      - -27455372.92
-      - 14459386.46
-      - 17952654.76
-      - -10605254.74
-    zero_l:
-      - -14063383.6
-      - 30331751.92
-      - -2034247.26
-      - 48130515.2
-    scale_r:
-      - 2.879445523352348e-06
-      - -2.252145560713656e-05
-      - -2.754225471994077e-06
-      - 2.763926644524643e-06
-    scale_l:
-      - 2.879445523352348e-06
-      - -2.252145560713656e-05
-      - -2.754225471994077e-06
-      - 2.763926644524643e-06
+    right:
+      zero: [48308998.54257341, -55854838.35192279, 98076369.19309235, -39246077.40848633]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]
+    left:
+      zero: [-67084141.6669952, 56469139.33715566, -85340727.53204206, -6584181.771324467]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]

bitbots_lowlevel/bitbots_ros_control/config/pressure_default.yaml

@@ -0,0 +1,8 @@
+pressure_converter:
+  ros__parameters:
+    right:
+      zero: [48308998.54257341, -55854838.35192279, 98076369.19309235, -39246077.40848633]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]
+    left:
+      zero: [-67084141.6669952, 56469139.33715566, -85340727.53204206, -6584181.771324467]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]

bitbots_lowlevel/bitbots_ros_control/config/pressure_donna.yaml

@@ -0,0 +1,8 @@
+pressure_converter:
+  ros__parameters:
+    right:
+      zero: [48308998.54257341, -55854838.35192279, 98076369.19309235, -39246077.40848633]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]
+    left:
+      zero: [-67084141.6669952, 56469139.33715566, -85340727.53204206, -6584181.771324467]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]

bitbots_lowlevel/bitbots_ros_control/config/pressure_jack.yaml

@@ -0,0 +1,8 @@
+pressure_converter:
+  ros__parameters:
+    right:
+      zero: [48308998.54257341, -55854838.35192279, 98076369.19309235, -39246077.40848633]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]
+    left:
+      zero: [-67084141.6669952, 56469139.33715566, -85340727.53204206, -6584181.771324467]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]

bitbots_lowlevel/bitbots_ros_control/config/pressure_melody.yaml

@@ -1,22 +1,8 @@
 pressure_converter:
   ros__parameters:
-    zero_r:
-    - -27455372.92
-    - 14459386.46
-    - 17952654.76
-    - -10605254.74
-    zero_l:
-      - -14063383.6
-      - 30331751.92
-      - -2034247.26
-      - 48130515.2
-    scale_r:
-      - 2.879445523352348e-06
-      - -2.252145560713656e-05
-      - -2.754225471994077e-06
-      - 2.763926644524643e-06
-    scale_l:
-      - 2.879445523352348e-06
-      - -2.252145560713656e-05
-      - -2.754225471994077e-06
-      - 2.763926644524643e-06
+    right:
+      zero: [48308998.54257341, -55854838.35192279, 98076369.19309235, -39246077.40848633]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]
+    left:
+      zero: [-67084141.6669952, 56469139.33715566, -85340727.53204206, -6584181.771324467]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]

bitbots_lowlevel/bitbots_ros_control/config/pressure_rory.yaml

@@ -1,22 +1,8 @@
 pressure_converter:
   ros__parameters:
-    zero_r:
-      - 48308998.54257341
-      - -55854838.35192279
-      - 98076369.19309235
-      - -39246077.40848633
-    zero_l:
-      - -67084141.6669952
-      - 56469139.33715566
-      - -85340727.53204206
-      - -6584181.771324467
-    scale_r:
-      - -5.719128286267938e-08
-      - -3.441808570613519e-08
-      - 3.580081515665939e-08
-      - 3.67922464123119e-08
-    scale_l:
-      - -5.719128286267938e-08
-      - -3.441808570613519e-08
-      - 3.580081515665939e-08
-      - 3.67922464123119e-08
+    right:
+      zero: [48308998.54257341, -55854838.35192279, 98076369.19309235, -39246077.40848633]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]
+    left:
+      zero: [-67084141.6669952, 56469139.33715566, -85340727.53204206, -6584181.771324467]
+      scale: [-5.719128286267938e-08, -3.441808570613519e-08, 3.580081515665939e-08, 3.67922464123119e-08]

bitbots_lowlevel/bitbots_ros_control/config/pressure_template.yaml

@@ -0,0 +1,59 @@
+pressure_converter:
+  common:
+    average:
+      type: int
+      description: "Number of values to average over."
+      read_only: true
+      default_value: 5
+      validation:
+        gt<>: [0]
+    calibration_buffer_length:
+      type: int
+      description: "Number of values to average over for the calibration of the scale and zero."
+      read_only: true
+      default_value: 50
+      validation:
+        gt<>: [0]
+    cop_threshold:
+      type: double
+      description: "Minimum value for the center of pressure to be considered valid."
+      read_only: true
+      default_value: 5.0
+      validation:
+        gt_eq<>: [0.0]
+  right:
+    topic:
+      type: string
+      description: "Topic for the right foot pressure sensor."
+      read_only: true
+      default_value: "/foot_pressure_right"
+    zero:
+      type: double_array
+      description: "Zero values for the right foot pressure sensor."
+      read_only: true
+      validation:
+        fixed_size<>: [4]
+    scale:
+      type: double_array
+      description: "Scale values for the right foot pressure sensor."
+      read_only: true
+      validation:
+        fixed_size<>: [4]
+  left:
+    topic:
+      type: string
+      description: "Topic for the left foot pressure sensor."
+      read_only: true
+      default_value: "/foot_pressure_left"
+    zero:
+      type: double_array
+      description: "Zero values for the left foot pressure sensor."
+      read_only: true
+      validation:
+        fixed_size<>: [4]
+    scale:
+      type: double_array
+      description: "Scale values for the left foot pressure sensor."
+      read_only: true
+      validation:
+        fixed_size<>: [4]

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/include/bitbots_ros_control/pressure_converter.hpp

@@ -1,6 +1,4 @@
 #include <tf2_ros/transform_broadcaster.h>
-#include <yaml-cpp/emitter.h>
-#include <yaml-cpp/yaml.h>
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <bitbots_msgs/msg/foot_pressure.hpp>
@@ -14,33 +12,43 @@
 #include <rclcpp/rclcpp.hpp>
 #include <std_srvs/srv/empty.hpp>
 
+#include "pressure_converter_parameters.hpp"
+
+struct FootConfig {
+  std::string topic;
+  std::vector<double> zero;
+  std::vector<double> scale;
+  explicit FootConfig(const pressure_converter::Params::Left &foot_config)
+      : topic(foot_config.topic), zero(foot_config.zero), scale(foot_config.scale) {}
+  explicit FootConfig(const pressure_converter::Params::Right &foot_config)
+      : topic(foot_config.topic), zero(foot_config.zero), scale(foot_config.scale) {}
+};
+
 class PressureConverter {
  public:
-  PressureConverter(rclcpp::Node::SharedPtr nh, char side);
+  PressureConverter(rclcpp::Node::SharedPtr nh, pressure_converter::Params::Common config, FootConfig foot_config,
+                    char side);
 
  private:
   rclcpp::Node::SharedPtr nh_;
-  rclcpp::executors::StaticSingleThreadedExecutor sub_executor_;
-  rclcpp::CallbackGroup::SharedPtr sub_cbg_;
-  std::thread* sub_executor_thread_;
+
+  const std::string side_;
+  const std::vector<std::string> wrench_topics_;
+  std::vector<std::string> wrench_frames_;
+  std::array<double, 4> zero_, scale_;
+  std::array<std::vector<double>, 4> previous_values_, calibration_buffer_;
+  bool save_calibration_buffer_ = false;
+  int current_index_ = 0;
+  const int average_, calibration_buffer_length_;
+  const double cop_threshold_;
+
+  // Create publisher and subscriber
   rclcpp::Publisher<bitbots_msgs::msg::FootPressure>::SharedPtr filtered_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr cop_pub_;
   std::vector<rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr> wrench_pubs_;
   rclcpp::Subscription<bitbots_msgs::msg::FootPressure>::SharedPtr sub_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
 
-  std::vector<std::string> wrench_frames_;
-  std::vector<double> zero_, scale_;
-  std::vector<std::vector<double>> previous_values_, zero_and_scale_values_;
-  bool save_zero_and_scale_values_;
-  int current_index_;
-  int average_, scale_and_zero_average_;
-  double cop_threshold_;
-  char side_;
-  std::string req_type_;
-  std::string scale_lr_, zero_lr_, cop_lr_, sole_lr_;
-  std::shared_ptr<bitbots_msgs::srv::FootScale::Request> request_;
-
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr zero_service_;
   rclcpp::Service<bitbots_msgs::srv::FootScale>::SharedPtr scale_service_;
 
@@ -51,5 +59,5 @@ class PressureConverter {
   bool scaleCallback(const std::shared_ptr<bitbots_msgs::srv::FootScale::Request> req,
                      std::shared_ptr<bitbots_msgs::srv::FootScale::Response> resp);
   void collectMessages();
-  void saveYAML();
+  void showYAML();
 };

bitbots_lowlevel/bitbots_ros_control/launch/pressure_converter.launch

@@ -2,8 +2,7 @@
 <launch>
 
     <node pkg="bitbots_ros_control" exec="pressure_converter">
-        <param from="$(find-pkg-share bitbots_ros_control)/config/pressure_converter.yaml" />
-        <param from="$(find-pkg-share bitbots_ros_control)/config/pressure_$(env ROBOT_NAME nobot).yaml" />
+        <param from="$(find-pkg-share bitbots_ros_control)/config/pressure_$(env ROBOT_NAME default).yaml" />
     </node>
 
 </launch>

bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch

@@ -25,7 +25,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 bitbots_tts)/launch/tts.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_lowlevel/bitbots_ros_control/package.xml

@@ -23,12 +23,13 @@
   <depend>bitbots_docs</depend>
   <depend>bitbots_msgs</depend>
   <depend>bitbots_robot_description</depend>
+  <depend>bitbots_tts</depend>
   <depend>bitbots_utils</depend>
   <depend>controller_interface</depend>
   <depend>controller_manager</depend>
   <depend>dynamixel_workbench_toolbox</depend>
+  <depend>generate_parameter_library</depend>
   <depend>hardware_interface</depend>
-  <depend>bitbots_tts</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>realtime_tools</depend>