Esempio n. 1
0
  void initialize()
  {
    ALValue angles;
    ALValue names;

    // Create a proxy to ALVideoDevice on the robot.


    float fractionMaxSpeed = 0.1f;

    names = ALValue::array("HeadYaw","HeadPitch","LShoulderPitch","LShoulderRoll","LElbowYaw","LElbowRoll");
    angles = ALValue::array(0.0f, -0.0f, 1.4f, 0.35f, -1.4f, -1.05f);
    motionProxy->setAngles(names, angles, fractionMaxSpeed);

    names = ALValue::array("LHipYawPitch","LHipRoll","LHipPitch","LKneePitch","LAnklePitch","LAnkleRoll");
    angles = ALValue::array(0.0f, -0.0f, -0.44f, 0.7f, -0.35, 0.0f);
    motionProxy->setAngles(names, angles, fractionMaxSpeed);

    names = ALValue::array("RHipYawPitch","RHipRoll","RHipPitch","RKneePitch","RAnklePitch","RAnkleRoll");
    angles = ALValue::array(0.0f, -0.0f, -0.44f, 0.7f, -0.35f, 0.0f);
    motionProxy->setAngles(names, angles, fractionMaxSpeed);

    names = ALValue::array("RShoulderPitch","RShoulderRoll","RElbowYaw","RElbowRoll");
    angles = ALValue::array(1.4f, -0.35f, 1.4f, 1.05f);
    motionProxy->setAngles(names, angles, fractionMaxSpeed);
  }
