Send queue

.gitignore

@@ -0,0 +1 @@
+*.pyc

gauss_bringup/config/gauss_driver.yaml

@@ -20,19 +20,21 @@ gpio_can_interrupt: 6
 #  Those params have been chosen to get a good (connection performance + speed / CPU usage) ratio
 #
 
-ros_control_loop_frequency:              100.0
+ros_control_loop_frequency:              80.0
 
 gauss_hw_check_connection_frequency:     2.0
 publish_hw_status_frequency:             1.0
 publish_software_version_frequency:      1.0
 publish_learning_mode_frequency:         1.0
 read_rpi_diagnostics_frequency:          0.25
 
-dxl_hardware_control_loop_frequency:     100.0
+dxl_hardware_control_loop_frequency:     50.0
 dxl_hw_write_frequency:                  50.0
 dxl_hw_data_read_frequency:              25.0
 dxl_hw_status_read_frequency:            0.5
 
-can_hardware_control_loop_frequency:     100.0
-can_hw_write_frequency:                  50.0
+can_hardware_control_loop_frequency:     60.0
+can_hw_write_frequency:                  60.0
 can_hw_check_connection_frequency:       1.0
+
+data_control_loop_frequency:             80.0

gauss_driver/include/gauss_driver/communication/can_communication.h

