Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

Aibo3DControllerBehavior.h

Go to the documentation of this file.

//-*-c++-*-
#ifndef INCLUDED_Aibo3DControllerBehavior_h_
#define INCLUDED_Aibo3DControllerBehavior_h_

#include <iostream>
#include "Wireless/Wireless.h"
#include "Behaviors/BehaviorBase.h"
#include "Motion/MotionManager.h"
#include "Motion/RemoteControllerMC.h"
#include "Events/EventRouter.h"
#include "Events/EventBase.h"
#include "Shared/RobotInfo.h"
#include "Behaviors/Controller.h"
#include "Shared/WorldState.h"

//! gets input from the GUI
int aibo3dcontrollercmd_callback(char *buf, int bytes);
class Aibo3DControllerBehavior;

//! so aibo3dcontrollercmd_callback knows where to send the input from the GUI
Aibo3DControllerBehavior *aibo3dControllerBehavior = NULL;

  
//! Listens to aibo3d control commands coming in from the command port.
class Aibo3DControllerBehavior : public BehaviorBase {
 protected:
  MotionManager::MC_ID rcontrol_id; //!< remote controller motion command's id
 
  //! The input command stream socket
  Socket *cmdsock;

  float val[NumPIDJoints]; //!< the value to use for each of the PID joints
  char *fbuf;  //!< alias to val
  unsigned int pos; //!< a counter to know when we've gotten 4 frames

 private:
  Aibo3DControllerBehavior(const Aibo3DControllerBehavior&); //!< don't call
  Aibo3DControllerBehavior operator=(const Aibo3DControllerBehavior&); //!< don't call

 public:
  //! constructor
  Aibo3DControllerBehavior() :
    BehaviorBase(),
    rcontrol_id(MotionManager::invalid_MC_ID),
    cmdsock(wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048)),
    fbuf((char*)val), pos(0)
  { wireless->setDaemon(cmdsock);
    aibo3dControllerBehavior = this; }
  //! destructor
  virtual ~Aibo3DControllerBehavior() { aibo3dControllerBehavior = NULL; }

  //! processes input from the GUI
  int registerData(char *buf, int bytes) {
    int read=0;
    while (read<bytes) {
      fbuf[pos]=buf[read];
      pos++;
      read++;
      if (pos==NumPIDJoints*sizeof(float)) {
        updateRC();
        pos=0;
      }
    }
    return read;
  }

  void updateRC() {
    RemoteControllerMC *rcontrol = (RemoteControllerMC*)motman->checkoutMotion(rcontrol_id);
    for (unsigned int i=0; i<NumPIDJoints; i++)
      rcontrol->cmds[i]=val[i];
    rcontrol->setDirty();
    motman->checkinMotion(rcontrol_id);
  }
  
  virtual void DoStart() {
    // Behavior startup
    BehaviorBase::DoStart();
    for(unsigned int i=0; i<NumPIDJoints; i++)
      val[i]=state->outputs[i];
    // Enable remote control stream
    rcontrol_id = motman->addMotion(SharedObject<RemoteControllerMC>());
    updateRC();
    // Turn on wireless
    wireless->setReceiver(cmdsock->sock, aibo3dcontrollercmd_callback);
    wireless->listen(cmdsock->sock, config->main.aibo3d_port);
    // open gui
    /*    std::vector<std::string> tmp;
          tmp.push_back("Aibo3D Load Instructions");
          tmp.push_back("To load Aibo3D, you will need to install java3d\nand then run Tekkotsu/tools/aibo3d/");
          tmp.back()+=getGUIType();
          Controller::loadGUI("ControllerMsg","LoadAibo3d",getPort(),tmp);*/
    Controller::loadGUI(getGUIType(),getGUIType(),getPort());
  }

  virtual void DoStop() {
    Controller::closeGUI(getGUIType());
    // Close socket; turn wireless off
    wireless->close(cmdsock);
    // Disable remote control
    motman->removeMotion(rcontrol_id);
    // Total behavior stop
    BehaviorBase::DoStop();
  }

  //! returns string corresponding to the Java GUI which should be launched
  virtual std::string getGUIType() const { return "org.tekkotsu.aibo3d.Aibo3DPick"; }
  //! returns port number the Java GUI should connect to
  virtual unsigned int getPort() const { return config->main.aibo3d_port; }

  virtual std::string getName() const { return "Aibo 3D"; } //!< returns name of behavior
  static std::string getClassDescription() { 
    char tmp[20];
    sprintf(tmp,"%d",config->main.aibo3d_port);
    return std::string("Listens to aibo3d control commands coming in from port ")+tmp;
  }
};


int aibo3dcontrollercmd_callback(char *buf, int bytes) {
  if (aibo3dControllerBehavior!=NULL)
    return aibo3dControllerBehavior->registerData(buf, bytes);
  return 0;
}

/*! @file
 * @brief Defines Aibo3DControllerBehavior, which listens to commands from the Aibo3D gui and shows current state
 * @author alokl (Creator)
 *
 * $Author: ejt $
 * $Name: tekkotsu-1_5 $
 * $Revision: 1.1 $
 * $State: Rel $
 * $Date: 2003/10/10 00:45:01 $
 */

#endif 

Tekkotsu v1.5

Generated Fri Oct 10 15:51:57 2003 by Doxygen 1.3.4