diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0d20b64 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +*.pyc diff --git a/gauss_bringup/config/gauss_driver.yaml b/gauss_bringup/config/gauss_driver.yaml index beea041..b625755 100644 --- a/gauss_bringup/config/gauss_driver.yaml +++ b/gauss_bringup/config/gauss_driver.yaml @@ -20,7 +20,7 @@ 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 @@ -28,11 +28,13 @@ 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 diff --git a/gauss_driver/include/gauss_driver/communication/can_communication.h b/gauss_driver/include/gauss_driver/communication/can_communication.h index 6c8a80d..050cf96 100644 --- a/gauss_driver/include/gauss_driver/communication/can_communication.h +++ b/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 diff --git a/gauss_driver/include/gauss_driver/communication/communication_base.h b/gauss_driver/include/gauss_driver/communication/communication_base.h index ee3f5e5..31cb85a 100644 --- a/gauss_driver/include/gauss_driver/communication/communication_base.h +++ b/gauss_driver/include/gauss_driver/communication/communication_base.h @@ -48,7 +48,7 @@ class CommunicationBase { virtual void getFirmwareVersions(std::vector &motor_names, std::vector &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 &leds, std::string &message) = 0; diff --git a/gauss_driver/include/gauss_driver/communication/fake_communication.h b/gauss_driver/include/gauss_driver/communication/fake_communication.h index 6bb0012..4d6e55f 100644 --- a/gauss_driver/include/gauss_driver/communication/fake_communication.h +++ b/gauss_driver/include/gauss_driver/communication/fake_communication.h @@ -52,7 +52,7 @@ class FakeCommunication : public CommunicationBase { void getFirmwareVersions(std::vector &motor_names, std::vector &firmware_versions); - void sendPositionToRobot(const double cmd[6]); + bool sendPositionToRobot(const double cmd[6]); void activateLearningMode(bool activate); bool setLeds(std::vector &leds, std::string &message); diff --git a/gauss_driver/include/gauss_driver/communication/gauss_communication.h b/gauss_driver/include/gauss_driver/communication/gauss_communication.h index 53bd21c..94f8988 100644 --- a/gauss_driver/include/gauss_driver/communication/gauss_communication.h +++ b/gauss_driver/include/gauss_driver/communication/gauss_communication.h @@ -59,7 +59,7 @@ class GaussCommunication : public CommunicationBase { void getFirmwareVersions(std::vector &motor_names, std::vector &firmware_versions); - void sendPositionToRobot(const double cmd[6]); + bool sendPositionToRobot(const double cmd[6]); void activateLearningMode(bool activate); bool setLeds(std::vector &leds, std::string &message); diff --git a/gauss_driver/include/gauss_driver/hw_interface/gauss_hardware_interface.h b/gauss_driver/include/gauss_driver/hw_interface/gauss_hardware_interface.h index ce2dc2d..d3f3924 100644 --- a/gauss_driver/include/gauss_driver/hw_interface/gauss_hardware_interface.h +++ b/gauss_driver/include/gauss_driver/hw_interface/gauss_hardware_interface.h @@ -25,6 +25,9 @@ #include #include #include +#include +#include +#include #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 data_control_loop_thread; + double data_control_loop_frequency_; + void dataControlLoop(); + std::mutex data_mutex_; + std::queue joint1_pos_cmd_queue_; + std::queue joint2_pos_cmd_queue_; + std::queue joint3_pos_cmd_queue_; + std::queue joint4_pos_cmd_queue_; + std::queue joint5_pos_cmd_queue_; + std::queue joint6_pos_cmd_queue_; + + std::vector< std::queue *> 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}; diff --git a/gauss_driver/src/communication/can_communication.cpp b/gauss_driver/src/communication/can_communication.cpp index 0775c99..0522a7d 100644 --- a/gauss_driver/src/communication/can_communication.cpp +++ b/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) { diff --git a/gauss_driver/src/communication/fake_communication.cpp b/gauss_driver/src/communication/fake_communication.cpp index 44c93a8..8004852 100644 --- a/gauss_driver/src/communication/fake_communication.cpp +++ b/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]) diff --git a/gauss_driver/src/communication/gauss_communication.cpp b/gauss_driver/src/communication/gauss_communication.cpp index b5913b7..7b95126 100644 --- a/gauss_driver/src/communication/gauss_communication.cpp +++ b/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"<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"<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) diff --git a/gauss_driver/src/hw_interface/gauss_hardware_interface.cpp b/gauss_driver/src/hw_interface/gauss_hardware_interface.cpp index e4f2624..b37e9e1 100644 --- a/gauss_driver/src/hw_interface/gauss_hardware_interface.cpp +++ b/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"<