diff --git a/bitbots_lowlevel/bitbots_ros_control/CMakeLists.txt b/bitbots_lowlevel/bitbots_ros_control/CMakeLists.txt index b8c4f68b0..e726580bd 100644 --- a/bitbots_lowlevel/bitbots_ros_control/CMakeLists.txt +++ b/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) diff --git a/bitbots_lowlevel/bitbots_ros_control/config/pressure_amy.yaml b/bitbots_lowlevel/bitbots_ros_control/config/pressure_amy.yaml index bc7d27ad3..578657c8c 100644 --- a/bitbots_lowlevel/bitbots_ros_control/config/pressure_amy.yaml +++ b/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 \ No newline at end of file + 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] diff --git a/bitbots_lowlevel/bitbots_ros_control/config/pressure_converter.yaml b/bitbots_lowlevel/bitbots_ros_control/config/pressure_converter.yaml deleted file mode 100644 index 750a7bb60..000000000 --- a/bitbots_lowlevel/bitbots_ros_control/config/pressure_converter.yaml +++ /dev/null @@ -1,7 +0,0 @@ -pressure_converter: - ros__parameters: - left_topic: "/foot_pressure_left" - right_topic: "/foot_pressure_right" - average: 5 - scale_and_zero_average: 50 - cop_threshold: 5.0 diff --git a/bitbots_lowlevel/bitbots_ros_control/config/pressure_default.yaml b/bitbots_lowlevel/bitbots_ros_control/config/pressure_default.yaml new file mode 100644 index 000000000..578657c8c --- /dev/null +++ b/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] diff --git a/bitbots_lowlevel/bitbots_ros_control/config/pressure_donna.yaml b/bitbots_lowlevel/bitbots_ros_control/config/pressure_donna.yaml index e69de29bb..578657c8c 100644 --- a/bitbots_lowlevel/bitbots_ros_control/config/pressure_donna.yaml +++ b/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] diff --git a/bitbots_lowlevel/bitbots_ros_control/config/pressure_jack.yaml b/bitbots_lowlevel/bitbots_ros_control/config/pressure_jack.yaml index e69de29bb..578657c8c 100644 --- a/bitbots_lowlevel/bitbots_ros_control/config/pressure_jack.yaml +++ b/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] diff --git a/bitbots_lowlevel/bitbots_ros_control/config/pressure_melody.yaml b/bitbots_lowlevel/bitbots_ros_control/config/pressure_melody.yaml index 473d24677..578657c8c 100644 --- a/bitbots_lowlevel/bitbots_ros_control/config/pressure_melody.yaml +++ b/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 \ No newline at end of file + 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] diff --git a/bitbots_lowlevel/bitbots_ros_control/config/pressure_nobot.yaml b/bitbots_lowlevel/bitbots_ros_control/config/pressure_nobot.yaml deleted file mode 100644 index b70902cfc..000000000 --- a/bitbots_lowlevel/bitbots_ros_control/config/pressure_nobot.yaml +++ /dev/null @@ -1,22 +0,0 @@ -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 \ No newline at end of file diff --git a/bitbots_lowlevel/bitbots_ros_control/config/pressure_rory.yaml b/bitbots_lowlevel/bitbots_ros_control/config/pressure_rory.yaml index b70902cfc..578657c8c 100644 --- a/bitbots_lowlevel/bitbots_ros_control/config/pressure_rory.yaml +++ b/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 \ No newline at end of file + 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] diff --git a/bitbots_lowlevel/bitbots_ros_control/config/pressure_template.yaml b/bitbots_lowlevel/bitbots_ros_control/config/pressure_template.yaml new file mode 100644 index 000000000..025bf4fea --- /dev/null +++ b/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] diff --git a/bitbots_lowlevel/bitbots_ros_control/config/wolfgang.yaml b/bitbots_lowlevel/bitbots_ros_control/config/wolfgang.yaml index a997be733..7627892f1 100644 --- a/bitbots_lowlevel/bitbots_ros_control/config/wolfgang.yaml +++ b/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 diff --git a/bitbots_lowlevel/bitbots_ros_control/include/bitbots_ros_control/pressure_converter.hpp b/bitbots_lowlevel/bitbots_ros_control/include/bitbots_ros_control/pressure_converter.hpp index 0e0cef13e..b6f35533f 100644 --- a/bitbots_lowlevel/bitbots_ros_control/include/bitbots_ros_control/pressure_converter.hpp +++ b/bitbots_lowlevel/bitbots_ros_control/include/bitbots_ros_control/pressure_converter.hpp @@ -1,6 +1,4 @@ #include -#include -#include #include #include @@ -14,33 +12,43 @@ #include #include +#include "pressure_converter_parameters.hpp" + +struct FootConfig { + std::string topic; + std::vector zero; + std::vector 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 wrench_topics_; + std::vector wrench_frames_; + std::array zero_, scale_; + std::array, 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::SharedPtr filtered_pub_; rclcpp::Publisher::SharedPtr cop_pub_; std::vector::SharedPtr> wrench_pubs_; rclcpp::Subscription::SharedPtr sub_; std::unique_ptr tf_broadcaster_; - std::vector wrench_frames_; - std::vector zero_, scale_; - std::vector> 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 request_; - rclcpp::Service::SharedPtr zero_service_; rclcpp::Service::SharedPtr scale_service_; @@ -51,5 +59,5 @@ class PressureConverter { bool scaleCallback(const std::shared_ptr req, std::shared_ptr resp); void collectMessages(); - void saveYAML(); + void showYAML(); }; diff --git a/bitbots_lowlevel/bitbots_ros_control/launch/pressure_converter.launch b/bitbots_lowlevel/bitbots_ros_control/launch/pressure_converter.launch index acd4f3068..04a832eca 100644 --- a/bitbots_lowlevel/bitbots_ros_control/launch/pressure_converter.launch +++ b/bitbots_lowlevel/bitbots_ros_control/launch/pressure_converter.launch @@ -2,8 +2,7 @@ - - + diff --git a/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch b/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch index 43ebbb957..266aac9a2 100644 --- a/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch +++ b/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch @@ -25,7 +25,7 @@ - + diff --git a/bitbots_lowlevel/bitbots_ros_control/package.xml b/bitbots_lowlevel/bitbots_ros_control/package.xml index 23abbeb60..df8b1f37a 100644 --- a/bitbots_lowlevel/bitbots_ros_control/package.xml +++ b/bitbots_lowlevel/bitbots_ros_control/package.xml @@ -23,12 +23,13 @@ bitbots_docs bitbots_msgs bitbots_robot_description + bitbots_tts bitbots_utils controller_interface controller_manager dynamixel_workbench_toolbox + generate_parameter_library hardware_interface - bitbots_tts pluginlib rclcpp realtime_tools diff --git a/bitbots_lowlevel/bitbots_ros_control/src/pressure_converter.cpp b/bitbots_lowlevel/bitbots_ros_control/src/pressure_converter.cpp index 1a929d88d..3c23f840a 100644 --- a/bitbots_lowlevel/bitbots_ros_control/src/pressure_converter.cpp +++ b/bitbots_lowlevel/bitbots_ros_control/src/pressure_converter.cpp @@ -2,116 +2,99 @@ using std::placeholders::_1; using std::placeholders::_2; -PressureConverter::PressureConverter(rclcpp::Node::SharedPtr nh, char side) { - nh_ = nh; - std::string topic; - rclcpp::CallbackGroup::SharedPtr sub_cbg_ = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - if (side == 'l') { - if (!nh_->has_parameter("left_topic")) - RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": left_topic not specified"); - topic = nh_->get_parameter("left_topic").as_string(); - } else if (side == 'r') { - if (!nh_->has_parameter("right_topic")) - RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": right_topic not specified"); - topic = nh_->get_parameter("right_topic").as_string(); - } - - if (!nh_->has_parameter("average")) - RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": average not specified"); - average_ = nh_->get_parameter("average").as_int(); - - if (!nh_->has_parameter("scale_and_zero_average")) - RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": scale_and_zero_average not specified"); - scale_and_zero_average_ = nh_->get_parameter("scale_and_zero_average").as_int(); - - scale_lr_ = "scale_"; - scale_lr_ += side; - zero_lr_ = "zero_"; - zero_lr_ += side; - cop_lr_ = "cop_"; - cop_lr_ += side; - sole_lr_ = side; - sole_lr_ += "_sole"; - - if (!nh_->has_parameter(zero_lr_)) { - RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": " << zero_lr_ << " not specified"); - zero_ = {0, 0, 0, 0}; - } else { - zero_ = nh_->get_parameter(zero_lr_).as_double_array(); - } - - if (!nh_->has_parameter(scale_lr_)) { - RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": " << scale_lr_ << " not specified"); - scale_ = {1, 1, 1, 1}; - } else { - scale_ = nh_->get_parameter(scale_lr_).as_double_array(); - } - if (!nh_->has_parameter("cop_threshold")) - RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": cop_threshold not specified"); - cop_threshold_ = nh_->get_parameter("cop_threshold").as_double(); - - side_ = side; - - // initialize vector for previous values for average calculations - for (int i = 0; i < 4; i++) { - std::vector x; - for (int j = 0; j < average_; j++) { - x.push_back(0.0); - } - previous_values_.push_back(x); - } - current_index_ = 0; - - save_zero_and_scale_values_ = false; - resetZeroAndScaleValues(); - - filtered_pub_ = nh_->create_publisher(topic + "/filtered", 1); - cop_pub_ = nh_->create_publisher("/" + cop_lr_, 1); - std::string wrench_topics[] = {"l_front", "l_back", "r_front", "r_back", "cop"}; - for (int i = 0; i < 5; i++) { - std::stringstream single_wrench_topic; - single_wrench_topic << topic << "/wrench/" << wrench_topics[i]; - wrench_pubs_.push_back(nh_->create_publisher(single_wrench_topic.str(), 1)); - } - for (int i = 0; i < 4; i++) { - std::stringstream single_wrench_frame; - single_wrench_frame << side << "_" - << "cleat_" << wrench_topics[i]; - wrench_frames_.push_back(single_wrench_frame.str()); - } - - scale_service_ = nh_->create_service( - topic + "/set_foot_scale", std::bind(&PressureConverter::scaleCallback, this, _1, _2)); - zero_service_ = nh_->create_service(topic + "/set_foot_zero", - std::bind(&PressureConverter::zeroCallback, this, _1, _2)); - rclcpp::SubscriptionOptions options; - rclcpp::QoS qos(0); - options.callback_group = sub_cbg_; - sub_ = nh_->create_subscription( - topic + "/raw", qos, std::bind(&PressureConverter::pressureCallback, this, _1), options); - tf_broadcaster_ = std::make_unique(*nh_); - - sub_executor_.add_callback_group(sub_cbg_, nh_->get_node_base_interface()); - sub_executor_thread_ = new std::thread([this]() { sub_executor_.spin(); }); -} +PressureConverter::PressureConverter(rclcpp::Node::SharedPtr nh, pressure_converter::Params::Common config, + FootConfig foot_config, char side) + : nh_(nh), + + side_(std::string(1, side)), + + wrench_topics_({"l_front", "l_back", "r_front", "r_back", "cop"}), + + wrench_frames_([this]() { + std::vector vec; + for (size_t i = 0; i < wrench_topics_.size(); i++) { + vec.push_back(side_ + "_cleat_" + wrench_topics_[i]); + } + return vec; + }()), + + zero_([foot_config]() { + // Copy first 4 elements of the config into an new array + std::array arr; + std::copy_n(foot_config.zero.begin(), 4, arr.begin()); + return arr; + }()), + + scale_([foot_config]() { + // Copy first 4 elements of the config into an new array + std::array arr; + std::copy_n(foot_config.scale.begin(), 4, arr.begin()); + return arr; + }()), + + previous_values_([config]() { + std::array, 4> arr; + for (auto &vec : arr) { + // Initialize vector with size elements, all set to 0 + vec.resize(config.average, 0); + } + return arr; + }()), + + average_(config.average), + + calibration_buffer_length_(config.calibration_buffer_length), + + cop_threshold_(config.cop_threshold), + + filtered_pub_(nh_->create_publisher(foot_config.topic + "/filtered", 1)), + + cop_pub_(nh_->create_publisher("/cop_" + side_, 1)), + + // Create wrench publishers + wrench_pubs_([this, foot_config]() { + std::vector::SharedPtr> vec; + for (size_t i = 0; i < wrench_topics_.size(); i++) { + vec.push_back(nh_->create_publisher( + foot_config.topic + "/wrench/" + wrench_topics_[i], 1)); + } + return vec; + }()), + + sub_([this, foot_config]() { + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + return nh_->create_subscription( + foot_config.topic + "/raw", 1, std::bind(&PressureConverter::pressureCallback, this, _1), options); + }()), + + tf_broadcaster_(std::make_unique(*nh_)), + + zero_service_(nh_->create_service( + foot_config.topic + "/set_foot_zero", std::bind(&PressureConverter::zeroCallback, this, _1, _2))), + + scale_service_(nh_->create_service( + foot_config.topic + "/set_foot_scale", std::bind(&PressureConverter::scaleCallback, this, _1, _2))) {} void PressureConverter::pressureCallback(bitbots_msgs::msg::FootPressure pressure_raw) { bitbots_msgs::msg::FootPressure filtered_msg; filtered_msg.header = pressure_raw.header; + // Add new values to our rolling buffer previous_values_[0][current_index_] = ((pressure_raw.left_front) - zero_[0]) * scale_[0]; previous_values_[1][current_index_] = ((pressure_raw.left_back) - zero_[1]) * scale_[1], previous_values_[2][current_index_] = ((pressure_raw.right_front) - zero_[2]) * scale_[2], previous_values_[3][current_index_] = ((pressure_raw.right_back) - zero_[3]) * scale_[3]; + current_index_ = (current_index_ + 1) % average_; + // Calculate the average of the rolling buffer filtered_msg.left_front = std::accumulate(previous_values_[0].begin(), previous_values_[0].end(), 0.0) / average_; filtered_msg.left_back = std::accumulate(previous_values_[1].begin(), previous_values_[1].end(), 0.0) / average_; filtered_msg.right_front = std::accumulate(previous_values_[2].begin(), previous_values_[2].end(), 0.0) / average_; filtered_msg.right_back = std::accumulate(previous_values_[3].begin(), previous_values_[3].end(), 0.0) / average_; - current_index_ = (current_index_ + 1) % average_; + // Clip to zero filtered_msg.left_front = std::max(filtered_msg.left_front, 0.0); filtered_msg.left_back = std::max(filtered_msg.left_back, 0.0); filtered_msg.right_front = std::max(filtered_msg.right_front, 0.0); @@ -127,17 +110,19 @@ void PressureConverter::pressureCallback(bitbots_msgs::msg::FootPressure pressur wrench_pubs_[i]->publish(w); } filtered_pub_->publish(filtered_msg); - if (save_zero_and_scale_values_) { - zero_and_scale_values_[0].push_back(pressure_raw.left_front); - zero_and_scale_values_[1].push_back(pressure_raw.left_back); - zero_and_scale_values_[2].push_back(pressure_raw.right_front); - zero_and_scale_values_[3].push_back(pressure_raw.right_back); + + // Store zero and scale values if we are in calibration mode + if (save_calibration_buffer_) { + calibration_buffer_[0].push_back(pressure_raw.left_front); + calibration_buffer_[1].push_back(pressure_raw.left_back); + calibration_buffer_[2].push_back(pressure_raw.right_front); + calibration_buffer_[3].push_back(pressure_raw.right_back); } - // publish center of pressure + // Publish center of pressure double pos_x = 0.085, pos_y = 0.045; geometry_msgs::msg::PointStamped cop; - cop.header.frame_id = sole_lr_; + cop.header.frame_id = side_ + "_sole"; cop.header.stamp = pressure_raw.header.stamp; double sum_of_forces = filtered_msg.left_front + filtered_msg.left_back + filtered_msg.right_back + filtered_msg.right_front; @@ -159,89 +144,64 @@ void PressureConverter::pressureCallback(bitbots_msgs::msg::FootPressure pressur geometry_msgs::msg::TransformStamped cop_tf; cop_tf.header = cop.header; - cop_tf.child_frame_id = cop_lr_; + cop_tf.child_frame_id = "cop_" + side_; cop_tf.transform.translation.x = cop.point.x; cop_tf.transform.translation.y = cop.point.y; cop_tf.transform.rotation.w = 1; tf_broadcaster_->sendTransform(cop_tf); geometry_msgs::msg::WrenchStamped w_cop; - w_cop.header.frame_id = cop_lr_; + w_cop.header.frame_id = "cop_" + side_; w_cop.header.stamp = pressure_raw.header.stamp; w_cop.wrench.force.z = sum_of_forces; wrench_pubs_[4]->publish(w_cop); } void PressureConverter::resetZeroAndScaleValues() { - zero_and_scale_values_.clear(); - for (int i = 0; i < 4; i++) { - std::vector x; - zero_and_scale_values_.push_back(x); + // Reset calibration buffer + for (auto &cleat : calibration_buffer_) { + cleat.clear(); } } void PressureConverter::collectMessages() { - save_zero_and_scale_values_ = true; - while (int(zero_and_scale_values_[0].size()) < scale_and_zero_average_) { - RCLCPP_WARN_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 0.25, "%ld of %d msgs", zero_and_scale_values_[0].size(), - scale_and_zero_average_); + save_calibration_buffer_ = true; + while (int(calibration_buffer_[0].size()) < calibration_buffer_length_) { + RCLCPP_WARN_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 20, "%ld of %d msgs", calibration_buffer_[0].size(), + calibration_buffer_length_); } - save_zero_and_scale_values_ = false; + save_calibration_buffer_ = false; } -void PressureConverter::saveYAML() { - std::string path_to_yaml, robot_name; - if (std::getenv("ROBOT_NAME")) { - robot_name = std::getenv("ROBOT_NAME"); - } else { - robot_name = "nobot"; +void PressureConverter::showYAML() { + std::string e = "zero: ["; + for (int i = 0; i < 4; i++) { + e += std::to_string(zero_[i]) + ", "; } - path_to_yaml = - ament_index_cpp::get_package_share_directory("bitbots_ros_control") + "/config/pressure_" + robot_name + ".yaml"; - - std::vector scale_r, scale_l, zero_r, zero_l; - zero_r = nh_->get_parameter("zero_r").as_double_array(); - zero_l = nh_->get_parameter("zero_l").as_double_array(); - scale_r = nh_->get_parameter("scale_r").as_double_array(); - scale_l = nh_->get_parameter("scale_l").as_double_array(); - - YAML::Emitter e; - e << YAML::BeginMap; - e << YAML::Key << "pressure_converter"; - e << YAML::BeginMap; - e << YAML::Key << "ros__parameters"; - e << YAML::BeginMap; - e << YAML::Key << "zero_r"; - e << YAML::Value << zero_r; - e << YAML::Key << "zero_l"; - e << YAML::Value << zero_l; - e << YAML::Key << "scale_r"; - e << YAML::Value << scale_r; - e << YAML::Key << "scale_l"; - e << YAML::Value << scale_l; - e << YAML::EndMap; - e << YAML::EndMap; - - std::ofstream yaml_file; - yaml_file.open(path_to_yaml); - yaml_file << e.c_str(); - yaml_file.close(); + e += "]\nscale: ["; + for (int i = 0; i < 4; i++) { + e += std::to_string(scale_[i]) + ", "; + } + e += "]"; + + RCLCPP_INFO_STREAM(nh_->get_logger(), + "The following calibration values are the new calibration values:" << std::endl + << e.c_str()); } bool PressureConverter::scaleCallback(const std::shared_ptr req, std::shared_ptr resp) { collectMessages(); double average = - std::accumulate(zero_and_scale_values_[req->sensor].begin(), zero_and_scale_values_[req->sensor].end(), 0.0) / - zero_and_scale_values_[req->sensor].size(); + std::accumulate(calibration_buffer_[req->sensor].begin(), calibration_buffer_[req->sensor].end(), 0.0) / + calibration_buffer_[req->sensor].size(); RCLCPP_WARN_STREAM(nh_->get_logger(), "avg: " << average); average -= zero_[req->sensor]; RCLCPP_WARN_STREAM(nh_->get_logger(), "avg_after: " << average); scale_[req->sensor] = req->weight / average; resetZeroAndScaleValues(); - nh_->set_parameter(rclcpp::Parameter(scale_lr_, rclcpp::ParameterValue(scale_))); - saveYAML(); + showYAML(); return true; } @@ -249,26 +209,29 @@ bool PressureConverter::zeroCallback(const std::shared_ptr resp) { collectMessages(); for (int i = 0; i < 4; i++) { - zero_[i] = std::accumulate(zero_and_scale_values_[i].begin(), zero_and_scale_values_[i].end(), 0.0) / - zero_and_scale_values_[i].size(); + zero_[i] = std::accumulate(calibration_buffer_[i].begin(), calibration_buffer_[i].end(), 0.0) / + calibration_buffer_[i].size(); } resetZeroAndScaleValues(); - nh_->set_parameter(rclcpp::Parameter(zero_lr_, rclcpp::ParameterValue(zero_))); - saveYAML(); + showYAML(); return true; } int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - // declare parameters automatically - rclcpp::NodeOptions options = rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true); - rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("pressure_converter", options); - rclcpp::executors::StaticSingleThreadedExecutor executor; + // Create node and executor + rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("pressure_converter"); + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 4); executor.add_node(nh); - PressureConverter r(nh, 'r'); - PressureConverter l(nh, 'l'); + // Get config + auto param_listener = pressure_converter::ParamListener(nh); + auto config = param_listener.get_params(); + + // Pass config to PressureConverters + PressureConverter r(nh, config.common, FootConfig(config.right), 'r'); + PressureConverter l(nh, config.common, FootConfig(config.left), 'l'); executor.spin(); diff --git a/bitbots_motion/bitbots_support_state_detector/CMakeLists.txt b/bitbots_motion/bitbots_support_state_detector/CMakeLists.txt new file mode 100644 index 000000000..f4d4f9095 --- /dev/null +++ b/bitbots_motion/bitbots_support_state_detector/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.5) +project(bitbots_support_state_detector) + +# Add support for C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +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(generate_parameter_library REQUIRED) +find_package(rclcpp REQUIRED) + +add_compile_options(-Wall -Werror -Wno-unused) + +generate_parameter_library(support_state_detector_parameters + config/support_state_detector_template.yaml) + +add_executable(support_state_detector src/support_state_detector.cpp) + +target_link_libraries(support_state_detector rclcpp::rclcpp + support_state_detector_parameters) + +ament_target_dependencies(support_state_detector biped_interfaces bitbots_msgs + generate_parameter_library std_msgs) + +enable_bitbots_docs() + +install(TARGETS support_state_detector DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/bitbots_motion/bitbots_support_state_detector/config/support_state_detector_sim.yaml b/bitbots_motion/bitbots_support_state_detector/config/support_state_detector_sim.yaml new file mode 100644 index 000000000..4ca718fb6 --- /dev/null +++ b/bitbots_motion/bitbots_support_state_detector/config/support_state_detector_sim.yaml @@ -0,0 +1,9 @@ +support_state_detector: + ros__parameters: + k: 0.7 + 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 + \ No newline at end of file diff --git a/bitbots_motion/bitbots_support_state_detector/config/support_state_detector_template.yaml b/bitbots_motion/bitbots_support_state_detector/config/support_state_detector_template.yaml new file mode 100644 index 000000000..c9046cdef --- /dev/null +++ b/bitbots_motion/bitbots_support_state_detector/config/support_state_detector_template.yaml @@ -0,0 +1,45 @@ +support_state_detector: + k: { + type: double, + default_value: 0.97, + description: "Factor for lowpass filtering the foot pressure", + 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: "foot_pressure_left/filtered", + 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" + } diff --git a/bitbots_motion/bitbots_support_state_detector/docs/_static/logo.png b/bitbots_motion/bitbots_support_state_detector/docs/_static/logo.png new file mode 100644 index 000000000..f8afdd5d0 Binary files /dev/null and b/bitbots_motion/bitbots_support_state_detector/docs/_static/logo.png differ diff --git a/bitbots_motion/bitbots_support_state_detector/docs/conf.py b/bitbots_motion/bitbots_support_state_detector/docs/conf.py new file mode 100644 index 000000000..0b6342a9c --- /dev/null +++ b/bitbots_motion/bitbots_support_state_detector/docs/conf.py @@ -0,0 +1,187 @@ +# +# Full list of options at http://www.sphinx-doc.org/en/master/config + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +import os +import sys + +import catkin_pkg.package +from exhale import utils + +package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +catkin_package = catkin_pkg.package.parse_package( + os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME) +) +sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) + + +# -- Helper functions -------------------------------------------------------- + + +def count_files(): + """:returns tuple of (num_py, num_cpp)""" + num_py = 0 + num_cpp = 0 + + for _, _, files in os.walk(os.path.join(package_dir, "src")): + for f in files: + if f.endswith(".py"): + num_py += 1 + for _, _, files in os.walk(os.path.join(package_dir, "include")): + for f in files: + if f.endswith(".h") or f.endswith(".hpp"): + num_cpp += 1 + + return num_py, num_cpp + + +# -- Project information ----------------------------------------------------- + +project = catkin_package.name +copyright = "2019, Bit-Bots" +author = ", ".join([a.name for a in catkin_package.authors]) + +# The short X.Y version +version = str(catkin_package.version) +# The full version, including alpha/beta/rc tags +release = str(catkin_package.version) + +# -- General configuration --------------------------------------------------- + +# If your documentation needs a minimal Sphinx version, state it here. +# +# needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + "sphinx.ext.autodoc", + "sphinx.ext.doctest", + "sphinx.ext.intersphinx", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.imgmath", + "sphinx.ext.viewcode", + "sphinx_rtd_theme", +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ["_templates"] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# +# source_suffix = ['.rst', '.md'] +source_suffix = ".rst" + +# The master toctree document. +master_doc = "index" + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = None + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = None + +# -- Exhale and Breath setup ------------------------------------------------- + +# Tell sphinx what the primary language being documented is. +num_files_py, num_files_cpp = count_files() +primary_domain = "py" if num_files_py >= num_files_cpp else "cpp" + +# Tell sphinx what the pygments highlight language should be. +highlight_language = primary_domain + +if num_files_cpp > 0: + extensions += [ + "breathe", + "exhale", + ] + + breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} + breathe_default_project = project + + def specifications_for_kind(kind): + # Show all members for classes and structs + if kind == "class" or kind == "struct": + return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] + # An empty list signals to Exhale to use the defaults + else: + return [] + + exhale_args = { + # These arguments are required + "containmentFolder": "cppapi", + "rootFileName": "library_root.rst", + "rootFileTitle": "C++ Library API", + "doxygenStripFromPath": "..", + "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), + # Suggested optional arguments + "createTreeView": True, + "exhaleExecutesDoxygen": True, + "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), + } + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = "sphinx_rtd_theme" + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# +# html_theme_options = {} + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ["_static"] + +# Custom sidebar templates, must be a dictionary that maps document names +# to template names. +# +# The default sidebars (for documents that don't match any pattern) are +# defined by theme itself. Builtin themes are using these templates by +# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', +# 'searchbox.html']``. +# +# html_sidebars = {} + +html_logo = os.path.join("_static", "logo.png") +html_favicon = os.path.join("_static", "logo.png") + + +# -- Options for intersphinx extension --------------------------------------- + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {"https://docs.python.org/": None} + +# -- Options for todo extension ---------------------------------------------- + +# If true, `todo` and `todoList` produce output, else they produce nothing. +todo_include_todos = True + +# -- RST Standard variables --------------------------------------------------- +rst_prolog = f".. |project| replace:: {project}\n" +rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) +rst_prolog += ".. |modindex| replace:: {}\n".format( + ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" +) diff --git a/bitbots_motion/bitbots_support_state_detector/docs/index.rst b/bitbots_motion/bitbots_support_state_detector/docs/index.rst new file mode 100644 index 000000000..e76aa433a --- /dev/null +++ b/bitbots_motion/bitbots_support_state_detector/docs/index.rst @@ -0,0 +1,21 @@ +Welcome to |project|'s documentation! +================================================ + +Description +----------- + +|description| + +.. toctree:: + :maxdepth: 2 + + cppapi/library_root + pyapi/modules + + +Indices and tables +================== + +* :ref:`genindex` +* |modindex| +* :ref:`search` diff --git a/bitbots_motion/bitbots_support_state_detector/launch/detector.launch b/bitbots_motion/bitbots_support_state_detector/launch/detector.launch new file mode 100644 index 000000000..7a7d41cfa --- /dev/null +++ b/bitbots_motion/bitbots_support_state_detector/launch/detector.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/bitbots_motion/bitbots_support_state_detector/package.xml b/bitbots_motion/bitbots_support_state_detector/package.xml new file mode 100644 index 000000000..01f6ac2a4 --- /dev/null +++ b/bitbots_motion/bitbots_support_state_detector/package.xml @@ -0,0 +1,31 @@ + + + + bitbots_support_state_detector + 1.0.13 + The bitbots_support_state_detector package. It is used to detect the support state of the robot (meaning which foot is on the ground). + + Florian Vahl + + MIT + + Florian Vahl + + ament_cmake + biped_interfaces + bitbots_docs + bitbots_msgs + bitbots_utils + rclcpp + generate_parameter_library + + + + tested_robot + c++ + + ament_cmake + + + ament_cmake + diff --git a/bitbots_motion/bitbots_support_state_detector/src/support_state_detector.cpp b/bitbots_motion/bitbots_support_state_detector/src/support_state_detector.cpp new file mode 100644 index 000000000..f8642fa79 --- /dev/null +++ b/bitbots_motion/bitbots_support_state_detector/src/support_state_detector.cpp @@ -0,0 +1,116 @@ +#include +#include +#include +#include + +#include "support_state_detector_parameters.hpp" + +using std::placeholders::_1; + +class SupportStateDetector : public rclcpp::Node { + public: + SupportStateDetector() + : Node("SupportStateDetector"), + param_listener_(get_node_parameters_interface()), + config_(param_listener_.get_params()), + pressure_left_sub_(this->create_subscription( + config_.foot_pressure_topic_left, 1, std::bind(&SupportStateDetector::pressure_left_callback, this, _1))), + pressure_right_sub_(this->create_subscription( + config_.foot_pressure_topic_right, 1, std::bind(&SupportStateDetector::pressure_right_callback, this, _1))), + pub_foot_pressure_support_state_( + this->create_publisher("foot_pressure/walk_support_state", 1)), + pub_summed_pressure_left_( + this->create_publisher("foot_pressure/summed_pressure_left", 1)), + pub_summed_pressure_right_( + this->create_publisher("foot_pressure/summed_pressure_right", 1)) { + curr_stance_.phase = biped_interfaces::msg::Phase::DOUBLE_STANCE; + } + + void loop() { + // Update parameters if they have changed + if (param_listener_.is_old(config_)) { + config_ = param_listener_.get_params(); + } + + // Build the phase message based on the current stance + biped_interfaces::msg::Phase phase; + if (curr_stand_left_ and !curr_stand_right_) { + phase.phase = biped_interfaces::msg::Phase::LEFT_STANCE; + } else if (!curr_stand_left_ and curr_stand_right_) { + phase.phase = biped_interfaces::msg::Phase::RIGHT_STANCE; + } else { // if both are high its double support, but if both are too low, pressure is shared on both feet + phase.phase = biped_interfaces::msg::Phase::DOUBLE_STANCE; + } + + // Only send message if phase has changed + if (phase.phase != curr_stance_.phase) { + // Add timestamp to message + phase.header.stamp = this->now(); + // Store current phase + curr_stance_.phase = phase.phase; + // Publish phase + pub_foot_pressure_support_state_->publish(phase); + } + } + + private: + void pressure_left_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_; + + curr_stand_left_ = pressure_filtered_left_ > config_.summed_pressure_threshold_left; + + if (config_.debug) { + std_msgs::msg::Float64 pressure_msg; + pressure_msg.data = pressure_filtered_left_; + pub_summed_pressure_left_->publish(pressure_msg); + } + } + + void pressure_right_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_; + + curr_stand_right_ = pressure_filtered_right_ > config_.summed_pressure_threshold_right; + + if (config_.debug) { + std_msgs::msg::Float64 pressure_msg; + pressure_msg.data = pressure_filtered_right_; + pub_summed_pressure_right_->publish(pressure_msg); + } + } + + // Declare parameter listener and struct from the generate_parameter_library + support_state_detector::ParamListener param_listener_; + support_state_detector::Params config_; + + // Declare subscriptions and publishers + rclcpp::Subscription::SharedPtr pressure_left_sub_; + rclcpp::Subscription::SharedPtr pressure_right_sub_; + rclcpp::Publisher::SharedPtr pub_foot_pressure_support_state_; + // if debug is true, publish a debug for summed pressure + rclcpp::Publisher::SharedPtr pub_summed_pressure_left_; + rclcpp::Publisher::SharedPtr pub_summed_pressure_right_; + + // Declare variables + bool curr_stand_left_ = false; + bool curr_stand_right_ = false; + float_t pressure_filtered_left_ = 0; + float_t pressure_filtered_right_ = 0; + biped_interfaces::msg::Phase curr_stance_; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + rclcpp::Duration timer_duration = rclcpp::Duration::from_seconds(1.0 / 500.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(); +} diff --git a/bitbots_navigation/bitbots_odometry/CMakeLists.txt b/bitbots_navigation/bitbots_odometry/CMakeLists.txt index c235c8dfd..2476246c9 100644 --- a/bitbots_navigation/bitbots_odometry/CMakeLists.txt +++ b/bitbots_navigation/bitbots_odometry/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rot_conv REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -40,6 +41,7 @@ ament_target_dependencies( rclcpp rot_conv sensor_msgs + std_msgs tf2 tf2_eigen tf2_geometry_msgs diff --git a/bitbots_navigation/bitbots_odometry/package.xml b/bitbots_navigation/bitbots_odometry/package.xml index 38270700a..6edade884 100644 --- a/bitbots_navigation/bitbots_odometry/package.xml +++ b/bitbots_navigation/bitbots_odometry/package.xml @@ -21,8 +21,10 @@ nav_msgs rot_conv sensor_msgs + std_msgs tf2_eigen tf2_geometry_msgs + tf2_ros tf2