From 8abd1512c5e29cf0b8f2520c31caad0695da2f92 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Sat, 9 Dec 2023 18:28:03 +0100 Subject: [PATCH 1/2] Add stuck detection --- docker/Dockerfile.pc | 2 +- spes_move/include/spes_move/move.hpp | 3 +++ spes_move/src/move.cpp | 22 +++++++++++++++++++++- 3 files changed, 25 insertions(+), 2 deletions(-) diff --git a/docker/Dockerfile.pc b/docker/Dockerfile.pc index 4dcffb4..a6b106e 100644 --- a/docker/Dockerfile.pc +++ b/docker/Dockerfile.pc @@ -75,7 +75,7 @@ ENV DOCKERUSER=${DOCKERUSER} COPY bashrc /tmp/bashrc RUN cat /tmp/bashrc >> /${DOCKERUSER}/.bashrc -RUN wget https://s3.us-west-1.amazonaws.com/download.behaviortree.dev/groot2_linux_installer/Groot2-v1.1.1-x86_64.AppImage -O /usr/bin/groot2 && \ +RUN wget https://s3.us-west-1.amazonaws.com/download.behaviortree.dev/groot2_linux_installer/Groot2-v1.4.0-x86_64.AppImage -O /usr/bin/groot2 && \ chmod +x /usr/bin/groot2 USER ${DOCKERUSER} diff --git a/spes_move/include/spes_move/move.hpp b/spes_move/include/spes_move/move.hpp index 87cd499..d4cd9bc 100644 --- a/spes_move/include/spes_move/move.hpp +++ b/spes_move/include/spes_move/move.hpp @@ -51,6 +51,9 @@ namespace spes_move int update_rate_; double stopping_distance_; + double angular_stuck_coeff_; + double linear_stuck_coeff_; + spes_msgs::msg::MoveState state_msg_; rclcpp::Publisher::SharedPtr cmd_vel_pub_; diff --git a/spes_move/src/move.cpp b/spes_move/src/move.cpp index 1b78cba..aff33ad 100644 --- a/spes_move/src/move.cpp +++ b/spes_move/src/move.cpp @@ -347,6 +347,20 @@ namespace spes_move break; } + // Detect stuck + const double planned_rotation_velocity = rotation_ruckig_output_.new_velocity[0]; + const double planned_translation_velocity = translation_ruckig_output_.new_velocity[0]; + if (abs(last_error_x_) > planned_translation_velocity * linear_stuck_coeff_ || abs(last_error_yaw_) > planned_rotation_velocity * angular_stuck_coeff_) { + stop_robot(); + RCLCPP_WARN(get_logger(), "Stuck detected, stopping..."); + + state_msg_.error = spes_msgs::msg::MoveState::ERROR_STUCK; + + state_ = spes_msgs::msg::MoveState::STATE_IDLE; + update_state_msg(tf_base_target); + state_pub_->publish(state_msg_); + } + // Stop if there is a collision if (!command_->ignore_obstacles) { @@ -382,7 +396,7 @@ namespace spes_move if (is_collision_ahead) { stop_robot(); - RCLCPP_WARN(get_logger(), "Collision Ahead - Exiting Move"); + RCLCPP_WARN(get_logger(), "Collision detected, stopping..."); state_msg_.error = spes_msgs::msg::MoveState::ERROR_OBSTACLE; @@ -554,6 +568,9 @@ namespace spes_move declare_parameter("linear.tolerance", rclcpp::ParameterValue(0.01)); get_parameter("linear.tolerance", default_command_->linear_properties.tolerance); + declare_parameter("linear.stuck_coeff", rclcpp::ParameterValue(0.1)); + get_parameter("linear.stuck_coeff", linear_stuck_coeff_); + // Angular declare_parameter("angular.kp", rclcpp::ParameterValue(5.0)); get_parameter("angular.kp", default_command_->angular_properties.kp); @@ -569,6 +586,9 @@ namespace spes_move declare_parameter("angular.tolerance", rclcpp::ParameterValue(0.03)); get_parameter("angular.tolerance", default_command_->angular_properties.tolerance); + + declare_parameter("angular.stuck_coeff", rclcpp::ParameterValue(0.1)); + get_parameter("angular.stuck_coeff", angular_stuck_coeff_); } void Move::debouncing_reset() From e17688bf2cd9ec222745b4d6916a665cbd0d8243 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Sat, 9 Dec 2023 18:29:51 +0100 Subject: [PATCH 2/2] fix --- spes_move/src/move.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spes_move/src/move.cpp b/spes_move/src/move.cpp index aff33ad..67cea77 100644 --- a/spes_move/src/move.cpp +++ b/spes_move/src/move.cpp @@ -350,7 +350,7 @@ namespace spes_move // Detect stuck const double planned_rotation_velocity = rotation_ruckig_output_.new_velocity[0]; const double planned_translation_velocity = translation_ruckig_output_.new_velocity[0]; - if (abs(last_error_x_) > planned_translation_velocity * linear_stuck_coeff_ || abs(last_error_yaw_) > planned_rotation_velocity * angular_stuck_coeff_) { + if (abs(last_error_x_) > abs(planned_translation_velocity * linear_stuck_coeff_) || abs(last_error_yaw_) > abs(planned_rotation_velocity * angular_stuck_coeff_)) { stop_robot(); RCLCPP_WARN(get_logger(), "Stuck detected, stopping...");