hek_teleop.h

Go to the documentation of this file.

//
// Package:   RoadNarrows Robotics Hekateros Robotic Manipulator ROS Package
//
// Link:      https://github.com/roadnarrows-robotics/hekateros
//
// ROS Node:  hek_teleop
//
// File:      hek_teleop.h
//
/*
 * @EulaBegin@
 * 
 * Permission is hereby granted, without written agreement and without
 * license or royalty fees, to use, copy, modify, and distribute this
 * software and its documentation for any purpose, provided that
 * (1) The above copyright notice and the following two paragraphs
 * appear in all copies of the source code and (2) redistributions
 * including binaries reproduces these notices in the supporting
 * documentation.   Substantial modifications to this software may be
 * copyrighted by their authors and need not follow the licensing terms
 * described here, provided that the new terms are clearly indicated in
 * all files where they apply.
 * 
 * IN NO EVENT SHALL THE AUTHOR, ROADNARROWS LLC, OR ANY MEMBERS/EMPLOYEES
 * OF ROADNARROW LLC OR DISTRIBUTORS OF THIS SOFTWARE BE LIABLE TO ANY
 * PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL
 * DAMAGES ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION,
 * EVEN IF THE AUTHORS OR ANY OF THE ABOVE PARTIES HAVE BEEN ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 * 
 * THE AUTHOR AND ROADNARROWS LLC SPECIFICALLY DISCLAIM ANY WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN
 * "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
 * 
 * @EulaEnd@
 */

#ifndef _HEK_TELEOP_H
#define _HEK_TELEOP_H

//
// System
//
#include <sys/types.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <string>
#include <map>

//
// Boost
//
#include <boost/bind.hpp>
#include "boost/assign.hpp"

//
// ROS
//
#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"

//
// ROS generated core, industrial, and hekateros messages.
//
#include "trajectory_msgs/JointTrajectory.h"
#include "sensor_msgs/JointState.h"
#include "industrial_msgs/RobotStatus.h"
#include "hekateros_control/HekJointStateExtended.h"
#include "hekateros_control/HekRobotStatusExtended.h"

//
// ROS generatated hekateros services.
//
#include "hekateros_control/Calibrate.h"
#include "hekateros_control/ClearAlarms.h"
#include "hekateros_control/CloseGripper.h"
#include "hekateros_control/EStop.h"
#include "hekateros_control/Freeze.h"
#include "hekateros_control/GetProductInfo.h"
#include "hekateros_control/GotoBalancedPos.h"
#include "hekateros_control/GotoParkedPos.h"
#include "hekateros_control/GotoZeroPt.h"
#include "hekateros_control/IsAlarmed.h"
#include "hekateros_control/IsCalibrated.h"
#include "hekateros_control/IsDescLoaded.h"
#include "hekateros_control/OpenGripper.h"
#include "hekateros_control/Release.h"
#include "hekateros_control/ResetEStop.h"
#include "hekateros_control/SetRobotMode.h"
#include "hekateros_control/Stop.h"

//
// ROS generated action servers.
//

//
// ROS generated HID messages.
//
#include "hid/ConnStatus.h"           // subscribe
#include "hid/Controller360State.h"   // subscribe
#include "hid/LEDPattern.h"           // service
#include "hid/RumbleCmd.h"            // publish

//
// ROS generatated HID services.
//
#include "hid/SetLED.h"
#include "hid/SetRumble.h"

//
// RoadNarrows
//
#include "rnr/rnrconfig.h"
#include "rnr/log.h"
#include "rnr/hid/HIDXbox360.h"

//
// RoadNarrows embedded hekateros library.
//
#include "Hekateros/hekateros.h"
#include "Hekateros/hekXmlCfg.h"
#include "Hekateros/hekRobot.h"
#include "Hekateros/hekUtils.h"


namespace hekateros_control
{
  class HekTeleop
  {
  public:
    typedef std::map<std::string, ros::ServiceServer> MapServices;

    typedef std::map<std::string, ros::ServiceClient> MapClientServices;

    typedef std::map<std::string, ros::Publisher> MapPublishers;

    typedef std::map<std::string, ros::Subscriber> MapSubscriptions;

    typedef std::map<std::string, double> MapDouble;

    typedef std::map<std::string, bool> MapBool;

    typedef std::map<std::string, int> MapJointTraj;

    enum TeleopState
    {
      TeleopStateUninit,    
      TeleopStatePaused,    
      TeleopStateReady      
    };

    enum TeleopMode
    {
      TeleopModeFirstPerson,  
      TeleopModeShoulder,     
      TeleopModeElbow         
    };

    enum ButtonId
    {
      ButtonIdGotoBalPos    = rnr::Xbox360FeatIdAButton,  
      ButtonIdEStop         = rnr::Xbox360FeatIdBButton,  
      ButtonIdGotoParkedPos = rnr::Xbox360FeatIdXButton,  
      ButtonIdGotoZeroPt    = rnr::Xbox360FeatIdYButton,  