Esempio n. 2
0
int main(int argc, char *argv[]) {
	std::cout << "..::: starting NAOJI" << std::endl;
	std::cout << "Copyright (c) 2009, Asura-FIT" << std::endl;
	std::cout << "Copyright (c) 2007, Aldebaran-robotics" << std::endl
			<< std::endl;

	int i = 1;
	std::string brokerName = "naoji_main";
	std::string brokerIP = "127.0.0.1";
	int brokerPort = 0;
	// Default parent broker IP
	std::string parentBrokerIP = "127.0.0.1";
	// Default parent broker port
	int parentBrokerPort = kBrokerPort;

	// checking options
	while (i < argc) {
		if (argv[i][0] != '-')
			return usage(argv[0]);
		else if (std::string(argv[i]) == "-b")
			brokerIP = std::string(argv[++i]);
		else if (std::string(argv[i]) == "-p")
			brokerPort = atoi(argv[++i]);
		else if (std::string(argv[i]) == "-pip")
			parentBrokerIP = std::string(argv[++i]);
		else if (std::string(argv[i]) == "-pport")
			parentBrokerPort = atoi(argv[++i]);
		else if (std::string(argv[i]) == "-h")
			return usage(argv[0]);
		i++;
	}

	std::cout << "Try to connect to parent Broker at ip :" << parentBrokerIP
			<< " and port : " << parentBrokerPort << std::endl;
	std::cout << "Start the server bind on this ip :  " << brokerIP
			<< " and port : " << brokerPort << std::endl;

	  // Starting Broker
	 ALPtr<ALBroker> pBroker = ALBroker::createBroker(brokerName, brokerIP, brokerPort, parentBrokerIP,  parentBrokerPort);

	// Starting Broker ALPtr<ALBroker> pBroker = ALBroker::createBroker(brokerName, brokerIP, brokerPort, parentBrokerIP,  parentBrokerPort);
	pBroker->setBrokerManagerInstance(ALBrokerManager::getInstance());

	ALModule::createModule<NaojiModule>(pBroker, "NaojiModule");

	struct sigaction new_action;
	/* Set up the structure to specify the new action. */
	new_action.sa_handler = _terminationHandler;
	sigemptyset(&new_action.sa_mask);
	new_action.sa_flags = 0;

	sigaction(SIGINT, &new_action, NULL);

	pBroker.reset(); // because of while( 1 ), broker counted by brokermanager
	while (1) {
		SleepMs(100);
	}
}
Esempio n. 3
0
ALImageTranscriber::ALImageTranscriber(shared_ptr<Sensors> s,
                                       ALPtr<ALBroker> broker)
    : ThreadedImageTranscriber(s,"ALImageTranscriber"),
      log(), camera(), lem_name(""), camera_active(false),
      image(reinterpret_cast<uint16_t*>(new uint8_t[IMAGE_BYTE_SIZE])),
      table(new unsigned char[yLimit * uLimit * vLimit]),
      params(y0, u0, v0, y1, u1, v1, yLimit, uLimit, vLimit)
{

    try {
        log = broker->getLoggerProxy();
        // Possible values are
        // lowDebug, debug, lowInfo, info, warning, error, fatal
        log->setVerbosity("error");
    }catch (ALError &e) {
        cout << "Could not create a proxy to ALLogger module" << endl;
    }

#ifdef USE_VISION
    registerCamera(broker);
    if(camera_active) {
        try{
            initCameraSettings(BOTTOM_CAMERA);
        }catch(ALError &e){
            cout << "Failed to init the camera settings:"<<
                e.toString()<< endl;
            camera_active = false;
        }
    } else {
        cout << "\tCamera is inactive!" << endl;
    }
#endif

    initTable("/home/nao/naoqi/lib/naoqi/table.mtb");
}
Esempio n. 4
0
ALImageTranscriber::ALImageTranscriber(shared_ptr<Synchro> synchro,
                                       shared_ptr<Sensors> s,
                                       ALPtr<ALBroker> broker)
    : ThreadedImageTranscriber(s,synchro,"ALImageTranscriber"),
      log(), camera(), lem_name(""), camera_active(false),
      image(new unsigned char[IMAGE_BYTE_SIZE])
{
    try {
        log = broker->getLoggerProxy();
        // Possible values are
        // lowDebug, debug, lowInfo, info, warning, error, fatal
        log->setVerbosity("error");
    }catch (ALError &e) {
        std::cerr << "Could not create a proxy to ALLogger module" << std::endl;
    }

#ifdef USE_VISION
    registerCamera(broker);
    if(camera_active) {
        try{
        initCameraSettings(BOTTOM_CAMERA);
        }catch(ALError &e){
            cout << "Failed to init the camera settings:"<<e.toString()<<endl;
            camera_active = false;
        }
    }
    else
        cout << "\tCamera is inactive!" << endl;
#endif
}
Esempio n. 5
0
void ALImageTranscriber::registerCamera(ALPtr<ALBroker> broker) {
    try {
        camera = broker->getProxy("NaoCam");
        camera_active =true;
    }catch (ALError &e) {
        log->error("ALImageTranscriber",
                   "Could not create a proxy to NaoCam module");
        camera_active =false;
        return;
    }

    lem_name = "ALImageTranscriber_LEM";
    int format = NAO_IMAGE_SIZE;
    int colorSpace = NAO_COLOR_SPACE;
    int fps = DEFAULT_CAMERA_FRAMERATE;

    int resolution = format;



#ifdef DEBUG_MAN_INITIALIZATION
    printf("  Registering LEM with format=%i colorSpace=%i fps=%i\n", format,
           colorSpace, fps);
#endif

    try {
        lem_name = camera->call<std::string>("register", lem_name, format,
                                             colorSpace, fps);
        cout << "Registered Camera: " << lem_name << " successfully"<<endl;
    } catch (ALError &e) {
        cout << "Failed to register camera" << lem_name << endl;
        camera_active = false;
//         SleepMs(500);

//         try {
//             printf("LEM failed once, trying again\n");
//             lem_name = camera->call<std::string>("register", lem_name, format,
//                                                  colorSpace, fps);
//         }catch (ALError &e2) {
//             log->error("ALImageTranscriber", "Could not call the register method of the NaoCam "
//                        "module\n" + e2.toString());
//             return;
//         }
    }

}
Esempio n. 6
0
  void shoot()
  {
    ALValue angles;
    ALValue names;
 
   //Right Foot support 
    float fractionMaxSpeed = 0.07f;
    names = ALValue::array("RAnkleRoll","LAnkleRoll","RHipRoll","LHipRoll");
    angles = ALValue::array(-0.33f, -0.33f, 0.25f, 0.33f);
    motionProxy->angleInterpolationWithSpeed(names, angles, fractionMaxSpeed);

    //Rise Left Foot
    names = ALValue::array("LHipPitch", "LKneePitch","LAnklePitch");
    angles = ALValue::array(-0.523f, 1.398f, -0.875f);
    motionProxy->angleInterpolationWithSpeed(names, angles, fractionMaxSpeed);

    //Get Left Leg Position
    std::string effector = "LLeg"; 
    int space = 2; 
    bool useSensorValues = true; 
    std::vector<float> legPos = motionProxy->getPosition(effector, space, useSensorValues); 

    //Move Left Leg Forward
    legPos[0] += 0.20f;
    legPos[1] -= 0.05f;
    legPos[2] -= 0.05f;
    ALValue	times      = 0.5f; // seconds 
    bool isAbsolute = false; 
    int axisMask = 63;
    motionProxy->positionInterpolation(effector, space, legPos, axisMask, times, isAbsolute); 

    //Get Left Leg Position
    legPos = motionProxy->getPosition(effector, space, useSensorValues);

    //Move Left Leg Backward
    legPos[0] -= 0.2f;
    legPos[2] -= 0.095f;
    legPos[4] -= 0.4f;
    times      = 2.0f; // seconds  
    motionProxy->positionInterpolation(effector, space, legPos, axisMask, times, isAbsolute); 
  }
