impliment improved error handeling on driver failed

cfg/sas_robot_driver_franka_2.yaml

@@ -0,0 +1,8 @@
+"robot_ip_address": "172.16.0.3"
+"robot_mode": "PositionControl"
+"robot_port": 5007
+"robot_speed": 20.0
+"thread_sampling_time_nsec": 4000000
+"q_min": [-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895]
+"q_max": [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094,  2.6895]
+

cfg/sas_robot_driver_franka_3.yaml

@@ -0,0 +1,8 @@
+"robot_ip_address": "172.16.0.4"
+"robot_mode": "PositionControl"
+"robot_port": 5007
+"robot_speed": 20.0
+"thread_sampling_time_nsec": 4000000
+"q_min": [-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895]
+"q_max": [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094,  2.6895]
+

cfg/sas_robot_driver_franka_4.yaml

@@ -0,0 +1,8 @@
+"robot_ip_address": "172.16.0.5"
+"robot_mode": "PositionControl"
+"robot_port": 5007
+"robot_speed": 20.0
+"thread_sampling_time_nsec": 4000000
+"q_min": [-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895]
+"q_max": [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094,  2.6895]
+

include/robot_interface_franka.h

@@ -117,6 +117,9 @@ private:
 
 
     //-------To handle the threads-----------------
+    std::atomic<bool> exit_on_err_={false};
+
+
     void _start_joint_position_control_mode();
     std::thread joint_position_control_mode_thread_;
     void _start_joint_position_control_thread();
@@ -169,6 +172,8 @@ public:
 
     std::string get_status_message();
 
+    bool get_err_state() const {return exit_on_err_.load();}
+
     std::string get_robot_mode();
 
 

include/robot_interface_hand.h

@@ -52,7 +52,7 @@ using namespace Eigen;
 class RobotInterfaceHand
 {
 protected:
-    double speed_gripper_ = 0.02;
+    double speed_gripper_ = 0.05;//0.02
     std::string ip_ = "172.16.0.2";
     std::shared_ptr<franka::Gripper> gripper_sptr_;
     void _check_if_hand_is_connected();

include/sas_robot_driver_franka.h

@@ -42,6 +42,7 @@
 
 #include <sas_robot_driver/sas_robot_driver.h>
 #include "robot_interface_franka.h"
+#include <ros/common.h>
 
 using namespace DQ_robotics;
 using namespace Eigen;
@@ -74,6 +75,7 @@ private:
     //std::vector<double> end_effector_pose_euler_buffer_;
     //std::vector<double> end_effector_pose_homogenous_transformation_buffer_;
 
+    std::atomic_bool* break_loops_;
 
 
 public:

src/robot_interface_franka.cpp

@@ -32,6 +32,9 @@
 
 #include "robot_interface_franka.h"
 
+#include <ros/this_node.h>
+#include <rosconsole/macros_generated.h>
+
 
 /**
  * @brief robot_driver_franka::robot_driver_franka
@@ -376,6 +379,9 @@ void RobotInterfaceFranka::_start_echo_robot_state_mode(){
     }
     catch (franka::Exception const& e) {
         std::cout << e.what() << std::endl;
+        exit_on_err_.store(true);
+        status_message_=e.what();
+        // ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::echo_robot_state_mode::Error on:"+e.what());
     }
 }
 
@@ -453,6 +459,9 @@ void RobotInterfaceFranka::_start_joint_position_control_mode()
         }
         catch (franka::Exception const& e) {
             std::cout << e.what() << std::endl;
+            exit_on_err_.store(true);
+            status_message_=e.what();
+        // ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::joint_position_control_mode::Error on:"+e.what());
         }
 
 
@@ -524,6 +533,9 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode()
     }
     catch (franka::Exception const& e) {
         std::cout << e.what() << std::endl;
+        exit_on_err_.store(true);
+        status_message_=e.what();
+        // ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::joint_velocity_control_mode::Error on:"+e.what());
     }
 }
 
@@ -534,6 +546,7 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode()
  */
 void RobotInterfaceFranka::_setDefaultRobotBehavior()
 {
+
     robot_sptr_->setCollisionBehavior(
         {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
         {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},

src/sas_robot_driver_franka.cpp

@@ -34,12 +34,15 @@
 #include "sas_robot_driver_franka.h"
 #include "sas_clock/sas_clock.h"
 #include <dqrobotics/utils/DQ_Math.h>
+#include <ros/this_node.h>
+#include <rosconsole/macros_generated.h>
 
 namespace sas
 {
     RobotDriverFranka::RobotDriverFranka(const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
     RobotDriver(break_loops),
-    configuration_(configuration)
+    configuration_(configuration),
+    break_loops_(break_loops)
    {
         joint_positions_.resize(7);
         joint_velocities_.resize(7);
@@ -125,6 +128,10 @@ namespace sas
      */
     VectorXd  RobotDriverFranka::get_joint_positions()
     {
+        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());
+            break_loops_->store(true);
+        }
         return robot_driver_interface_sptr_->get_joint_positions();
     }
 
@@ -137,6 +144,10 @@ namespace sas
     void RobotDriverFranka::set_target_joint_positions(const VectorXd& desired_joint_positions_rad)
     {
         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());
+            break_loops_->store(true);
+        }
     }
 
     /**
@@ -158,6 +169,10 @@ namespace sas
     {
         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()) {
+            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
+            break_loops_->store(true);
+        }
     }
 
     /**