C++ ALValue::arraySetSize方法代码示例

void Kick::Recovering_state_code(void)
{
//BUILDER COMMENT. DO NOT REMOVE. begin Recovering_state_code
	ALValue path;
	path.arraySetSize(6);
	path[0] = 0.0;
	path[2] = 0.0;
	path[3] = 0.0;
	path[4] = 0.0;
	path[5] = 0.0;
	if(foot == LEFT)
	{
		path[1] = 0.05;
		pmotion->positionInterpolation("LLeg", 2, path, 63, 1.5, false);
		path[1] = -0.05;
		pmotion->positionInterpolation("RLeg", 2, path, 63, 1.5, false);
	}else
	{
		path[1] = -0.05;
		pmotion->positionInterpolation("RLeg", 2, path, 63, 1.5, false);
		path[1] = 0.05;
		pmotion->positionInterpolation("LLeg", 2, path, 63, 1.5, false);
	}

	path[0] = 0.0;
	path[1] = 0.00;
	path[2] = -0.03;
	path[3] = 0.0;
	path[4] = 0.0;
	path[5] = 0.0;

	pmotion->positionInterpolation("Torso", 2, path, 63, 1.0, false);

//BUILDER COMMENT. DO NOT REMOVE. end Recovering_state_code
}

/**
 * saveImageRemote : test remote image
 * @param pName path of the file
 */
void vision::testRemote(){

  //Now you can get the pointer to the video structure.
  ALValue results;
  results.arraySetSize(7);

  try
  {
    results = ( camera->call<ALValue>( "getImageRemote", name ) );
  }catch( ALError& e)
  {
    log->error( "vision", "could not call the getImageRemote method of the NaoCam module" );
  }

  if (results.getType()!= ALValue::TypeArray) return;

  const char* dataPointerIn =  static_cast<const char*>(results[6].GetBinary());
  int size = results[6].getSize();

  //You can get some informations of the image.
  int width = (int) results[0];
  int height = (int) results[1];
  int nbLayers = (int) results[2];
  int colorSpace = (int) results[3];
  long long timeStamp = ((long long)(int)results[4])*1000000LL + ((long long)(int)results[5]);

  // now you create an openCV image and you save it in a file.
  IplImage* image = cvCreateImage( cvSize( width, height ), 8, nbLayers );

//  image->imageData = ( char* ) imageIn->getFrame();
  image->imageData = ( char* ) dataPointerIn;

  std::string pName = "aaa";

  //const char* imageName = ( pName + DecToString(results[4]) + ".jpg").c_str();
  const char* imageName = ( pName + "test" + ".jpg").c_str();

printf("imageName %s\n", imageName);
  cvSaveImage( imageName, image );
printf("image saved\n");

try
{
  results = ( camera->call<ALValue>( "releaseImage", name ) );
}catch( ALError& e)
{
  log->error( "vision", "could not call the releaseImage method of the NaoCam module" );
}

printf("image memory released\n");

//  cvReleaseImage( &image );
  cvReleaseImageHeader(&image);
printf("image released\n");
printf("testRemote finished\n");
}

/**
 * Creates the appropriate aliases with the DCM
 */
void NaoEnactor::initDCMAliases(){
    ALValue positionCommandsAlias;
    positionCommandsAlias.arraySetSize(3);
    positionCommandsAlias[0] = string("AllActuatorPosition");
    positionCommandsAlias[1].arraySetSize(Kinematics::NUM_JOINTS);

    ALValue hardCommandsAlias;
    hardCommandsAlias.arraySetSize(3);
    hardCommandsAlias[0] = string("AllActuatorHardness");
    hardCommandsAlias[1].arraySetSize(Kinematics::NUM_JOINTS);

    for (unsigned int i = 0; i<Kinematics::NUM_JOINTS; i++){
        positionCommandsAlias[1][i] = jointsP[i];
        hardCommandsAlias[1][i] = jointsH[i];
    }

    dcmProxy->createAlias(positionCommandsAlias);
    dcmProxy->createAlias(hardCommandsAlias);
}

