From a0f33a9768522f29156e6df508db659c187fbe0a Mon Sep 17 00:00:00 2001 From: QuentinLin Date: Sat, 3 Aug 2024 13:53:06 +0900 Subject: [PATCH] test implimentation for automatic error recovery not done --- include/sas_robot_driver_franka.h | 1 - src/joint/robot_interface_franka.cpp | 37 ++++++++++++++++++++++----- src/joint/robot_interface_franka.h | 14 ++++++++++ src/joint/sas_robot_driver_franka.cpp | 19 ++++++++++---- src/sas_robot_driver_franka_node.cpp | 4 +-- 5 files changed, 61 insertions(+), 14 deletions(-) diff --git a/include/sas_robot_driver_franka.h b/include/sas_robot_driver_franka.h index 7b1093b..70ee6cf 100644 --- a/include/sas_robot_driver_franka.h +++ b/include/sas_robot_driver_franka.h @@ -61,7 +61,6 @@ struct RobotDriverFrankaConfiguration double speed; RobotInterfaceFranka::FrankaInterfaceConfiguration interface_configuration; DQ robot_reference_frame = DQ(0); - bool automatic_error_recovery = false; }; diff --git a/src/joint/robot_interface_franka.cpp b/src/joint/robot_interface_franka.cpp index 6dad297..98ee012 100644 --- a/src/joint/robot_interface_franka.cpp +++ b/src/joint/robot_interface_franka.cpp @@ -346,6 +346,17 @@ void RobotInterfaceFranka::initialize() break; } _update_status_message("Initialized.",verbose_); + restart_on_err_.store(false); + if(franka_configuration_.automatic_error_recovery) { + _start_automatic_error_recovery_monitor(); + } +} + +void RobotInterfaceFranka::_start_automatic_error_recovery_monitor() { + std::cerr<<"automatic error recovery is not handeled"< upper_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0}; std::array joint_impedance={3000, 3000, 3000, 2500, 2500, 2000, 2000}; // K_theta (Nm/rad) std::array cartesian_impedance={3000, 3000, 3000, 300, 300, 300}; // K_x (N/m) + bool automatic_error_recovery = false; }; enum class MODE{ @@ -155,6 +156,8 @@ private: //-------To handle the threads----------------- std::atomic exit_on_err_={false}; + // monitor restart progress + std::atomic restart_on_err_={false}; void _start_joint_position_control_mode(); @@ -171,6 +174,16 @@ private: void _start_joint_velocity_control_mode(); std::thread joint_velocity_control_mode_thread_; void _start_joint_velocity_control_thread(); + + void _start_automatic_error_recovery_monitor(); + std::thread automatic_error_recovery_thread; + void _automatic_error_recovery_monitor(); + void _trigger_automatic_error_recovery() { + restart_on_err_.store(true); + // before implimentation + exit_on_err_.store(true); + } + //---------------------------------------------- @@ -214,6 +227,7 @@ public: std::string get_status_message(); bool get_err_state() const {return exit_on_err_.load();} + bool get_restart_state() const {return restart_on_err_.load();} std::string get_robot_mode(); diff --git a/src/joint/sas_robot_driver_franka.cpp b/src/joint/sas_robot_driver_franka.cpp index 028a668..86731d1 100644 --- a/src/joint/sas_robot_driver_franka.cpp +++ b/src/joint/sas_robot_driver_franka.cpp @@ -52,7 +52,8 @@ namespace sas //joint_positions_buffer_.resize(8,0); //end_effector_pose_euler_buffer_.resize(7,0); //end_effector_pose_homogenous_transformation_buffer_.resize(10,0); - std::cout<( configuration.interface_configuration, @@ -146,9 +149,9 @@ namespace sas ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message()); break_loops_->store(true); } - if(!ros::ok()) { - ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::ROS shutdown requested."); - break_loops_->store(true); + if(robot_driver_interface_sptr_->get_restart_state()) { + ROS_WARN_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message()+". attempting restart"); + return robot_driver_interface_sptr_->get_joint_positions(); } _update_stiffness_contact_and_pose(); return robot_driver_interface_sptr_->get_joint_positions(); @@ -162,6 +165,9 @@ namespace sas */ void RobotDriverFranka::set_target_joint_positions(const VectorXd& desired_joint_positions_rad) { + if(robot_driver_interface_sptr_->get_restart_state()) { + return; + } robot_driver_interface_sptr_->set_target_joint_positions(desired_joint_positions_rad); if(robot_driver_interface_sptr_->get_err_state()) { ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message()); @@ -186,6 +192,9 @@ namespace sas */ void RobotDriverFranka::set_target_joint_velocities(const VectorXd &desired_joint_velocities) { + if(robot_driver_interface_sptr_->get_restart_state()) { + return; + } desired_joint_velocities_ = desired_joint_velocities; robot_driver_interface_sptr_->set_target_joint_velocities(desired_joint_velocities); if(robot_driver_interface_sptr_->get_err_state()) { diff --git a/src/sas_robot_driver_franka_node.cpp b/src/sas_robot_driver_franka_node.cpp index c561a4c..6a1037f 100644 --- a/src/sas_robot_driver_franka_node.cpp +++ b/src/sas_robot_driver_franka_node.cpp @@ -131,8 +131,8 @@ int main(int argc, char **argv) }else{ROS_INFO_STREAM(ros::this_node::getName()+"::Robot parameter file path not set. Robot Model Unknow.");} if(nh.hasParam(ros::this_node::getName()+"/automatic_error_recovery")) { - sas::get_ros_param(nh,"/automatic_error_recovery",robot_driver_franka_configuration.automatic_error_recovery); - if(robot_driver_franka_configuration.automatic_error_recovery) + sas::get_ros_param(nh,"/automatic_error_recovery",franka_interface_configuration.automatic_error_recovery); + if(franka_interface_configuration.automatic_error_recovery) { ROS_WARN_STREAM(ros::this_node::getName()+"::Automatic error recovery enabled. STATUS EXPERIMENTAL"); }