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>