console.h

Go to the documentation of this file.

#ifndef __THREEMXL_CONSOLE_H
#define __THREEMXL_CONSOLE_H

#include <ros/ros.h>
#include <threemxl/platform/hardware/dynamixel/CDxlGeneric.h>

class DxlROSConsole;

class DxlROSCommand
{
  public:
        typedef std::vector<std::string> ArgList;

  protected:
        DxlROSConsole *console_; 
        size_t nargs_;       
        std::string name_;   
        std::string help_;   

  public:
        DxlROSCommand(DxlROSConsole *console, std::string name, size_t nargs, std::string help) :
                console_(console), nargs_(nargs), name_(name), help_(help)
        {
        }
        
        bool match(std::string cmd, ArgList args)
        {
          return cmd == name_ && args.size() == nargs_;
        }
        
        bool matchHelp(std::string cmd, ArgList args)
        {
          return cmd == name_;
        }

        std::string getHelp()
        {
          return help_;
        }

        bool execute(ArgList args);
};

class Lockable
{
  protected:
    pthread_mutex_t mutex_; 
    pthread_cond_t condition_; 

  public:
    Lockable()
    {
      pthread_mutex_init(&mutex_, NULL);
      pthread_cond_init(&condition_, NULL);
    }
    
    virtual ~Lockable()
    {
      pthread_mutex_destroy(&mutex_);
      pthread_cond_destroy(&condition_);
    }
  
    void lock() { pthread_mutex_lock(&mutex_); }
    
    void unlock() { pthread_mutex_unlock(&mutex_); }
    

    void wait(double interval);
    
    /* \note Must be locked. */
    void signal() { pthread_cond_signal(&condition_); }
};

class DxlROSConsole : public Lockable
{
  public:
    typedef std::vector<DxlROSCommand> CommandList;
    typedef std::vector<CDxlGeneric*>  MotorList;
        
  protected:
    ros::NodeHandle nh_;   
    CDxlGeneric *motor_;   
    MotorList motors_;     
    LxSerial serial_port_; 
    CommandList commands_; 
    char *path_;           
    pthread_t hb_thread_;  
    double hb_interval_;   

  public:
    DxlROSConsole() : nh_("~"), motor_(NULL), path_(NULL), hb_interval_(0)
    {
      // Spawn heartbeat thread
      pthread_create(&hb_thread_, NULL, spin_hb, (void*)this);
    }
    

    ~DxlROSConsole()
    {
      // Prod heartbeat thread. Does not actually work if ros::ok() is true
      lock();
      signal();
      unlock();
      
      // Wait for heartbeat thread to finish
      pthread_join(hb_thread_, NULL);
    
      for (size_t ii=0; ii < motors_.size(); ++ii)
        delete motors_[ii];
        
      if (serial_port_.is_port_open())
        serial_port_.port_close();
          
      nh_.shutdown();
    }
    
    CDxlGeneric *getMotor() { return motor_; }
    
    CDxlGeneric *createMotor();
    

    void setMotor(int id);
    
    MotorList &getMotors() { return motors_; }
    

    void setHeartbeatInterval(double interval);


    void init(char *path);
    

    void execute(std::string cmd);
    

    void spin();
    

    static void *spin_hb(void *obj);
};    

#endif /* __THREEMXL_CONSOLE_H */