hekateros_as_follow_traj.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_as_follow_traj.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_AS_TRAJECTORY_H
#define _HEKATEROS_AS_TRAJECTORY_H

//
// System and Boost
//
#include <sys/types.h>
#include <unistd.h>
#include <math.h>
#include <boost/bind.hpp>
#include <string>

//
// ROS
//
#include "ros/ros.h"

//
// ROS generated core, industrial, and hekateros messages.
//
#include "std_msgs/String.h"
#include "control_msgs/FollowJointTrajectoryAction.h"

//
// ROS generated action servers.
//
#include "actionlib/server/simple_action_server.h"

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

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


namespace hekateros_control
{
  class ASFollowTrajectory
  {
  public:
    // Monitoring tuning. Position tolerances are in radians.
    static const double TolWaypoint;  
    static const double TolGoal;      
    static const int    MaxIters;     

    ASFollowTrajectory(std::string name, HekaterosControl &node) :
      action_name_(name),       // action name
      node_(node),              // hekateros node
      as_(node.getNodeHandle(), // simple action server
          name,                       // action name
          boost::bind(&ASFollowTrajectory::execute_cb, this, _1),
                                      // execute callback
          false)                      // don't auto-start
    {
      // 
      // Optionally register the goal and feeback callbacks
      //
      as_.registerPreemptCallback(boost::bind(&ASFollowTrajectory::preempt_cb,
                                  this));

      // start the action server
      start();
    }

    virtual ~ASFollowTrajectory()
    {
    }

    void start()
    {
      as_.start();
    }

    void execute_cb(
                  const control_msgs::FollowJointTrajectoryGoalConstPtr &goal);

    void preempt_cb();

  protected:
    std::string       action_name_;         
    HekaterosControl &node_;                
    actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>
                                      as_;  
    control_msgs::FollowJointTrajectoryFeedback feedback_;
    control_msgs::FollowJointTrajectoryResult   result_;

    double  m_fTolerance;                   
    double  m_fWaypointDist;                
    int     m_iterMonitor;                  

    int moveToWaypoint(trajectory_msgs::JointTrajectory &jt, ssize_t iWaypoint);

    void monitorMove(trajectory_msgs::JointTrajectory &jt, ssize_t iWaypoint);

    bool atWaypoint(trajectory_msgs::JointTrajectory &jt, ssize_t iWaypoint)
    {
      return fabs(m_fWaypointDist) < m_fTolerance;
    }

    bool failedWaypoint(trajectory_msgs::JointTrajectory &jt,
                        ssize_t iWaypoint);

    void publishFeedback(ssize_t iWaypoint);
  };

} // namespace hekateros_control

#endif // _HEKATEROS_AS_TRAJECTORY_H