From 07112558fa0d1fe77a3ab862caf2830436db4be7 Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Wed, 7 Jun 2023 13:36:54 +0900
Subject: [PATCH] Added support for configuration file mode

---
 cfg/sas_robot_driver_franka_1.yaml   |  1 +
 include/sas_robot_driver_franka.h    | 14 +-------------
 src/sas_robot_driver_franka.cpp      | 22 +++++++++++++++++++++-
 src/sas_robot_driver_franka_node.cpp | 18 +-----------------
 4 files changed, 24 insertions(+), 31 deletions(-)

diff --git a/cfg/sas_robot_driver_franka_1.yaml b/cfg/sas_robot_driver_franka_1.yaml
index 53bbded..de14f80 100644
--- a/cfg/sas_robot_driver_franka_1.yaml
+++ b/cfg/sas_robot_driver_franka_1.yaml
@@ -1,4 +1,5 @@
 "robot_ip_address": "172.16.0.2"
+"robot_mode": "VelocityControl"
 "robot_port": 5007
 "robot_speed": 20.0
 "thread_sampling_time_nsec": 4000000
diff --git a/include/sas_robot_driver_franka.h b/include/sas_robot_driver_franka.h
index b7738e4..e4662b2 100644
--- a/include/sas_robot_driver_franka.h
+++ b/include/sas_robot_driver_franka.h
@@ -22,6 +22,7 @@ namespace sas
 struct RobotDriverFrankaConfiguration
 {
     std::string ip_address;
+    std::string mode;
     int port;
     double speed;
 };
@@ -42,20 +43,7 @@ private:
     std::vector<double> end_effector_pose_euler_buffer_;
     std::vector<double> end_effector_pose_homogenous_transformation_buffer_;
 
-    //DQ _homogenous_vector_to_dq(const VectorXd& homogenousvector) const;
-    //VectorXd _dq_to_homogenous_vector(const DQ& pose) const;
-    //VectorXd _get_end_effector_pose_homogenous_transformation();
-    //double _sign(const double &a) const;
 
-  //  void _connect(); //Throws std::runtime_error()
-
-   // void _motor_on(); //Throws std::runtime_error()
-   // void _motor_off() noexcept; //No exceptions should be thrown in the path to turn off the robot
-
-   // void _set_speed(const float& speed, const float& acceleration, const float& deacceleration); //Throws std::runtime_error()
-
-   // void _slave_mode_on(int mode); //Throws std::runtime_error()
-    //void _slave_mode_off() noexcept; //No exceptions should be thrown in the path to turn off the robot
 
 public:
     const static int SLAVE_MODE_JOINT_CONTROL;
diff --git a/src/sas_robot_driver_franka.cpp b/src/sas_robot_driver_franka.cpp
index e61d4f5..9fb7118 100644
--- a/src/sas_robot_driver_franka.cpp
+++ b/src/sas_robot_driver_franka.cpp
@@ -17,8 +17,28 @@ namespace sas
         end_effector_pose_homogenous_transformation_buffer_.resize(10,0);
         std::cout<<configuration.ip_address<<std::endl;
 
+        RobotInterfaceFranka::MODE mode = RobotInterfaceFranka::MODE::None;
+
+        std::cout<<configuration.mode<<std::endl;
+
+        if (configuration_.mode == std::string("None"))
+        {
+            mode = RobotInterfaceFranka::MODE::None;
+        }else if (configuration_.mode == std::string("PositionControl"))
+        {
+            mode = RobotInterfaceFranka::MODE::PositionControl;
+        }else if (configuration_.mode == std::string("VelocityControl"))
+        {
+            mode = RobotInterfaceFranka::MODE::VelocityControl;
+        }else
+        {
+            throw std::runtime_error(std::string("Wrong mode. ") + std::string("You used ") + configuration_.mode
+                                     + std::string(". However, you must use None, PositionControl or VelocityControl"));
+        }
+
+
         robot_driver_interface_sptr_ = std::make_shared<RobotInterfaceFranka>(configuration.ip_address,
-                                                                        RobotInterfaceFranka::MODE::VelocityControl, //None, PositionControl
+                                                                        mode, //None, PositionControl, VelocityControl
                                                                         RobotInterfaceFranka::HAND::ON);
     }
 
diff --git a/src/sas_robot_driver_franka_node.cpp b/src/sas_robot_driver_franka_node.cpp
index 7167746..01da09e 100644
--- a/src/sas_robot_driver_franka_node.cpp
+++ b/src/sas_robot_driver_franka_node.cpp
@@ -41,12 +41,7 @@ int main(int argc, char **argv)
     sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration;
 
     sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address);
-   // sas::get_ros_param(nh,"/robot_port",      robot_driver_franka_configuration.port);
-    //sas::get_ros_param(nh,"/robot_speed",     robot_driver_franka_configuration.speed);
-
-    //robot_driver_franka_configuration.ip_address = "172.16.0.2";
-    //robot_driver_franka_configuration.port =  0;
-    //robot_driver_franka_configuration.speed = 0.0;
+    sas::get_ros_param(nh,"/robot_mode",      robot_driver_franka_configuration.mode, false);
 
     sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
 
@@ -54,17 +49,6 @@ int main(int argc, char **argv)
     sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
     sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
 
-    //auto qmin = sas::std_vector_double_to_vectorxd(robot_driver_ros_configuration.q_min);
-    //auto qmax = sas::std_vector_double_to_vectorxd(robot_driver_ros_configuration.q_max);
-
-
-    //std::vector<double> q_min = {-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895};
-    //std::vector<double> q_max = { 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094,  2.6895};
-
-    //robot_driver_ros_configuration.thread_sampling_time_nsec = 4000000;
-    //robot_driver_ros_configuration.q_min = q_min;
-    //robot_driver_ros_configuration.q_max = q_max;
-
     robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
     
     try