Skip to content

Commit

Permalink
write out left and right
Browse files Browse the repository at this point in the history
  • Loading branch information
@val-ba
val-ba committed Feb 5, 2024
1 parent c53f5eb commit 353e9c4
Show file tree
Hide file tree
Showing 2 changed files with 32 additions and 28 deletions.
20 changes: 12 additions & 8 deletions bitbots_motion/bitbots_odometry/include/bitbots_odometry/walk_support_state_detector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,30 +13,34 @@ class WalkSupportStateDetector: public rclcpp::Node {
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::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_l_callback(bitbots_msgs::msg::FootPressure msg);
void pressure_r_callback(bitbots_msgs::msg::FootPressure msg);
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_r_;
rclcpp::Time up_r_;
long step_duration_right_;
rclcpp::Time up_right_;

long step_duration_l_;
rclcpp::Time up_l_;
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_;
};

}
40 changes: 20 additions & 20 deletions bitbots_motion/bitbots_odometry/src/walk_support_state_detector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,24 +8,24 @@ param_listener_(get_node_parameters_interface())

// if sim use raw, if not sim use filtered

pressure_l_sub_ = this->create_subscription<bitbots_msgs::msg::FootPressure>(
config_.foot_pressure_topic_left, 1, std::bind(&WalkSupportStateDetector::pressure_l_callback, this, _1));
pressure_r_sub_ = this->create_subscription<bitbots_msgs::msg::FootPressure>(
config_.foot_pressure_topic_right, 1, std::bind(&WalkSupportStateDetector::pressure_r_callback, this, _1));
pressure_left_sub_ = this->create_subscription<bitbots_msgs::msg::FootPressure>(
config_.foot_pressure_topic_left, 1, std::bind(&WalkSupportStateDetector::pressure_left_callback, this, _1));
pressure_right_sub_ = this->create_subscription<bitbots_msgs::msg::FootPressure>(
config_.foot_pressure_topic_right, 1, std::bind(&WalkSupportStateDetector::pressure_right_callback, this, _1));
pub_foot_pressure_support_state_ = this->create_publisher<biped_interfaces::msg::Phase>("foot_pressure/walk_support_state", 1);

// if debug, publish a debug for summed pressure
if (config_.debug){
pub_summed_pressure_l_ = this->create_publisher<std_msgs::msg::Float64>("foot_pressure/summed_pressure_left", 1);
pub_summed_pressure_r_ = this->create_publisher<std_msgs::msg::Float64>("foot_pressure/summed_pressure_right", 1);
pub_summed_pressure_left_ = this->create_publisher<std_msgs::msg::Float64>("foot_pressure/summed_pressure_left", 1);
pub_summed_pressure_right_ = this->create_publisher<std_msgs::msg::Float64>("foot_pressure/summed_pressure_right", 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();
step_duration_right_ = 0;
up_right_ = this->now();
step_duration_left_ = 0;
up_left_ = this->now();

}

Expand All @@ -41,10 +41,10 @@ void WalkSupportStateDetector::loop() {
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()){
else if (curr_stand_left_ && ! curr_stand_right_ && (up_right_ + rclcpp::Duration::from_nanoseconds(int(config_.temporal_step_offset*step_duration_right_))) < this->now()){
phase.phase = 0;
}
else if (!curr_stand_left_ && curr_stand_right_ &&(up_l_ + rclcpp::Duration::from_nanoseconds(int(config_.temporal_step_offset*step_duration_l_))) < this->now()){
else if (!curr_stand_left_ && curr_stand_right_ &&(up_left_ + rclcpp::Duration::from_nanoseconds(int(config_.temporal_step_offset*step_duration_left_))) < this->now()){
phase.phase = 1;
}
if (phase.phase != curr_stance_.phase){
Expand All @@ -56,48 +56,48 @@ void WalkSupportStateDetector::loop() {

}

void WalkSupportStateDetector::pressure_l_callback(bitbots_msgs::msg::FootPressure msg) {
void WalkSupportStateDetector::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_;
prev_stand_left_ = curr_stand_left_;
std_msgs::msg::Float64 pressure_msg;
pressure_msg.data = pressure_filtered_left_;
if (config_.debug){
pub_summed_pressure_l_->publish(pressure_msg);
pub_summed_pressure_left_->publish(pressure_msg);
}
if (pressure_filtered_left_ > config_.summed_pressure_threshold){
if (curr_stand_left_ != true){
up_l_ = this->now();
up_left_ = 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_;
step_duration_left_ = (1-config_.m)*(this->now().nanoseconds() - up_left_.nanoseconds()) + config_.m*step_duration_left_;
curr_stand_left_ = false;
}
}

}

void WalkSupportStateDetector::pressure_r_callback(bitbots_msgs::msg::FootPressure msg) {
void WalkSupportStateDetector::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_;
prev_stand_right_ = curr_stand_right_;
std_msgs::msg::Float64 pressure_msg;
pressure_msg.data = pressure_filtered_right_;
if (config_.debug){
pub_summed_pressure_r_->publish(pressure_msg);
pub_summed_pressure_right_->publish(pressure_msg);
}
if (pressure_filtered_right_ > config_.summed_pressure_threshold){
if (curr_stand_right_ != true){
up_r_ = this->now();
up_right_ = 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_;
step_duration_right_ = (1-config_.m)*(this->now().nanoseconds() - up_right_.nanoseconds()) + config_.m*step_duration_right_;
curr_stand_right_ = false;
}
}
Expand Down

0 comments on commit 353e9c4

Please sign in to comment.