void SimuNaoProvider::initDCMAliases()
{
	ALValue positionCommandsAlias;
	positionCommandsAlias.arraySetSize(2);
	positionCommandsAlias[0] = std::string("AllActuatorPosition");
	
	ALValue hardCommandsAlias;
	hardCommandsAlias.arraySetSize(2);
	hardCommandsAlias[0] = std::string("AllActuatorHardness");

	positionCommandsAlias[1].arraySetSize(numOfJoints);
	hardCommandsAlias[1].arraySetSize(numOfJoints);
	for (int i=0;i<numOfJoints;i++)
	{
		positionCommandsAlias[1][i] = std::string(jointNames[i]) + "/Position/Actuator/Value";
		hardCommandsAlias[1][i] = std::string(jointNames[i]) + "/Hardness/Actuator/Value";
	}
	
	dcmProxy->createAlias(positionCommandsAlias);
	dcmProxy->createAlias(hardCommandsAlias);

}

/**
 * @brief 
 */
void oru_walk::initPosition()
{
    /// @attention Hardcoded parameter!
    unsigned int init_time = 1200;


    ALValue initPositionCommands;

    initPositionCommands.arraySetSize(6);
    initPositionCommands[0] = string("jointActuator");
    initPositionCommands[1] = string("ClearAll");
    initPositionCommands[2] = string("time-separate");
    initPositionCommands[3] = 0;
    initPositionCommands[4].arraySetSize(1);
    initPositionCommands[5].arraySetSize(JOINTS_NUM);
    for (int i = 0; i < JOINTS_NUM; i++)
    {
        initPositionCommands[5][i].arraySetSize(1);
    }
    initJointAngles (initPositionCommands[5]);
    for (int i = 0; i < LOWER_JOINTS_NUM; i++)
    {
        ref_joint_angles[i] = initPositionCommands[5][i][0];
    }



    // set time
    try
    {
        initPositionCommands[4][0] = dcm_proxy->getTime(init_time);
    }
    catch (const ALError &e)
    {
        ORUW_THROW_ERROR("Error on DCM getTime : ", e);
    }


    // send commands
    try
    {
        dcm_proxy->setAlias(initPositionCommands);
    }
    catch (const AL::ALError &e)
    {
        ORUW_THROW_ERROR("Error with DCM setAlias : ", e);
    }

    qi::os::msleep(init_time);
    qiLogInfo ("module.oru_walk", "Execution of initPosition() is finished.");
}

int Sensors::Execute()
{
	if(firstrun)
	{
		//Starting US Sensors
		ALValue commands;
		commands.arraySetSize(3);
		commands[0] = string("Device/SubDeviceList/US/Actuator/Value");
		commands[1] = string("Merge");
		commands[2].arraySetSize(1);
		commands[2][0].arraySetSize(2);
		commands[2][0][0] = 68.0;
		commands[2][0][1] = dcm->getTime(10);
		dcm->set(commands);
		rtm.start();
#ifndef KROBOT_IS_REMOTE
		KAlBroker::Instance().GetBroker()->getProxy("DCM")->getModule()->atPostProcess(KALBIND(&Sensors::synchronisedDCMcallback , this));
#endif
		firstrun = false;
	}

#ifdef KROBOT_IS_REMOTE
	//Fetch into vectors
	jointaccess.GetValues(jointValues);
	sensoraccess.GetValues(sensorValues);
	fetchValues();
	publishData(ASM, "sensors");

	if(updateButtons())
	{
		publishSignal(BM, "buttonevents");
	}

#endif
	float oldvalue;
	vector<float> RobotValues = motion->getRobotPosition(true);

	//A vector containing the World Absolute Robot Position. (Absolute Position X, Absolute Position Y, Absolute Angle Z)
	for (unsigned i = 0; i < RobotValues.size(); i++)
	{
		oldvalue = RPM.sensordata(i).sensorvalue();
		RPM.mutable_sensordata(i)->set_sensorvalue(RobotValues[i]);
		RPM.mutable_sensordata(i)->set_sensorvaluediff(RobotValues[i] - oldvalue);
	}

	RPM.set_timediff(timediff);
	publishData(RPM, "sensors");
	return 0;
}

