Merge pull request #88 from BCLab-UNM/hotfix-manual-gripper-control

Manual gripper control fix

Author: gmfricke

src/abridge/src/abridge.cpp

@@ -45,6 +45,11 @@ const float deltaTime = 0.1; //abridge's update interval
 int currentMode = 0;
 string publishedName;
 
+// Allowing messages to be sent to the arduino too fast causes a disconnect
+// This is the minimum time between messages to the arduino in microseconds.
+// Only used with the gripper commands to fix a manual control bug.
+unsigned int min_usb_send_delay = 100;
+
 float heartbeat_publish_interval = 2;
 
 
@@ -197,6 +202,10 @@ void driveCommandHandler(const geometry_msgs::Twist::ConstPtr& message) {
 // radians, write them to a string and send that to the arduino
 // for processing.
 void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
+
+  // To throttle the message rate so we don't lose connection to the arduino
+  usleep(min_usb_send_delay);
+  
   char cmd[16]={'\0'};
 
   // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
@@ -211,6 +220,9 @@ void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
 }
 
 void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
+  // To throttle the message rate so we don't lose connection to the arduino
+  usleep(min_usb_send_delay);
+  
     char cmd[16]={'\0'};
 
     // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small

src/rqt_rover_gui/src/JoystickGripperInterface.cpp

