test implimentation for automatic error recovery not done

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;
 };
 
 

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"<<std::endl;
+}
+void RobotInterfaceFranka::_automatic_error_recovery_monitor() {
+    std::cerr<<"automatic error recovery is not handeled"<<std::endl;
 }
 
 
@@ -405,8 +416,12 @@ void RobotInterfaceFranka::_start_echo_robot_state_mode(){
         _update_status_message("Finished echo_robot_state.",verbose_);
     }
     catch (franka::Exception const& e) {
-        std::cout << e.what() << std::endl;
+        std::cerr << e.what() << std::endl;
-        exit_on_err_.store(true);
+        if(!franka_configuration_.automatic_error_recovery) {
+            exit_on_err_.store(true);
+        }else {
+            _trigger_automatic_error_recovery();
+        }
         status_message_=e.what();
         // ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::echo_robot_state_mode::Error on:"+e.what());
     }
@@ -485,8 +500,12 @@ void RobotInterfaceFranka::_start_joint_position_control_mode()
 
         }
         catch (franka::Exception const& e) {
-            std::cout << e.what() << std::endl;
+            std::cerr << e.what() << std::endl;
-            exit_on_err_.store(true);
+            if(!franka_configuration_.automatic_error_recovery) {
+                exit_on_err_.store(true);
+            }else {
+                _trigger_automatic_error_recovery();
+            }
             status_message_=e.what();
         // ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::joint_position_control_mode::Error on:"+e.what());
         }
@@ -559,11 +578,17 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode()
             });
     }
     catch (franka::Exception const& e) {
-        std::cout << e.what() << std::endl;
+        std::cerr << e.what() << std::endl;
-        exit_on_err_.store(true);
+        if(!franka_configuration_.automatic_error_recovery) {
+            exit_on_err_.store(true);
+        }else {
+            _trigger_automatic_error_recovery();
+        }
         status_message_=e.what();
         // ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::joint_velocity_control_mode::Error on:"+e.what());
     }
+
+
 }
 
 

src/joint/robot_interface_franka.h

@@ -77,6 +77,7 @@ public:
         std::array<double, 6> upper_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
         std::array<double, 7> joint_impedance={3000, 3000, 3000, 2500, 2500, 2000, 2000};  // K_theta (Nm/rad)
         std::array<double, 6> 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<bool> exit_on_err_={false};
+    // monitor restart progress
+    std::atomic<bool> 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();
 

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.ip_address<<std::endl;
+        // std::cout<<configuration.ip_address<<std::endl;
+        ROS_INFO_STREAM(ros::this_node::getName() + " RobotDriverFranka::RobotDriverFranka: connecting to robot at " + configuration.ip_address);
 
         RobotInterfaceFranka::MODE mode = RobotInterfaceFranka::MODE::None;
 
@@ -72,7 +73,9 @@ namespace sas
             throw std::runtime_error(std::string("Wrong mode. ") + std::string("You used ") + configuration_.mode
                                      + std::string(". However, you must use None, PositionControl or VelocityControl"));
         }
-
+        if(configuration.interface_configuration.automatic_error_recovery) {
+            ROS_WARN_STREAM("["+ros::this_node::getName()+"]::Automatic error recovery is enabled. THIS FEATURE IS VERY EXPERIMENTAL");
+        }
 
         robot_driver_interface_sptr_ = std::make_shared<RobotInterfaceFranka>(
             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()) {
+        if(robot_driver_interface_sptr_->get_restart_state()) {
-            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::ROS shutdown requested.");
+            ROS_WARN_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message()+". attempting restart");
-            break_loops_->store(true);
+            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()) {

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);
+        sas::get_ros_param(nh,"/automatic_error_recovery",franka_interface_configuration.automatic_error_recovery);
-        if(robot_driver_franka_configuration.automatic_error_recovery)
+        if(franka_interface_configuration.automatic_error_recovery)
         {
             ROS_WARN_STREAM(ros::this_node::getName()+"::Automatic error recovery enabled. STATUS EXPERIMENTAL");
         }