hekateros_control.h

Go to the documentation of this file.

//
// Package:   RoadNarrows Robotics Hekateros Robotiic Manipulator ROS Package
//
// Link:      https://github.com/roadnarrows-robotics/hekateros
//
// ROS Node:  hekateros_control
//
// File:      hekateros_control.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 _HEKATEROS_CONTROL_H
#define _HEKATEROS_CONTROL_H

//
// System
//
#include <string>
#include <map>

//
// Boost libraries
//
#include <boost/bind.hpp>

//
// ROS
//
#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "control_msgs/FollowJointTrajectoryAction.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.
//
#include "hekateros_control/CalibrateAction.h"

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

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

//
// Node headers.
//
#include "hekateros_control.h"


namespace hekateros_control
{
  class HekaterosControl
  {
  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;

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

    virtual ~HekaterosControl();

    virtual int configure(const std::string &strCfgFile);

    int connect(const std::string &strDevDynabus,
                int                nBaudRateDynabus,
                const std::string &strDevArduino,
                int                nBaudRateArduino)
    {
      m_robot.connect(strDevDynabus, nBaudRateDynabus,
                      strDevArduino, nBaudRateArduino);
    }

    int disconnect()
    {
      m_robot.disconnect();
    }

    virtual void advertiseServices();

    virtual void clientServices()
    {
      // No client services
    }

    virtual void advertisePublishers(int nQueueDepth=10);

    virtual void subscribeToTopics(int nQueueDepth=10);

    virtual void publish();

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

    hekateros::HekRobot &getRobot()
    {
      return m_robot;
    }

    void updateJointStateMsg(hekateros::HekJointStatePoint &state,
                             sensor_msgs::JointState       &msg);

    void updateExtendedJointStateMsg(hekateros::HekJointStatePoint &state,
                                     HekJointStateExtended         &msg);

    void updateRobotStatusMsg(hekateros::HekRobotState     &status,
                              industrial_msgs::RobotStatus &msg);

    void updateExtendedRobotStatusMsg(hekateros::HekRobotState &status,
                                      HekRobotStatusExtended   &msg);

  protected:
    ros::NodeHandle    &m_nh;     
    double              m_hz;     
    hekateros::HekRobot m_robot;  

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

    // Messages for published data.
    sensor_msgs::JointState       m_msgJointState;  
    HekJointStateExtended         m_msgJointStateEx;
    industrial_msgs::RobotStatus  m_msgRobotStatus; 
    HekRobotStatusExtended        m_msgRobotStatusEx;

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

    bool calibrate(hekateros_control::Calibrate::Request  &req,
                   hekateros_control::Calibrate::Response &rsp);

    bool clearAlarms(hekateros_control::ClearAlarms::Request  &req,
                     hekateros_control::ClearAlarms::Response &rsp);

    bool closeGripper(hekateros_control::CloseGripper::Request  &req,
                      hekateros_control::CloseGripper::Response &rsp);

    bool estop(hekateros_control::EStop::Request  &req,
               hekateros_control::EStop::Response &rsp);

    bool freeze(hekateros_control::Freeze::Request  &req,
                hekateros_control::Freeze::Response &rsp);

    bool getProductInfo(hekateros_control::GetProductInfo::Request  &req,
                        hekateros_control::GetProductInfo::Response &rsp);

    bool gotoBalancedPos(hekateros_control::GotoBalancedPos::Request  &req,
                         hekateros_control::GotoBalancedPos::Response &rsp);

    bool gotoParkedPos(hekateros_control::GotoParkedPos::Request  &req,
                       hekateros_control::GotoParkedPos::Response &rsp);

    bool gotoZeroPt(hekateros_control::GotoZeroPt::Request  &req,
                    hekateros_control::GotoZeroPt::Response &rsp);

    bool isAlarmed(hekateros_control::IsAlarmed::Request  &req,
                   hekateros_control::IsAlarmed::Response &rsp);

    bool isCalibrated(hekateros_control::IsCalibrated::Request  &req,
                      hekateros_control::IsCalibrated::Response &rsp);

    bool isDescLoaded(hekateros_control::IsDescLoaded::Request  &req,
                      hekateros_control::IsDescLoaded::Response &rsp);

    bool openGripper(hekateros_control::OpenGripper::Request  &req,
                     hekateros_control::OpenGripper::Response &rsp);

    bool release(hekateros_control::Release::Request  &req,
                 hekateros_control::Release::Response &rsp);

    bool resetEStop(hekateros_control::ResetEStop::Request  &req,
                    hekateros_control::ResetEStop::Response &rsp);

    bool setRobotMode(hekateros_control::SetRobotMode::Request  &req,
                      hekateros_control::SetRobotMode::Response &rsp);

    bool stop(hekateros_control::Stop::Request  &req,
              hekateros_control::Stop::Response &rsp);


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

    void publishJointState();

    void publishRobotStatus();


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

    void execJointCmd(const trajectory_msgs::JointTrajectory &jt);
  };

} // namespace hc


#endif // _HEKATEROS_CONTROL_H