void Kick::Initial_state_code(void)
{
//BUILDER COMMENT. DO NOT REMOVE. begin Initial_state_code

//Primero ir a PoseInit
pmotion->setWalkTargetVelocity(0.0, 0.0, 0.0, 0.7);

ALValue path;
path.arraySetSize(6);
path[0] = 0.0;
path[2] = 0.0;
path[3] = 0.0;
path[4] = 0.0;
path[5] = 0.0;


path[1] = 0.03;
pmotion->positionInterpolation("LLeg", 2, path, 63, 0.5, false);
path[1] = -0.03;
pmotion->positionInterpolation("RLeg", 2, path, 63, 0.5, false);

HPoint3D ball3D;
/*HPoint2D ball2D;

ball2D.x = _BallDetectorOld->getX()*160.0+80.0;
ball2D.y = _BallDetectorOld->getY()*120.0+60.0;
ball2D.h = 1.0;

_Kinematics->get3DPositionZ(ball3D, ball2D, 0.04);

path[0] = 0.0;
path[2] = -0.03;
path[3] = 0.0;
path[4] = 0.0;
path[5] = 0.0;
*/

ball3D.Y = 1.0;

if(ball3D.Y > 0.0)
	path[1] = -0.07;
else
	path[1] = 0.07;

pmotion->positionInterpolation("Torso", 2, path, 63, 1.0, false);

//BUILDER COMMENT. DO NOT REMOVE. end Initial_state_code
}

/**
 * @brief Set stiffness of joints.
 *
 * @param[in] stiffnessValue value of stiffness [0;1]
 */
void oru_walk::setStiffness(const float &stiffnessValue)
{
    ALValue stiffnessCommands;


    if ((stiffnessValue < 0) || (stiffnessValue > 1))
    {
        ORUW_THROW("Wrong parameters");
    }


    // Prepare one dcm command:
    // it will linearly "Merge" all joint stiffness
    // from last value to "stiffnessValue" in 1 seconde
    stiffnessCommands.arraySetSize(3);
    stiffnessCommands[0] = std::string("jointStiffness");
    stiffnessCommands[1] = std::string("Merge");
    stiffnessCommands[2].arraySetSize(1);
    stiffnessCommands[2][0].arraySetSize(2);
    stiffnessCommands[2][0][0] = stiffnessValue;


    /// @attention Hardcoded parameter!
    unsigned int stiffness_change_time = 1000;
    try
    {
        stiffnessCommands[2][0][1] = dcm_proxy->getTime(stiffness_change_time);
    }
    catch (const ALError &e)
    {
        ORUW_THROW_ERROR("Error on DCM getTime : ", e);
    }


    try
    {
        dcm_proxy->set(stiffnessCommands);
    }
    catch (const ALError &e)
    {
        ORUW_THROW_ERROR("Error when sending stiffness to DCM : ", e);
    }

    qi::os::msleep(stiffness_change_time);
    qiLogInfo ("module.oru_walk", "Execution of setStiffness() is finished.");
}

JNIEXPORT jlong JNICALL Java_jp_ac_fit_asura_naoji_jal_JALMemory__1createQuery(
		JNIEnv *env, jclass, jlong jmemoryPtr, jobjectArray jstrings) {
	JALMemory *jmemory = reinterpret_cast<JALMemory*> (jmemoryPtr);
	assert(jmemory != NULL);
	Query *query = new Query(jmemory);
	jsize size = env->GetArrayLength(jstrings);
	ALValue names;
	names.arraySetSize(size);
	for (int i = 0; i < size; i++) {
		jstring jstr = static_cast<jstring> (env->GetObjectArrayElement(
				jstrings, i));
		jassert(env, jstr != NULL);
		names[i] = toString(env, jstr);
		env->DeleteLocalRef(jstr);
	}
	query->names = names;

	return reinterpret_cast<jlong> (query);
}