@@ -106,6 +106,8 @@ class CanCommunication {
 
         void synchronizeSteppers(bool begin_traj);
 
+        bool has_data_flag = false;
+
     private:
 
         // Gauss hardware version

gauss_driver/include/gauss_driver/communication/communication_base.h

@@ -48,7 +48,7 @@ class CommunicationBase {
         virtual void getFirmwareVersions(std::vector<std::string> &motor_names,
                 std::vector<std::string> &firmware_versions) = 0;
 
-        virtual void sendPositionToRobot(const double cmd[6]) = 0;
+        virtual bool sendPositionToRobot(const double cmd[6]) = 0;
         virtual void activateLearningMode(bool activate) = 0;
         virtual bool setLeds(std::vector<int> &leds, std::string &message) = 0;
 

gauss_driver/include/gauss_driver/communication/fake_communication.h

@@ -52,7 +52,7 @@ class FakeCommunication : public CommunicationBase {
         void getFirmwareVersions(std::vector<std::string> &motor_names,
                 std::vector<std::string> &firmware_versions);
 
-        void sendPositionToRobot(const double cmd[6]); 
+        bool sendPositionToRobot(const double cmd[6]); 
         void activateLearningMode(bool activate);
         bool setLeds(std::vector<int> &leds, std::string &message);
 

gauss_driver/include/gauss_driver/communication/gauss_communication.h

@@ -59,7 +59,7 @@ class GaussCommunication : public CommunicationBase {
         void getFirmwareVersions(std::vector<std::string> &motor_names,
                 std::vector<std::string> &firmware_versions);
 
-        void sendPositionToRobot(const double cmd[6]); 
+        bool sendPositionToRobot(const double cmd[6]); 
         void activateLearningMode(bool activate);
         bool setLeds(std::vector<int> &leds, std::string &message);
 

gauss_driver/include/gauss_driver/hw_interface/gauss_hardware_interface.h

@@ -25,6 +25,9 @@
 #include <hardware_interface/robot_hw.h>
 #include <ros/ros.h>
 #include <vector>
+#include <mutex>
+#include <queue>
+#include <thread>
 
 #include "gauss_driver/communication/communication_base.h"
 #include "common/common.h"
@@ -54,7 +57,21 @@ class GaussHardwareInterface: public hardware_interface::RobotHW {
 
         hardware_interface::JointStateInterface joint_state_interface;
         hardware_interface::PositionJointInterface joint_position_interface;
+
+        boost::shared_ptr<std::thread> data_control_loop_thread; 
+        double data_control_loop_frequency_;
+        void dataControlLoop();
 
+        std::mutex data_mutex_;
+        std::queue<double> joint1_pos_cmd_queue_;   
+        std::queue<double> joint2_pos_cmd_queue_;   
+        std::queue<double> joint3_pos_cmd_queue_;   
+        std::queue<double> joint4_pos_cmd_queue_;   
+        std::queue<double> joint5_pos_cmd_queue_;   
+        std::queue<double> joint6_pos_cmd_queue_;   
+
+        std::vector< std::queue<double> *>  pos_cmd_queues_;
+
         double cmd[6] = { 0, 0.64, -1.39, 0, 0, 0};
         double pos[6] = { 0, 0.64, -1.39, 0, 0, 0};
         double vel[6] = {0};

gauss_driver/src/communication/can_communication.cpp

@@ -403,13 +403,16 @@ void CanCommunication::hardwareControlWrite()
         if (write_position_enable) {
 
             for (int i = 0 ; i < motors.size(); i++) {
-                if (motors.at(i)->isEnabled()) {
+                if (motors.at(i)->isEnabled() && has_data_flag) {
+                    // ROS_INFO("sendPositionCommand i %d pos %d", i, motors.at(i)->getPositionCommand());
+
                     if (can->sendPositionCommand(motors.at(i)->getId(), motors.at(i)->getPositionCommand()) != CAN_OK) {
-                        //ROS_ERROR("Failed to send position");
+                        ROS_ERROR("Failed to send position");
                     }
                 }
             }
         }
+        has_data_flag = false;
 
         // write micro steps
         if (write_micro_steps_enable) {

gauss_driver/src/communication/fake_communication.cpp

@@ -101,11 +101,12 @@ bool FakeCommunication::isCalibrationInProgress()
     return false;
 }
 
-void FakeCommunication::sendPositionToRobot(const double cmd[6])
+bool FakeCommunication::sendPositionToRobot(const double cmd[6])
 {
     for (int i = 0 ; i < 6 ; i++) {
         echo_pos[i] = cmd[i]; 
     }
+    return true;
 }
 
 void FakeCommunication::getCurrentPosition(double pos[6])

gauss_driver/src/communication/gauss_communication.cpp

@@ -453,7 +453,7 @@ void GaussCommunication::getCurrentPosition(double pos[6])
     }
 }
 
-void GaussCommunication::sendPositionToRobot(const double cmd[6])
+bool GaussCommunication::sendPositionToRobot(const double cmd[6])
 {
     bool is_calibration_in_progress = false;
     if (can_enabled) {
@@ -463,8 +463,16 @@ void GaussCommunication::sendPositionToRobot(const double cmd[6])
     // don't send position command when calibrating motors
     if (!is_calibration_in_progress) {
         if (hardware_version == 1) {
-            if (can_enabled) { canComm->setGoalPositionV1(cmd[0], cmd[1], cmd[2], cmd[3]); }
-            if (dxl_enabled) { dxlComm->setGoalPositionV1(cmd[4], cmd[5]); }
+            if(!canComm->has_data_flag){ // if do not have data. send it
+                // std::cout<<"---------- canComm->has_data_flag is false"<<std::endl;
+                if (can_enabled) { canComm->setGoalPositionV1(cmd[0], cmd[1], cmd[2], cmd[3]); }
+                if (dxl_enabled) { dxlComm->setGoalPositionV1(cmd[4], cmd[5]); }
+                canComm->has_data_flag = true;
+                return true;
+            }else{
+                // std::cout<<"---------- canComm->has_data_flag is true"<<std::endl;
+                return false;
+            }
 
             // if disabled (debug purposes)
             if (!can_enabled) {
@@ -478,6 +486,7 @@ void GaussCommunication::sendPositionToRobot(const double cmd[6])
                 pos_dxl_disabled_v1[0] = cmd[4];
                 pos_dxl_disabled_v1[1] = cmd[5];
             }
+            return true;
         }
         else if (hardware_version == 2) {
             if (can_enabled) { canComm->setGoalPositionV2(cmd[0], cmd[1], cmd[2]); }
@@ -497,6 +506,7 @@ void GaussCommunication::sendPositionToRobot(const double cmd[6])
             }
         }
     }
+    return false;
 }
 
 void GaussCommunication::activateLearningMode(bool activate) 

gauss_driver/src/hw_interface/gauss_hardware_interface.cpp

@@ -56,6 +56,17 @@ GaussHardwareInterface::GaussHardwareInterface(CommunicationBase* gauss_comm)
     registerInterface(&joint_position_interface);
 
     ROS_INFO("Interfaces registered.");
+
+    pos_cmd_queues_.push_back(&joint1_pos_cmd_queue_);
+    pos_cmd_queues_.push_back(&joint2_pos_cmd_queue_);
+    pos_cmd_queues_.push_back(&joint3_pos_cmd_queue_);
+    pos_cmd_queues_.push_back(&joint4_pos_cmd_queue_);
+    pos_cmd_queues_.push_back(&joint5_pos_cmd_queue_);
+    pos_cmd_queues_.push_back(&joint6_pos_cmd_queue_);
+
+    ros::param::get("~data_control_loop_frequency", data_control_loop_frequency_);
+    data_control_loop_thread.reset(new std::thread(boost::bind(&GaussHardwareInterface::dataControlLoop, this)));
+
 }
 
 void GaussHardwareInterface::setCommandToCurrentPosition()
@@ -104,5 +115,60 @@ void GaussHardwareInterface::write()
     //pos[4] = cmd[4];
     //pos[5] = cmd[5];
 
-    comm->sendPositionToRobot(cmd);
+    // comm->sendPositionToRobot(cmd);
+    data_mutex_.lock();
+    for(size_t i = 0; i < 6; i++)
+    {
+        if( pos_cmd_queues_.at(i)->size() !=0 ){         
+            if(fabs(cmd[i] - pos_cmd_queues_.at(i)->back()) <= 1e-8){
+                continue;
+            }
+        }
+        pos_cmd_queues_.at(i)->push(cmd[i]);
+    }
+    data_mutex_.unlock();
 }
+
+
+void GaussHardwareInterface::dataControlLoop()
+{
+    ROS_INFO("thread dataControlLoop");
+
+    ros::Rate data_control_loop_frequency_rate = ros::Rate(data_control_loop_frequency_); 
+
+    while (ros::ok()) {
+       data_mutex_.lock();
+
+       double pos_cmd_tmp[6];
+       bool got_data_flag = false;
+
+       for(size_t i = 0; i < 6; i++)
+        {
+            if(!pos_cmd_queues_.at(i)->size()){
+                continue;
+            }else{
+                got_data_flag = true;
+            }
+
+            double cmd = pos_cmd_queues_.at(i)->front();
+            // printf("--------pos queue size %d, cmd %f\n",pos_cmd_queues_.at(i)->size(), cmd);
+            pos_cmd_tmp[i] = cmd;
+        } 
+
+        if(got_data_flag){
+            if(comm->sendPositionToRobot(pos_cmd_tmp)){ //send success
+                for(size_t i = 0; i < 6; i++)
+                {
+                    pos_cmd_queues_.at(i)->pop();                
+                }
+                // std::cout<<"---------- send success"<<std::endl;
+            }else{
+                // std::cout<<"---------- wait to send"<<std::endl;
+            }
+        }
+
+        data_mutex_.unlock();
+
+        data_control_loop_frequency_rate.sleep();      
+    }
+}