@@ -17,10 +17,6 @@ JoystickGripperInterface::JoystickGripperInterface(const JoystickGripperInterfac
     this->gripperWristAnglePublisher        = source.gripperWristAnglePublisher;
     this->gripperFingerAnglePublisher       = source.gripperFingerAnglePublisher;
 
-    // QTimer state
-    this->joystickGripperWristControlTimer  = source.joystickGripperWristControlTimer;
-    this->joystickGripperFingerControlTimer = source.joystickGripperFingerControlTimer;
-
     // Wrist state
     this->wristAngle                        = source.wristAngle;
     this->wristAngleChangeRate              = source.wristAngleChangeRate;
@@ -35,10 +31,6 @@ JoystickGripperInterface::JoystickGripperInterface(const JoystickGripperInterfac
     this->fingerAngleMin                    = source.fingerAngleMin;
     this->fingerJoystickVector              = source.fingerJoystickVector;
 
-    // Other
-    this->commandReapplyRate                = source.commandReapplyRate;
-    this->stickCenterTolerance              = source.stickCenterTolerance;
-
     this->nh                                = source.nh;
 }
 
@@ -49,19 +41,6 @@ JoystickGripperInterface::JoystickGripperInterface(ros::NodeHandle nh, string ro
 
     this->nh = nh;
 
-    // Joystick auto repeat for gripper control
-    joystickGripperWristControlTimer = new QTimer(this);
-    joystickGripperFingerControlTimer = new QTimer(this);
-
-    // Connect the command timers to their handlers
-    connect(joystickGripperWristControlTimer, SIGNAL(timeout()), this, SLOT(joystickGripperWristControlTimerEventHandler()));
-    connect(joystickGripperFingerControlTimer, SIGNAL(timeout()), this, SLOT(joystickGripperFingerControlTimerEventHandler()));
-
-    connect(this, SIGNAL(sendJoystickGripperWristControlTimerStart(int)), joystickGripperWristControlTimer, SLOT(start(int)));
-    connect(this, SIGNAL(sendJoystickGripperWristControlTimerStop()), joystickGripperWristControlTimer, SLOT(stop()));
-    connect(this, SIGNAL(sendJoystickGripperFingerControlTimerStart(int)), joystickGripperFingerControlTimer, SLOT(start(int)));
-    connect(this, SIGNAL(sendJoystickGripperFingerControlTimerStop()), joystickGripperFingerControlTimer, SLOT(stop()));
-
     // Initialize gripper angles in radians
     wristAngle = 0;
     fingerAngle = 0;
@@ -83,12 +62,7 @@ JoystickGripperInterface::JoystickGripperInterface(ros::NodeHandle nh, string ro
     // Representation of the desired movement speed and direction generated by the joystick
     fingerJoystickVector = 0.0;
     wristJoystickVector = 0.0;
-
-    commandReapplyRate = 100; // in milliseconds
-
-    stickCenterTolerance = 0.05; // How close to zero does output from the joystick have to be for
-    // us to consider the user to have centered the stick
-
+    
     // Setup the gripper angle command publishers
     gripperWristAnglePublisher = nh.advertise<std_msgs::Float32>("/"+roverName+"/wristAngle/cmd", 10, this);
     gripperFingerAnglePublisher = nh.advertise<std_msgs::Float32>("/"+roverName+"/fingerAngle/cmd", 10, this);
@@ -106,15 +80,20 @@ void JoystickGripperInterface::moveWrist(float vec) {
 
     wristJoystickVector = -vec; // negate to make down the positive angle
 
-    // Check whether the stick is near the center deadzone - if so stop issuing movement commands
-    // if not apply the movement indicated by vec
-    if (fabs(wristJoystickVector) < stickCenterTolerance) {
-        emit sendJoystickGripperWristControlTimerStop();
-    } else {
-        // The event handler calculates the new angle for the wrist and sends it to the gripper wrist control topic
-        emit sendJoystickGripperWristControlTimerStart(commandReapplyRate);
-    }
+    // Calculate the new wrist angle to request
+    wristAngle += wristJoystickVector*wristAngleChangeRate;
 
+    // Don't exceed the min and max angles
+    if (wristAngle > wristAngleMax) wristAngle = wristAngleMax;
+    else if (wristAngle < wristAngleMin) wristAngle = wristAngleMin;
+
+    // If the wrist angle is small enough to use negative exponents set to zero
+    // negative exponents confuse the downstream conversion to string
+    if (fabs(wristAngle) < 0.001) wristAngle = 0.0f;
+
+    std_msgs::Float32 angleMsg;
+    angleMsg.data = wristAngle;
+    gripperWristAnglePublisher.publish(angleMsg);
 
 }
 
@@ -127,45 +106,26 @@ void JoystickGripperInterface::moveFingers(float vec){
 
     fingerJoystickVector = vec;
 
-    // Check whether the stick is near the center deadzone - if so stop issuing movement commands
-    // if not apply the movement indicated by vec
-    if (fabs(fingerJoystickVector) < stickCenterTolerance) {
-        emit sendJoystickGripperFingerControlTimerStop();
-    } else {
-        // The event handler calculates the new angle for the wrist and sends it to the gripper wrist control topic
-        emit sendJoystickGripperFingerControlTimerStart(commandReapplyRate);
-    }
-
-
-}
-
-// Update and broadcast gripper wrist commands
-// Called by event timer so the commands continue to be generated even when the joystick
-// is not moving
-void JoystickGripperInterface::joystickGripperWristControlTimerEventHandler(){
-
     // Calculate the new wrist angle to request
-    wristAngle += wristJoystickVector*wristAngleChangeRate;
+    fingerAngle += fingerJoystickVector*fingerAngleChangeRate;
 
     // Don't exceed the min and max angles
-    if (wristAngle > wristAngleMax) wristAngle = wristAngleMax;
-    else if (wristAngle < wristAngleMin) wristAngle = wristAngleMin;
+    if (fingerAngle > fingerAngleMax) fingerAngle = fingerAngleMax;
+    else if (fingerAngle < fingerAngleMin) fingerAngle = fingerAngleMin;
 
-    // If the wrist angle is small enough to use negative exponents set to zero
+    // If the finger angle is small enough to use negative exponents set to zero
     // negative exponents confuse the downstream conversion to string
-    if (fabs(wristAngle) < 0.001) wristAngle = 0.0f;
+    if (fabs(fingerAngle) < 0.001) fingerAngle = 0.0f;
 
+    // Publish the finger angle commands
     std_msgs::Float32 angleMsg;
-    angleMsg.data = wristAngle;
-    gripperWristAnglePublisher.publish(angleMsg);
-
+    angleMsg.data = fingerAngle;
+    gripperFingerAnglePublisher.publish(angleMsg);
 }
 
 void JoystickGripperInterface::changeRovers(string roverName)
 {
    ready = false;
-    joystickGripperWristControlTimer->stop();
-    joystickGripperFingerControlTimer->stop();
 
     gripperWristAnglePublisher.shutdown();
     gripperFingerAnglePublisher.shutdown();
@@ -187,36 +147,10 @@ void JoystickGripperInterface::changeRovers(string roverName)
   ready = true;
 }
 
-// Update and broadcast gripper commands for the fingers
-// Called by event timer so the commands continue to be generated even when the joystick
-// is not moving
-void JoystickGripperInterface::joystickGripperFingerControlTimerEventHandler(){
-    // Calculate the new wrist angle to request
-    fingerAngle += fingerJoystickVector*fingerAngleChangeRate;
-
-    // Don't exceed the min and max angles
-    if (fingerAngle > fingerAngleMax) fingerAngle = fingerAngleMax;
-    else if (fingerAngle < fingerAngleMin) fingerAngle = fingerAngleMin;
-
-
-    // If the finger angle is small enough to use negative exponents set to zero
-    // negative exponents confuse the downstream conversion to string
-    if (fabs(fingerAngle) < 0.001) fingerAngle = 0.0f;
-
-    // Publish the finger angle commands
-    std_msgs::Float32 angleMsg;
-    angleMsg.data = fingerAngle;
-    gripperFingerAnglePublisher.publish(angleMsg);
-
-}
-
 // Clean up the publishers when this object is destroyed
 JoystickGripperInterface::~JoystickGripperInterface(){
     ready = false;
 
-    if(joystickGripperWristControlTimer) delete joystickGripperWristControlTimer;
-    if(joystickGripperFingerControlTimer) delete joystickGripperFingerControlTimer;
-
     gripperWristAnglePublisher.shutdown();
     gripperFingerAnglePublisher.shutdown();
 }

src/rqt_rover_gui/src/JoystickGripperInterface.h

@@ -67,30 +67,18 @@ class JoystickGripperInterface : public QObject // inherits from QObject for QT
 
 private slots:
 
-    // These timed events calculate and broadcast the new gripper angles on the gripper control topics
-    void joystickGripperWristControlTimerEventHandler();
-    void joystickGripperFingerControlTimerEventHandler();
-
-
 private:
 
     // Publishers for sending gripper commands to the ROS topics
     ros::Publisher gripperWristAnglePublisher;
     ros::Publisher gripperFingerAnglePublisher;
 
-    QTimer* joystickGripperWristControlTimer; // Timer to trigger the wrist angle command
-    QTimer* joystickGripperFingerControlTimer; // Timer to trigger the finger angle command
-
     // Joystick gripper controller state
     float wristAngle, wristAngleChangeRate, wristAngleMax, wristAngleMin, wristJoystickVector;
 
     // The finger movements are symmetric so we just refer to one finger angle etc
     float fingerAngle, fingerAngleChangeRate, fingerAngleMax, fingerAngleMin, fingerJoystickVector;
 
-    int commandReapplyRate; // Rate at which to apply the joystick command in Hz
-
-    float stickCenterTolerance;
-
     std::string roverName;
 
     ros::NodeHandle nh;