Esempio n. 7
0
 void ALRosGatherer::init(ALPtr<ALBroker> broker) {
   fDataGathererProxy = broker->getProxy("ALDataGatherer");
 }
Esempio n. 8
0
int main( int argc, char *argv[] )
{
#ifdef REDIRECT_C_STDERR
  // Redirect stderr to stdout
  FILE *_syderr = stderr;
  stderr = stdout;
#endif

  std::cout << "..::: starting MANMODULE revision " << MANMODULE_VERSION_REVISION << " :::.." << std::endl;
  std::cout << "Copyright (c) 2007, Aldebaran-robotics" << std::endl << std::endl;

  int  i = 1;
  std::string brokerName = "manmodule";
  std::string brokerIP = "0.0.0.0";
  int brokerPort = 0 ;
  // Default parent broker IP
  std::string parentBrokerIP = "127.0.0.1";
  // Default parent broker port
  int parentBrokerPort = kBrokerPort;

  // checking options
  while( i < argc ) {
    if ( argv[i][0] != '-' ) return usage( argv[0] );
    else if ( std::string( argv[i] ) == "-b" )        brokerIP          = std::string( argv[++i] );
    else if ( std::string( argv[i] ) == "-p" )        brokerPort        = atoi( argv[++i] );
    else if ( std::string( argv[i] ) == "-pip" )      parentBrokerIP    = std::string( argv[++i] );
    else if ( std::string( argv[i] ) == "-pport" )    parentBrokerPort  = atoi( argv[++i] );
    else if ( std::string( argv[i] ) == "-h" )        return usage( argv[0] );
    i++;
  }

  // If server port is not set
  if ( !brokerPort )
    brokerPort = FindFreePort( brokerIP );

  std::cout << "Try to connect to parent Broker at ip :" << parentBrokerIP
            << " and port : " << parentBrokerPort << std::endl;
  std::cout << "Start the server bind on this ip :  " << brokerIP
            << " and port : " << brokerPort << std::endl;

  // Starting Broker
 ALPtr<ALBroker> pBroker = ALBroker::createBroker(brokerName, brokerIP, brokerPort, parentBrokerIP,  parentBrokerPort);
 pBroker->setBrokerManagerInstance(ALBrokerManager::getInstance());


  //<OGETINSTANCE> don't remove this comment
 //ALPtr<Man> manptr = ALModule::createModule<Man>(pBroker,"Man" );
  //</OGETINSTANCE> don't remove this comment

#ifndef _WIN32
  struct sigaction new_action;
  /* Set up the structure to specify the new action. */
  new_action.sa_handler = _terminationHandler;
  sigemptyset( &new_action.sa_mask );
  new_action.sa_flags = 0;

  sigaction( SIGINT, &new_action, NULL );
#endif

  //man = boost::shared_ptr<Man>(new Man(pBroker,"Man"));
  ALCreateMan(pBroker);

  //man->getTrigger()->await_off();
//   //   Not sure what the purpose of this modulegenerator code is: //EDIT -JS
   pBroker.reset(); // because of while( 1 ), broker counted by brokermanager
   while( 1)
   {
     SleepMs( 100 );
   }

  cout << "Main method finished" <<endl;


#ifdef _WIN32
  _terminationHandler( 0 );
#endif

  exit( 0 );
}