add doSwitch()

hans-robot · Sep 30, 2018 · ce5f552 · ce5f552
1 parent 2373674
commit ce5f552
Show file tree

Hide file tree

Showing 4 changed files with 252 additions and 35 deletions.
diff --git a/elfin_ethercat_driver/include/elfin_ethercat_driver/elfin_ethercat_client.h b/elfin_ethercat_driver/include/elfin_ethercat_driver/elfin_ethercat_client.h
@@ -146,7 +146,7 @@ class ElfinEtherCATClient
     bool inTrqMode();
     bool inPosBasedMode();
     void setPosMode();
-    void setPosEffortMode();
+    void setPosTrqMode();
     void setTrqMode();
     bool enable_cb(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp);
     bool reset_fault_cb(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp);

diff --git a/elfin_ethercat_driver/src/elfin_ethercat_client.cpp b/elfin_ethercat_driver/src/elfin_ethercat_client.cpp
@@ -611,23 +611,20 @@ void ElfinEtherCATClient::setPosMode()
     writeOutput_unit(elfin_rxpdo::AXIS1_CONTROLWORD, 0x801f);
     writeOutput_unit(elfin_rxpdo::AXIS2_CONTROLWORD, 0x801f);
     writeOutput_unit(elfin_rxpdo::UDM_CMD, 0x44);
-    usleep(10000);
 }
 
-void ElfinEtherCATClient::setPosEffortMode()
+void ElfinEtherCATClient::setPosTrqMode()
 {
     writeOutput_unit(elfin_rxpdo::AXIS1_CONTROLWORD, 0x801f);
     writeOutput_unit(elfin_rxpdo::AXIS2_CONTROLWORD, 0x801f);
     writeOutput_unit(elfin_rxpdo::UDM_CMD, 0x144);
-    usleep(10000);
 }
 
 void ElfinEtherCATClient::setTrqMode()
 {
     writeOutput_unit(elfin_rxpdo::AXIS1_CONTROLWORD, 0x081f);
     writeOutput_unit(elfin_rxpdo::AXIS2_CONTROLWORD, 0x081f);
     writeOutput_unit(elfin_rxpdo::UDM_CMD, 0x66);
-    usleep(10000);
 }
 
 bool ElfinEtherCATClient::enable_cb(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& resp)

diff --git a/elfin_ros_control/include/elfin_ros_control/elfin_hardware_interface.h b/elfin_ros_control/include/elfin_ros_control/elfin_hardware_interface.h
@@ -49,6 +49,7 @@ Created on Wed Oct 25 11:36:26 2017
 
 #include <boost/shared_ptr.hpp>
 #include <boost/thread.hpp>
+#include <boost/thread/mutex.hpp>
 
 #include <std_msgs/Float64.h>
 #include <sensor_msgs/JointState.h>
@@ -97,6 +98,10 @@ class ElfinHWInterface : public hardware_interface::RobotHW
     ~ElfinHWInterface();
     bool prepareSwitch(const std::list<hardware_interface::ControllerInfo> &start_list,
                        const std::list<hardware_interface::ControllerInfo> &stop_list);
+
+    void doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list,
+                  const std::list<hardware_interface::ControllerInfo> &stop_list);
+
     void read_init();
     void read_update(const ros::Time &time_now);
     void write_update();
@@ -115,6 +120,12 @@ class ElfinHWInterface : public hardware_interface::RobotHW
 
     ros::Time read_update_time_;
     ros::Duration read_update_dur_;
+
+    std::vector<bool> pre_switch_flags_;
+    std::vector<boost::shared_ptr<boost::mutex> > pre_switch_mutex_ptrs_;
+
+    bool isModuleMoving(int module_num);
+    double motion_threshold_;
 };
 
 }