Agent::Agent(AL::ALPtr<AL::ALBroker> pBroker, const std::string& pName)
   : ALModule(pBroker, pName),
     alwalk_state(INACTIVE),
     shuttingDown(false),
     skipped_frames(0),
     sit_step(-1.0f),
     limp(true),
     head_limp(false),
     chest_down(0),
     chest_up(0),
     chest_presses(0),
     old_battery(0),
     old_battery_status(0) {
   uint8_t i;

   // need to initialise first, before using log
   log = new ALLoggerProxy(pBroker);
   if (log == NULL)
      throw ALERROR(name, "constructor", "log == NULL");

   log->info(name, "Constructing");

   try {
      po::options_description cmdline_options =
            store_and_notify(std::vector<string>(0), vm, NULL);
      wirelessIwconfigArgs = vm["network.wireless.iwconfig_flags"].as<string>();
      wirelessIfconfigArgs =
                      vm["network.wireless.static.ifconfig_flags"].as<string>();
      wiredIfconfigArgs =
                         vm["network.wired.static.ifconfig_flags"].as<string>();

      wirelessStatic = vm["network.wireless.static"].as<bool>();
      wiredStatic = vm["network.wired.static"].as<bool>();

      playerNum = vm["player.number"].as<int>();
      teamNum = vm["player.team"].as<int>();

      doNetworking();
   } catch(po::error& e) {
      log->error(name, "failed parsing command line arguments");
      log->error(name, e.what());
   } catch(std::exception& e) {
      log->error(name, "failed parsing command line arguments");
      log->error(name, e.what());
   }

   dcm = new DCMProxy(pBroker);
   if (dcm == NULL)
      throw ALERROR(name, "constructor", "dcm == NULL");

   memory = new ALMemoryProxy(pBroker);
   if (memory == NULL)
      throw ALERROR(name, "constructor", "memory == NULL");

   motion = new ALMotionProxy(pBroker);
   motion->setWalkArmsEnable(false, false);
   ALValue config;
   config.arraySetSize(13);
   for (i = 0; i < 13; ++i)
      config[i].arraySetSize(2);
   config[0][0]  = "WALK_MAX_TRAPEZOID",       config[0][1]  = 4.5;    // 4.5
   config[1][0]  = "WALK_MIN_TRAPEZOID",       config[1][1]  = 3.5;    // 3.5
   config[2][0]  = "WALK_STEP_MAX_PERIOD",     config[2][1]  = 30;     // 30
   config[3][0]  = "WALK_STEP_MIN_PERIOD",     config[3][1]  = 21;     // 21
   config[4][0]  = "WALK_MAX_STEP_X",          config[4][1]  = 0.04;   // 0.04
   config[5][0]  = "WALK_MAX_STEP_Y",          config[5][1]  = 0.04;   // 0.04
   config[6][0]  = "WALK_MAX_STEP_THETA",      config[6][1]  = 20;     // 20
   config[7][0]  = "WALK_STEP_HEIGHT",         config[7][1]  = 0.015;  // 0.015
   config[8][0]  = "WALK_FOOT_SEPARATION",     config[8][1]  = 0.095;  // 0.095
   config[9][0]  = "WALK_FOOT_ORIENTATION",    config[9][1]  = 0;      // 0
   config[10][0] = "WALK_TORSO_HEIGHT",        config[10][1] = 0.31;   // 0.31
   config[11][0] = "WALK_TORSO_ORIENTATION_X", config[11][1] = 0;      // 0
   config[12][0] = "WALK_TORSO_ORIENTATION_Y", config[12][1] = 0;      // 0
   motion->setMotionConfig(config);

   leg_names.push_back("LHipYawPitch"), stand_angles.push_back(0.f);
   leg_names.push_back("LHipRoll"), stand_angles.push_back(0.f);
   leg_names.push_back("LHipPitch"), stand_angles.push_back(DEG2RAD(-30.f));
   leg_names.push_back("LKneePitch"), stand_angles.push_back(DEG2RAD(60.f));
   leg_names.push_back("LAnklePitch"), stand_angles.push_back(DEG2RAD(-30.f));
   leg_names.push_back("LAnkleRoll"), stand_angles.push_back(0.f);
   leg_names.push_back("RHipRoll"), stand_angles.push_back(0.f);
   leg_names.push_back("RHipPitch"), stand_angles.push_back(DEG2RAD(-30.f));
   leg_names.push_back("RKneePitch"), stand_angles.push_back(DEG2RAD(60.f));
   leg_names.push_back("RAnklePitch"), stand_angles.push_back(DEG2RAD(-30.f));
   leg_names.push_back("RAnkleRoll"), stand_angles.push_back(0.f);

   // open shared memory as RW, create if !exist, permissions 600
   shared_fd = shm_open(AGENT_MEMORY, O_RDWR | O_CREAT, 0600);
   if (shared_fd < 0)
      throw ALERROR(name, "constructor", "shm_open() failed");
   // make shared memory file correct size
   if (ftruncate(shared_fd, sizeof(AgentData)) == -1)
      throw ALERROR(name, "constructor", "ftruncate() failed");
   // map shared memory to process memory
   shared_data = (AgentData*) mmap(NULL, sizeof(AgentData),
                                   PROT_READ | PROT_WRITE,
                                   MAP_SHARED, shared_fd, 0);
   if (shared_data == MAP_FAILED)
      throw ALERROR(name, "constructor", "mmap() failed");
//.........这里部分代码省略.........

void Agent::preCallback() {
   // Avoid race condition on shutdown
   if (shuttingDown) {
      return;
   }

   const unsigned int now = timeNow();
   shared_data->actuators_read = shared_data->actuators_latest;
   JointValues& joints = shared_data->joints[shared_data->actuators_read];
   SensorValues& sensors = shared_data->sensors[shared_data->sensors_latest];

   doLEDs(shared_data->leds[shared_data->actuators_read]);
   if (limp || shared_data->standing) {
      uint8_t i;
      for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
         head_angle_command[5][i][0] = sit_angles[i];
         head_stiffness_command[5][i][0] = 0.0f;
      }
      for (i = Joints::LShoulderPitch; i < Joints::NUMBER_OF_JOINTS; ++i) {
         angle_command[5][i-2][0] = sit_angles[i];
         stiffness_command[5][i-2][0] = 0.0f;
      }

      // Make chest button blink green if limp but still in contact
      // or blink red if not in contact or purple if ready to stand
      float blink = now / 200 & 1;
      if (shared_data->standing) {
         led_command[5][LEDs::ChestRed][0] = blink;
         led_command[5][LEDs::ChestGreen][0] = blink;
         led_command[5][LEDs::ChestBlue][0] = 0.0;
      } else if (skipped_frames <= MAX_SKIPS) {
         led_command[5][LEDs::ChestRed][0] = 0.0f;
         led_command[5][LEDs::ChestGreen][0] = blink;
         led_command[5][LEDs::ChestBlue][0] = 0.0f;
      } else {
         led_command[5][LEDs::ChestRed][0] = blink;
         led_command[5][LEDs::ChestGreen][0] = 0.0f;
         led_command[5][LEDs::ChestBlue][0] = 0.0f;
      }
   } else if (skipped_frames <= MAX_SKIPS && sit_step == -1.0f) {
      uint8_t i;
      for (i = Joints::HeadYaw; i <= Joints::HeadPitch; ++i) {
         head_angle_command[5][i][0] = joints.angles[i];
         head_stiffness_command[5][i][0] = joints.stiffnesses[i];
      }
      if (joints.AL_command == AL_ON) {
         static int stand_t = 0;
         motion->setStiffnesses("Body", 0.66);
         ALValue config;
         switch (alwalk_state) {
         case INACTIVE:
            config.arraySetSize(13);
            for (i = 0; i < 13; ++i)
               config[i].arraySetSize(2);
            config[0][0]  = "WALK_MAX_TRAPEZOID";
            config[0][1]  = 4.5;    // 4.5
            config[1][0]  = "WALK_MIN_TRAPEZOID";
            config[1][1]  = 3.5;    // 3.5
            config[2][0]  = "WALK_STEP_MAX_PERIOD";
            config[2][1]  = 30;     // 30
            config[3][0]  = "WALK_STEP_MIN_PERIOD";
            config[3][1]  = 21;     // 21
            config[4][0]  = "WALK_MAX_STEP_X";
            config[4][1]  = 0.04;   // 0.04
            config[5][0]  = "WALK_MAX_STEP_Y";
            config[5][1]  = 0.04;   // 0.04
            config[6][0]  = "WALK_MAX_STEP_THETA";
            config[6][1]  = 20;     // 20
            config[7][0]  = "WALK_STEP_HEIGHT";
            config[7][1]  = 0.015;  // 0.015
            config[8][0]  = "WALK_FOOT_SEPARATION";
            config[8][1]  = 0.095;  // 0.095
            config[9][0]  = "WALK_FOOT_ORIENTATION";
            config[9][1]  = 0;      // 0
            config[10][0] = "WALK_TORSO_HEIGHT";
            config[10][1] = joints.AL_height;   // 0.31
            config[11][0] = "WALK_TORSO_ORIENTATION_X";
            config[11][1] = 0;      // 0
            config[12][0] = "WALK_TORSO_ORIENTATION_Y";
            config[12][1] = 0;      // 0
            motion->setMotionConfig(config);
            stand_angles.clear();
            stand_angles.push_back(0.f);
            stand_angles.push_back(0.f);
            stand_angles.push_back(-joints.AL_bend);
            stand_angles.push_back(2*joints.AL_bend);
            stand_angles.push_back(-joints.AL_bend);
            stand_angles.push_back(0.f);
            stand_angles.push_back(0.f);
            stand_angles.push_back(-joints.AL_bend);
            stand_angles.push_back(2*joints.AL_bend);
            stand_angles.push_back(-joints.AL_bend);
            stand_angles.push_back(0.f);
            alwalk_state = WALKING;
            break;
         case WALKING:
            if (joints.AL_stop) {
               // motion->walkTo(0.001, 0.001, 0.001);
               motion->post.angleInterpolation(leg_names, stand_angles,
                                               0.25, true);
//.........这里部分代码省略.........

ALValue vision::getBalls() {

	ALValue resultBallRect;
	resultBallRect.arraySetSize(4);

	resultBallRect[0] = 0;
	resultBallRect[1] = 0;
	resultBallRect[2] = 0;
	resultBallRect[3] = 0;

	//First you have to declare an ALVisionImage to get the video buffer.
	// ( definition included in alvisiondefinitions.h and alvisiondefinitions.cpp )
	ALVisionImage* imageIn;

	//Now you can get the pointer to the video structure.
	try
	{
	imageIn = ( ALVisionImage* ) ( camera->call<int>( "getImageLocal", name ) );
	}catch( ALError& e)
	{
	log->error( "vision", "could not call the getImageLocal method of the NaoCam module" );
	}

	//You can get some informations of the image.
	int width = imageIn->fWidth;
	int height = imageIn->fHeight;
	int nbLayers = imageIn->fNbLayers;
	int colorSpace = imageIn->fColorSpace;
	long long timeStamp = imageIn->fTimeStamp;
	int seconds = (int)(timeStamp/1000000LL);

//	log->info( "vision", "Creating OpenCV image" );

	//You can get the pointer of the image.
	uInt8 *dataPointerIn = imageIn->getFrame();

	// now you create an openCV image and you save it in a file.
	IplImage* src = cvCreateImage( cvSize( width, height ), 8, nbLayers );

	//  src->imageData = ( char* ) imageIn->getFrame();
	src->imageData = ( char* ) dataPointerIn;

//log->info( "vision", "Searching field" );

	IplImage* mask = 0;
	IplImage* imageClipped = 0;

	// Get field
	CvRect* fieldRect = new CvRect[1];

//printf("before getLargestColoredContour\n");
	// Green field
	// parameters for pan/tilt camera
	//CvSeq* field = getLargestColoredContour(src, 155, 5, 100, 300, fieldRect);
	// parameters for Nao camera in lab
//    CvSeq* field = getLargestColoredContour(src, 175, 30, 25, 1000, fieldRect);
    // Params for WEBOTS
    CvSeq* field = getLargestColoredContour(src, 125, 30, 25, 100, &fieldRect, 1)[0];

    if (field != NULL) {
//    	printf("Field: %d, %d, %d, %d\n", fieldRect.x, fieldRect.y, fieldRect.width, fieldRect.height);

//log->info( "vision", "Searching ball1" );

		CvSize imageSize = cvSize(src->width, src->height);
		mask = cvCreateImage( imageSize, 8, 1 );
		cvZero(mask);

		CvScalar colorWHITE = CV_RGB(255, 255, 255);

		int elementCount = field->total;
		CvPoint* temp = new CvPoint[elementCount];
		CvPoint pt0 = **CV_GET_SEQ_ELEM( CvPoint*, field, elementCount - 1 );
		for (int i = 0; i < elementCount; i++) {
			CvPoint pt = **CV_GET_SEQ_ELEM( CvPoint*, field, i );
			temp[i].x = pt.x;
			temp[i].y = pt.y;
		}
		cvFillConvexPoly(mask, temp, elementCount, colorWHITE, 8, 0);

		imageClipped = cvCreateImage( imageSize, 8, 3 );
		cvZero(imageClipped);
		cvCopy(src, imageClipped, mask);

		// Get ball
		CvRect* ballRect= new CvRect[10];

//log->info( "vision", "Searching ball2" );
	    // parameters for pan/tilt camera
	    //getLargestColoredContour(imageClipped, 17, 10, 100, 50, ballRect);
	    // parameters for Nao camera in lab
//		CvSeq* ballHull = getLargestColoredContour(imageClipped, 40, 25, 50, 30, ballRect);
		// Params for webots
		CvSeq** ballHull = getLargestColoredContour(imageClipped, 55, 125, 50, 30, &ballRect, 0);

//log->info( "vision", "Searching ball3" );
        int* X_Arr= new int[10];
        int* Y_Arr= new int[10];
        int* Width_Arr= new int[10];
        int* Height_Arr= new int[10];
//.........这里部分代码省略.........

void * urgThread(void * arg) {

#if defined (__linux__)
  // thread name
  prctl(PR_SET_NAME, "ALLaser::urgThread", 0, 0, 0);
#endif

  ALValue urgdata;
  long *data = NULL;
  int data_max;
  int ret;
  int n;
  int i,imemory,refTime,end,sampleTime, beginThread;
  std::string valueName="Device/Laser/Value";

  /* Connection */

  connectToLaser();

  /* Reserve the Receive data buffer */
  data_max = urg_dataMax(&urg);
  if (data_max <= 0) {
    perror("data_max is less than 0");
    pthread_exit((void *)NULL);
  }
  data = (long*)malloc(sizeof(long) * data_max);
  memset(data, 0, sizeof(long) * data_max);

  if (data == NULL) {
    perror("data buffer");
    pthread_exit((void *)NULL);
  }
  /* prepare ALValue for ALMemory*/
  urgdata.arraySetSize(data_max);
  for (i=0; i< data_max;i++)
  {
    urgdata[i].arraySetSize(4);
    urgdata[i][0]= (int)0;
    urgdata[i][1]= (double)0.0;
    urgdata[i][2]= (int)0;
    urgdata[i][3]= (int)0;
  }
  /* insert ALvalue in ALMemory*/

  gSTM->insertData(valueName,urgdata);

  gSTM->insertData("Device/Laser/LaserEnable", (bool) 1);
  gSTM->insertData("Device/Laser/MinAngle",(float)((2.0 * M_PI)
          * (DEFAULT_MIN_ANGLE - MIDDLE_ANGLE) / RESOLUTION_LASER));
  gSTM->insertData("Device/Laser/MaxAngle",(float)((2.0 * M_PI)
          * (DEFAULT_MAX_ANGLE - MIDDLE_ANGLE) / RESOLUTION_LASER));
  gSTM->insertData("Device/Laser/MinLength",(float)(length_min));
  gSTM->insertData("Device/Laser/MaxLength",(float)(length_max));

  stringstream ss;
  ss << "ALLaser running";
  qiLogInfo("hardware.laser") << ss.str() << std::endl;

  while(1)
  {
    if(mode==SEND_MODE_ON){
      ret = urg_laserOn(&urg);
      if (ret < 0) {
        urg_exit(&urg, "urg_requestData()");
      }
      mode=MODE_ON;
      /* Connection */
      connectToLaser();
    }
    if(mode==SEND_MODE_OFF){
      scip_qt(&urg.serial_, NULL, ScipNoWaitReply);
      mode=MODE_OFF;
    }
    if(mode==MODE_ON){
      /* Request Data using GD-Command */
      ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
      if (ret < 0) {
        urg_exit(&urg, "urg_requestData()");
      }

      refTime = getLocalTime();
      /* Obtain Data */
      n = urg_receiveData(&urg, data, data_max);
      qiLogDebug("hardware.laser") << " n " << n << " expected " <<
            angle_max - angle_min << std::endl;
      if (n < 0) {
        urg_exit(&urg, "urg_receiveData()");
      }
      end= getLocalTime();
      sampleTime=end-refTime;

      imemory=0;
      for (i = 0; i < n; ++i) {
        int x, y;
        double angle = urg_index2rad(&urg, i);

        qiLogDebug("hardware.laser") << i << " angle " << angle <<
              " urgAngle " << urg_index2rad(&urg, i) <<
              " dist " << data[i] << std::endl;

//.........这里部分代码省略.........

/** Run the kick. */
void
NaoQiMotionKickTask::run()
{
  const char *shoot_hip_roll_name = NULL;
  const char *support_hip_roll_name = NULL;
  const char *shoot_hip_pitch_name = NULL;
  const char *support_hip_pitch_name = NULL;
  const char *shoot_knee_pitch_name = NULL;
  const char *shoot_ankle_pitch_name = NULL;
  const char *shoot_ankle_roll_name = NULL;
  const char *support_ankle_roll_name = NULL;

  float shoot_hip_roll = 0;
  float support_hip_roll = 0;
  float shoot_hip_pitch = 0;
  float support_hip_pitch = 0;
  float shoot_knee_pitch = 0;
  float shoot_ankle_pitch = 0;
  float shoot_ankle_roll = 0;
  float support_ankle_roll = 0;

  float BALANCE_HIP_ROLL = 0;
  float BALANCE_ANKLE_ROLL = 0;
  float STRIKE_OUT_HIP_ROLL = 0;

  if ( __leg == fawkes::HumanoidMotionInterface::LEG_LEFT ) {
    shoot_hip_roll_name = "LHipRoll";
    support_hip_roll_name = "RHipRoll";
    shoot_hip_pitch_name = "LHipPitch";
    support_hip_pitch_name = "RHipPitch";
    shoot_knee_pitch_name = "LKneePitch";
    shoot_ankle_pitch_name = "LAnklePitch";
    shoot_ankle_roll_name = "LAnkleRoll";
    support_ankle_roll_name = "RAnkleRoll";

    BALANCE_HIP_ROLL = 20;
    BALANCE_ANKLE_ROLL = -25;
    STRIKE_OUT_HIP_ROLL = 30;
  } else if (__leg == fawkes::HumanoidMotionInterface::LEG_RIGHT ) {
    shoot_hip_roll_name = "RHipRoll";
    support_hip_roll_name = "LHipRoll";
    shoot_hip_pitch_name = "RHipPitch";
    support_hip_pitch_name = "LHipPitch";
    shoot_knee_pitch_name = "RKneePitch";
    shoot_ankle_pitch_name = "RAnklePitch";
    shoot_ankle_roll_name = "RAnkleRoll";
    support_ankle_roll_name = "LAnkleRoll";

    BALANCE_HIP_ROLL = -20;
    BALANCE_ANKLE_ROLL = 25;
    STRIKE_OUT_HIP_ROLL = -30;
  }

  if (__quit)  return;
  goto_start_pos(0.2);

  ALValue names;
  ALValue target_angles;
  float speed = 0;

  // Balance on supporting leg
  names.arraySetSize(4);
  target_angles.arraySetSize(4);

  support_hip_roll = BALANCE_HIP_ROLL;
  shoot_hip_roll = BALANCE_HIP_ROLL;
  shoot_ankle_roll = BALANCE_ANKLE_ROLL;
  support_ankle_roll = BALANCE_ANKLE_ROLL;

  names = ALValue::array(support_hip_roll_name, shoot_hip_roll_name,
      support_ankle_roll_name, shoot_ankle_roll_name);
  target_angles = ALValue::array(deg2rad(support_hip_roll), deg2rad(shoot_hip_roll),
      deg2rad(support_ankle_roll), deg2rad(shoot_ankle_roll));
  speed = 0.15;

  //if (__quit)  return;
  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);

  names.clear();
  target_angles.clear();

  // Raise shooting leg
  names.arraySetSize(3);
  target_angles.arraySetSize(3);

  shoot_hip_roll = STRIKE_OUT_HIP_ROLL;
  shoot_knee_pitch = 90;
  shoot_ankle_pitch = -50;

  names = ALValue::array(shoot_hip_roll_name, shoot_knee_pitch_name, shoot_ankle_pitch_name);
  target_angles = ALValue::array(deg2rad(shoot_hip_roll), deg2rad(shoot_knee_pitch),
      deg2rad(shoot_ankle_pitch));
  speed = 0.2;

  if (__quit)  return;
  __almotion->angleInterpolationWithSpeed(names, target_angles, speed);

  names.clear();
  target_angles.clear();
//.........这里部分代码省略.........