      ButtonIdPause         = rnr::Xbox360FeatIdBack,     
      ButtonIdToggleMode    = rnr::Xbox360FeatIdCenterX,  
      ButtonIdStart         = rnr::Xbox360FeatIdStart,    

      ButtonIdPrevJoint     = rnr::Xbox360FeatIdPadDown,  
      ButtonIdNextJoint     = rnr::Xbox360FeatIdPadUp,    

      ButtonIdFineTune1     = rnr::Xbox360FeatIdLeftStickClick,
      ButtonIdFineTune2     = rnr::Xbox360FeatIdRightStickClick,

      ButtonIdMoveJoints    = rnr::Xbox360FeatIdLeftJoyY, 
      ButtonIdRotBase       = rnr::Xbox360FeatIdLeftJoyX, 
      ButtonIdPitchWrist    = rnr::Xbox360FeatIdRightJoyY,

      ButtonIdRotWristCw    = rnr::Xbox360FeatIdLeftBump, 
      ButtonIdRotWristCcw   = rnr::Xbox360FeatIdRightBump,

      ButtonIdOpenGripper   = rnr::Xbox360FeatIdRightTrigger,
      ButtonIdCloseGripper  = rnr::Xbox360FeatIdLeftTrigger  
    };

    typedef std::map<int, int> ButtonState;

    enum LEDPat
    {
      LEDPatOff       = XBOX360_LED_PAT_ALL_OFF,    
      LEDPatOn        = XBOX360_LED_PAT_ALL_BLINK,  
      LEDPatPaused    = XBOX360_LED_PAT_4_ON,       
      LEDPatReady     = XBOX360_LED_PAT_ALL_SPIN,   
      LEDPatShoulder  = XBOX360_LED_PAT_3_ON,       
      LEDPatElbow     = XBOX360_LED_PAT_1_ON        
    };

    struct FirstPersonState
    {
      bool    m_bNewGoal;   
      double  m_goalSign;   
      struct
      {
        double  alpha;      
        double  beta;       
        double  gamma;      
      } m_goalJoint;        
      struct
      {
        double x;           
        double y;           
      } m_goalCart;         
    };

    HekTeleop(ros::NodeHandle &nh, double hz);

    virtual ~HekTeleop();

    virtual void advertiseServices()
    {
      // none
    }

    virtual void clientServices();

    virtual void advertisePublishers(int nQueueDepth=2);

    virtual void subscribeToTopics(int nQueueDepth=2);

    virtual void publish()
    {
      // No periodic publishing. All event driven.
    }

    virtual void commCheck();

    void putRobotInSafeMode(bool bHard);

    bool canMove()
    {
      if((m_eState == TeleopStateReady) &&
         (m_msgRobotStatus.is_calibrated.val ==
                                       industrial_msgs::TriState::TRUE) &&
         (m_msgRobotStatus.e_stopped.val == industrial_msgs::TriState::FALSE) &&
         (m_msgRobotStatus.in_error.val == industrial_msgs::TriState::FALSE))
      {
        return true;
      }
      else
      {
        return false;
      }
    }

    ros::NodeHandle &getNodeHandle()
    {
      return m_nh;
    }

    int countsPerSecond(double seconds)
    {
      return (int)(seconds * m_hz);
    }

  protected:
    ros::NodeHandle  &m_nh;       
    double            m_hz;       

    // ROS services, publishers, subscriptions.
    MapServices       m_services;       
    MapClientServices m_clientServices; 
    MapPublishers     m_publishers;     
    MapSubscriptions  m_subscriptions;  

    // state
    TeleopState       m_eState;         
    TeleopMode        m_eMode;          
    bool              m_bHasXboxComm;   
    int               m_nWdXboxCounter; 
    int               m_nWdXboxTimeout; 
    bool              m_bHasRobotComm;  
    int               m_nWdRobotCounter;
    int               m_nWdRobotTimeout;
    bool              m_bRcvdRobotStatus; 
    bool              m_bRcvdJointState; 
    bool              m_bHasFullComm;   
    ButtonState       m_buttonState;    
    int               m_rumbleLeft;     
    int               m_rumbleRight;    
    FirstPersonState  m_fpState;        
    bool              m_bPreemptMove;   
    double            m_fMoveTuning;    
    MapDouble         m_mapCurPos;      
    MapDouble         m_mapCurVel;      
    MapDouble         m_mapGoalPos;     
    MapDouble         m_mapGoalVel;     
    MapBool           m_mapTeleop;      
    MapJointTraj      m_mapTraj;        

    // messages
    hekateros_control::HekRobotStatusExtended m_msgRobotStatus;
    hekateros_control::HekJointStateExtended  m_msgJointState;
    trajectory_msgs::JointTrajectoryPoint     m_msgActiveJointTrajPoint;
    trajectory_msgs::JointTrajectoryPoint     m_msgJointTrajPoint;
    trajectory_msgs::JointTrajectory          m_msgJointTraj;
    hid::ConnStatus                           m_msgConnStatus;


    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
    // Server Service callbacks
    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 

    // none


    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
    // Client Services
    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 

    void setLED(int pattern);

    void setRumble(int motorLeft, int motorRight);

    void closeGripper();

    void estop();

    void freeze();

    void gotoBalancedPos();

    void gotoParkedPos();

    void gotoZeroPt();

    void resetEStop();

    void setRobotMode(hekateros::HekRobotMode mode);

    void stop(const std::vector<std::string> &vecJointNames);


    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
    // Topic Publishers
    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 

    void publishJointCmd();

    void publishRumbleCmd(int motorLeft, int motorRight);


    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
    // Subscribed Topic Callbacks
    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 

    void cbRobotStatus(const hekateros_control::HekRobotStatusExtended &msg);

    void cbJointState(const hekateros_control::HekJointStateExtended &msg);
    
    void cbXboxConnStatus(const hid::ConnStatus &msg);

    void cbXboxBttnState(const hid::Controller360State &msg);


    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
    // Xbox Actions
    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 

    void msgToState(const hid::Controller360State &msg,
                    ButtonState                   &buttonState);

    bool buttonOffToOn(int id, ButtonState &buttonState)
    {
      return (m_buttonState[id] == 0) && (buttonState[id] == 1);
    }

    bool buttonDiff(int id, ButtonState &buttonState)
    {
      return m_buttonState[id] != buttonState[id];
    }

    void execAllButtonActions(ButtonState &buttonState);

    void buttonStart(ButtonState &buttonState);

    void buttonPause(ButtonState &buttonState);

    void buttonToggleMode(ButtonState &buttonState);

    void buttonPrevJoint(ButtonState &buttonState);

    void buttonNextJoint(ButtonState &buttonState);

    void buttonFineTune(ButtonState &buttonState);

    void buttonEStop(ButtonState &buttonState);

    void buttonGotoBalancedPos(ButtonState &buttonState)
    {
      if( buttonOffToOn(ButtonIdGotoBalPos, buttonState) )
      {
        gotoBalancedPos();
        m_fpState.m_bNewGoal = true;
        m_bPreemptMove = true;
      }
    }

    void buttonGotoParkedPos(ButtonState &buttonState)
    {
      if( buttonOffToOn(ButtonIdGotoParkedPos, buttonState) )
      {
        gotoParkedPos();
        m_fpState.m_bNewGoal = true;
        m_bPreemptMove = true;
      }
    }

    void buttonGotoZeroPt(ButtonState &buttonState)
    {
      if( buttonOffToOn(ButtonIdGotoZeroPt, buttonState) )
      {
        gotoZeroPt();
        m_fpState.m_bNewGoal = true;
        m_bPreemptMove = true;
      }
    }

    void buttonCloseGripper(ButtonState &buttonState);

    void buttonOpenGripper(ButtonState &buttonState);

    void buttonMoveJoints(ButtonState &buttonState);

    void buttonMoveFirstPerson(int joy);

    void buttonMoveShoulder(int joy);

    void buttonMoveElbow(int joy);
    
    void buttonRotateBase(ButtonState &buttonState);

    void buttonPitchWrist(ButtonState &buttonState);

    void buttonRotateWristCw(ButtonState &buttonState);

    void buttonRotateWristCcw(ButtonState &buttonState);


    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 
    // Support 
    //. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 

    void pause();

    void ready();

    void driveLEDsFigure8Pattern();

    bool hasActiveJoint(const std::string strJointName)
    {
      return m_mapGoalPos.find(strJointName) != m_mapGoalPos.end()? true: false;
    }

    bool hasActiveTrajectory()
    {
      return m_mapGoalPos.size() > 0? true: false;
    }

    bool hasWorkingTrajectory()
    {
      return m_msgJointTraj.joint_names.size() > 0? true: false;
    }

    /*
     * \brief Stop all unteleoperated joints.
     *
     * If a joint was being actively being control and now its not, stop.
     */
    void stopUnteleopJoints();

    /*
     * \brief Conditionally set a joint's trajectory to the working trajectory
     * point.
     *
     * If the joint has not been included in the current joint trajectory point
     * or that joint's goal position or velocity differ, then the new values
     * are added/replaced with the new values.
     *
     * \param strJointName    Joint name.
     * \param pos             Joint goal position (radians).
     * \param vel             Joint goal velocity (%).
     *
     * \return If added, returns the relevant index \h_ge 0 of the joint in
     * the trajectory point. Otherwise, -1 is returned.
     */
    ssize_t setJoint(const std::string &strJointName, double pos, double vel);

    ssize_t addJointToTrajectoryPoint(const std::string &strJointName);

    void clearWorkingTrajectory();
    
    void clearActiveTrajectory();
    
    void resetActiveTeleop();
  };

} // namespace hekateros_control


#endif // _HEK_TELEOP_H