void RobotController::onRecvMsg(RecvMsgEvent &evt)
{
	std::string sender = evt.getSender();

	// 送信者がゴミ認識サービスの場合
	char *all_msg = (char*)evt.getMsg();
	std::string msg;
	msg= evt.getMsg();

	if(msg == "start" && m_state ==0)
	{
		m_state = 1; // initial settings
	}
	else if(msg == "kitchen" && m_state ==0)
	{
		m_state = 10; // move to the kitchen
	}
	else if(msg == "lobby" && m_state ==0)
	{
		m_state = 20; // move to the lobby
	}
	else if(msg == "bed room" && m_state ==0)
	{
		m_state = 30; // move to the bed room
	}
	else
	{
		broadcastMsgToSrv("Task finished\n");
	}
}
Пример #2
0
void RobotController::onRecvMsg(RecvMsgEvent &evt)
{
	std::string sender = evt.getSender();
	std::string msg;
	msg= evt.getMsg();
	std::stringstream ss;
	ss << msg;
	std::string header;
	ss >> header;

	if (msg == "Task_start" && m_state == 0)
	{
		m_state = 10;      
		Kinect_data_flag = false;
		Record_Postures.clear();
	}

	if (msg == "Task_end") 
	{
		initialize();
	}

	if ( header == "KINECT_DATA_Sensor" && Kinect_data_flag == true)
	{
		Record_Kinect_Data(msg);
	}

}
void MyController::onRecvMsg(RecvMsgEvent &evt)
{

  // SimObj *stick = getObj("robot_test");
  SimObj *stick = getObj("ArrowStick");


  char *all_msg = (char*)evt.getMsg();
  // std::string msg;
  msg= evt.getMsg();

  LOG_MSG((msg.c_str()));


  char xPosStr[10]=" "; 
  char yPosStr[10]=" ";
  int result=0;

  result = sscanf(msg.c_str(), "%[^','],%[^',']", xPosStr, yPosStr ); 
  xPos = atof(xPosStr);
  yPos = atof(yPosStr);

  // LOG_MSG((" Position Received by Controller : %f %f ", xPos, yPos));
  //std::cout <<"xPos is" << xPos <<std::endl;

  messageCount++;
  std::cout << "The received mesage count is" << messageCount <<std::endl;

  // stick->setPosition(pos.x() + xPos, pos.y(), pos.z() + yPos);

}
Пример #4
0
void RobotController::onRecvMsg(RecvMsgEvent &evt)
{
  std::string sender = evt.getSender();

  // 送信者がゴミ認識サービスの場合
  char *all_msg = (char*)evt.getMsg();
  std::string msg;
  msg= evt.getMsg();

  if(msg == "go" && m_state ==20)
  {
        m_state = 0;
  }
  else if(msg == "take" && m_state == 7)
  {
        printf("Kinect is started on object\n");
        m_pointedObject = getPointedObjectName(m_avatar);
        m_state = 8;
  }
  else if(msg == "put" && m_state == 16)
  {
        printf("Kinect is started on trash \n");
        m_pointedtrash = getPointedTrashName(m_avatar);
        m_state = 17;
  }
  else
  {
        printf("Message is not accepted\n");
  }
}
void KinectService::onRecvMsg(RecvMsgEvent &evt) {

    std::string sender = evt.getSender();
    std::string msg = evt.getMsg();

    int strPos1 = 0;
    int strPos2;
    int strPos3;
    std::string headss;
    std::string ss = msg;

    strPos2 = ss.find("_", strPos1);
    headss.assign(ss, strPos1, strPos2-strPos1);
    if (headss == "Send") {
        Folder = "Motion_Data/";
        // Contol of body movement by KINECT
        File_ID = "";
        strPos3 = ss.find(".", strPos2+1);
        File_ID.assign(ss, strPos2+1, strPos3);
        File_ID += "dat";
        Folder += File_ID;
        std::cout << "The File path " <<  Folder << std::endl;
    }
    if (msg == "Start_motion" && sender == "moderator_0" )
    {
        in_stream_motion.open(Folder.c_str());
        std::string previousLine = "";
        while(getline(in_stream_motion,line)) // To get you all the lines.
        {
            list_motion.push_back(line);
        }
        in_stream_motion.close();
        Data_Size = 0;
        Start_motion = true;
    }
    if (msg == "Task_start"  && sender == "moderator_0")
    {
        Data_Size = 0;
        Start_t = clock();
        list_motion.clear();
        Send_Data = false;
    }
    if (msg == "Get_Data" && sender == "robot_000")
    {
        Send_Data = true;
    }
    if (msg == "Data_Off" && sender == "robot_000")
    {
        Send_Data = false;
    }

}
Пример #6
0
void MyController::onRecvMsg(RecvMsgEvent &evt) {

  //取得したメッセージを表示します
  string msg = evt.getMsg();
  //LOG_MSG(("msg : %s", msg.c_str()));
  SimObj *my = getObj(myname());

  if(strstr(msg.c_str()," "))
    {
      // phi theta に分ける 
      int n = 0;
      n = msg.find(" ");
      string phi_s = msg.substr(0,n);
      string theta_s = msg.substr(n+1);

      double phi = atof(theta_s.c_str());
      double theta = atof(phi_s.c_str());

      // 目玉関節を回転させる
      my->setJointAngle("LEYE_JOINT1",DEG2RAD(phi));
      my->setJointAngle("REYE_JOINT1",DEG2RAD(phi));
      my->setJointAngle("LEYE_JOINT0",DEG2RAD(theta));
      my->setJointAngle("REYE_JOINT0",DEG2RAD(theta));
    }
}
void MyController::onRecvMsg(RecvMsgEvent &evt)
{
	string msg = evt.getMsg();
	if (msg == "point1"){
		start = true;
	}
}
Пример #8
0
void MyController::onRecvMsg(RecvMsgEvent &evt)
{
	std::string msg = evt.getMsg();
	if(msg == "Task_start"){
		if(trials==0){
			m_state = 100;
		}
		else{
			m_state = 1100;
		}
		broadcastMsg("get message: Task_start");
	}
	else if(msg == "Task_end"){
		m_my->setWheelVelocity(0.0, 0.0);
		m_state = 0;
		broadcastMsg("get message: Task_end");
		trials++;
	}
	else if(msg == "Time_over"){
		m_my->setWheelVelocity(0.0, 0.0);
		m_state = 0;
	}
	/*else if(m_state == 207 && msg == "leave the elevator"){
		broadcastMsg("get message: leave the elevator");
		m_state = 208;
	}*/
}
void AvatarControllerByKinectV2::onRecvMsg(RecvMsgEvent &evt)
{
	try
	{
		std::string allMsg = evt.getMsg();

//		std::cout << "msg:" << allMsg << std::endl;

		// Decode message to sensor data of kinect v2.
		std::map<std::string, std::vector<std::string> > sensorDataMap = KinectV2SensorData::decodeSensorData(allMsg);

		if (sensorDataMap.find(MSG_KEY_DEV_TYPE) == sensorDataMap.end()){ return; }
		if(sensorDataMap[MSG_KEY_DEV_TYPE][0]     !=this->kinectV2DeviceManager.deviceType    ){ return; }
		if(sensorDataMap[MSG_KEY_DEV_UNIQUE_ID][0]!=this->kinectV2DeviceManager.deviceUniqueID){ return; }

		KinectV2SensorData sensorData;
		sensorData.setSensorData(sensorDataMap);

		ManNiiPosture posture = KinectV2DeviceManager::convertSensorData2ManNiiPosture(sensorData);

		// Set SIGVerse quaternions and positions
		SimObj *obj = getObj(myname());
		this->kinectV2DeviceManager.setRootPosition(obj, sensorData.rootPosition);
		KinectV2DeviceManager::setJointQuaternions2ManNii(obj, posture, sensorData);
	}
	catch(SimObj::Exception &err)
	{
		LOG_MSG(("Exception: %s", err.msg()));
	}
	catch(...)
	{
		std::cout << "some error occurred." << std::endl;
	}
}
//メッセージ受信時に呼び出される関数
void RobotController::onRecvMsg(RecvMsgEvent &evt)
{
	static int iImage = 0;

	//取得したメッセージを表示します
	std::string msg = evt.getMsg();
	LOG_MSG(("msg : %s", msg.c_str()));
}
Пример #11
0
void UserController::onRecvMsg(RecvMsgEvent &evt)
{
	std::string sender = evt.getSender();
	std::string msg    = evt.getMsg();
	//自分自身の取得
	SimObj *my = getObj(myname());

	if(msg == "Task_start"   && sender == "moderator_0"  && cycle < NUMBER_OF_REPETITION && Mission_complete == false) {

		parseFile("command.txt");

		message = ":"+ rooms[cycle] +";"+ objects[cycle]+".";
		brodcast_msg = "Go to the "+ rooms[cycle] +", grasp the "+ objects[cycle]+" and come back here";
		choice_movement(rooms[cycle]);
		//broadcastMsgToSrv(brodcast_msg);
		//sendMsg("moderator_0","init_time");
		//broadcastMsg(message);
		LOG_MSG((brodcast_msg.c_str()));
		//LOG_MSG(("%s: %s",sender.c_str(), brodcast_msg.c_str()));
		sendMsg("robot_000",brodcast_msg);
		sendMsg("moderator_0",brodcast_msg);
		sendMsg("SIGViewer",brodcast_msg);
		// m_state  = 0;
		//broadcastMsg(brodcast_msg);
       cycle = cycle+1;
	}
		if(msg == "Task_finished" && sender == "robot_000" && Mission_complete == false )
		{

	  choice_movement("finish");
		}

	if(msg == "Task_end" && sender == "moderator_0" && Mission_complete == false ) {
		
		sendMsg("moderator_0","init_time");
        choice_movement("finish");
	}
	
	if(msg == "Mission_complete" && sender == "moderator_0" && Mission_complete == false ) {
		
     Mission_complete = true;
     choice_movement("finish");
	}	


}
void MyController::onRecvMsg(RecvMsgEvent &evt) {
	std::string msg = evt.getMsg();
	if (msg == "Task_start"){
		sentMsg = false;
		flag1 = false;
		flag2 = false;
	}
}
Пример #13
0
void WheelControllerByMyo::onRecvMsg(RecvMsgEvent &evt)
{
	const std::string allMsg = evt.getMsg();

//	std::cout << allMsg << std::endl;

	std::map<std::string, std::vector<std::string> > sensorDataMap = MyoSensorData::decodeSensorData(allMsg);

	if (sensorDataMap.find(MSG_KEY_DEV_TYPE) == sensorDataMap.end()){ return; }

	if(sensorDataMap[MSG_KEY_DEV_TYPE][0]     !=this->myoDeviceManager.deviceType    ){ return; }
	if(sensorDataMap[MSG_KEY_DEV_UNIQUE_ID][0]!=this->myoDeviceManager.deviceUniqueID){ return; }

	MyoSensorData sensorData;
	sensorData.setSensorData(sensorDataMap);

//	std::cout << "roll=" << sensorData.roll << ",pitch=" << sensorData.pitch << ",yaw=" << sensorData.yaw << ",pose=" << sensorData.poseStr << std::endl;
//	std::cout << "emg1=" << sensorData.emgData[0] << ",emg2=" << sensorData.emgData[1] << ", emg3=" << sensorData.emgData[2] << std::endl;

	SimObj *obj = getObj(myname());

	float vel = 0.0;

	for(int i=0; i<MyoSensorData::EMG_SENSOR_NUM; i++)
	{
		vel += std::abs(sensorData.emgData[i]);
	}

	double rvel=0.0, lvel=0.0;

	// Turn left.
	if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::WaveIn))
	{
		rvel = 3.0*vel;
		lvel = 0.05*vel;
	}
	// Turn right.
	if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::WaveOut))
	{
		rvel = 0.05*vel;
		lvel = 3.0*vel;
	}
	// Brake.
	if(sensorData.poseStr==MyoSensorData::mapPoseType2Str.at(MyoSensorData::Fist))
	{
		rvel = -3.0*vel;
		lvel = -3.0*vel;
	}

//	std::cout << "rvel=" << rvel << ",lvel=" << lvel << std::endl;

	obj->setJointVelocity("JOINT_RWHEEL", rvel, 500.0);
	obj->setJointVelocity("JOINT_LWHEEL", lvel, 500.0);
}
void MyController::onRecvMsg(RecvMsgEvent &evt)
{
	string msg = evt.getMsg();
	if(msg == "check1"){
		check1 = true;
	}else if(msg == "elevator"){
		elevator = true;
	}else if(msg == "check3"){
		check3 = true;
	}
}
Пример #15
0
void MyController::onRecvMsg(RecvMsgEvent &evt)
{
	string msg = evt.getMsg();
	if(msg == "walk"){
		start = true;
	}
	if(msg == "Task_start"){
		start = false;
		my->setPosition(initPos);
		count = 0;
	}
}
Пример #16
0
void MyController::onRecvMsg(RecvMsgEvent &evt) {
	string msg = evt.getMsg();
	if (msg == "checkpoint1"){
		if(!check1_clear){
			check1 = true;
			LOG_MSG(("checkpoint1"));
		}
	}
	else if (msg == "checkpoint1_clear"){
		if(check1){
			check1_clear = true;
			LOG_MSG(("checkpoint1_clear"));
		}
	}
	else if (msg == "elevator_clear"){
		if(entered){
			elevator_clear = true;
		}
	}
	else if (msg == "Entered"){
		if(!elevator_clear){
			//elevator = true;
			entered = true;
		}
	}
	else if (msg == "crowd_clear"){
		crowd_clear = true;
		if(check1_clear && elevator_clear && crowd_clear){
			all_clear = true;
		}
	}
	/*else if (msg == "finish_line"){
		if(check1_clear && elevator_clear && crowd_clear){
			finish_line = true;
		}
	}*/
	else if(msg == "Task_start"){
		check1 = false;
		check1_clear = false;
		//elevator = false;
		elevator_clear = false;
		crowd_clear = false;
		collision = false;
		all_clear = false;
		a = false;
		b = false;
		c = false;
		d = false;
		e = false;
	}
}
void DemoRobotController::onRecvMsg(RecvMsgEvent &evt)
{
	std::string sender = evt.getSender();
	std::string msg = evt.getMsg();
	LOG_MSG(("%s: %s",sender.c_str(), msg.c_str()));

	if(sender == "moderator_0"){
		if(msg == "Task_start"){
			m_task++;
			m_time = 0.0;
			m_state = 50;
			if(m_task == 1)	m_graspObjectName = m_trashName1;
			else	m_graspObjectName = m_trashName2;
		}
		if(msg == "Time_over"){
			m_time = 0.0;
			m_state = 0;
		}
		if(msg == "Task_end"){
			m_time = 0.0;
			m_state = 0;
		}
	}
}
Пример #18
0
void AvatarControllerByOculusCV1::onRecvMsg(RecvMsgEvent &evt)
{
	const std::string allMsg = evt.getMsg();

	//std::cout << allMsg << std::endl;

	std::map<std::string, std::vector<std::string> > sensorDataMap = OculusRiftCV1SensorData::decodeSensorData(allMsg);

	if (sensorDataMap.find(MSG_KEY_DEV_TYPE) == sensorDataMap.end()){ return; }

	if(sensorDataMap[MSG_KEY_DEV_TYPE][0]     !=this->oculusCV1DeviceManager.deviceType    ){ return; }
	if(sensorDataMap[MSG_KEY_DEV_UNIQUE_ID][0]!=this->oculusCV1DeviceManager.deviceUniqueID){ return; }

	OculusRiftCV1SensorData sensorData;
	sensorData.setSensorData(sensorDataMap);

	ManNiiPosture posture = OculusCV1DeviceManager::convertQuaternion2ManNiiPosture(sensorData.getQuaternion());

	SimObj *obj = getObj(myname());
	OculusCV1DeviceManager::setJointQuaternions2ManNii(obj, posture);
}
///@brief Receive Message.
void AvatarControllerByPerceptionNeuron::onRecvMsg(RecvMsgEvent &evt)
{
	try
	{
		std::string allMsg = evt.getMsg();

//		std::cout << "msg:" << allMsg << std::endl;

		// Decode message to sensor data of Perception Neuron.
		std::map<std::string, std::vector<std::string> > sensorDataMap = PerceptionNeuronSensorData::decodeSensorData(allMsg);

		if (sensorDataMap.find(MSG_KEY_DEV_TYPE) == sensorDataMap.end()){ return; }
		if(sensorDataMap[MSG_KEY_DEV_TYPE][0]     !=this->perceptionNeuronDeviceManager.deviceType    ){ return; }
		if(sensorDataMap[MSG_KEY_DEV_UNIQUE_ID][0]!=this->perceptionNeuronDeviceManager.deviceUniqueID){ return; }

		// Make sensor data.
		PerceptionNeuronSensorData sensorData;

		sensorData.setSensorData(sensorDataMap);

		ManBvhPosture posture = this->perceptionNeuronDeviceManager.convertSensorData2ManBvhPosture(sensorData);

		// Set the posture to avatar.
		SimObj *obj = getObj(myname());
		this->perceptionNeuronDeviceManager.setPosture2ManBvhYXZ(obj, posture);
	}
	catch(SimObj::Exception &err)
	{
		LOG_MSG(("Exception: %s", err.msg()));
	}
	catch(std::string &err)
	{
		LOG_MSG(("Exception: %s", err.c_str()));
	}
	catch(...)
	{
		std::cout << "some error occurred." << std::endl;
	}
}
Пример #20
0
void CameraController::onRecvMsg(RecvMsgEvent &evt) {
  std::string msg = evt.getMsg();
  std::vector<MoveData> moves = buildMoveData(msg);
  if (!moves.empty()) { // seg fault if task is not started
    MoveData move = moves[0]; // only one move needed for this operation
    if (move.releasedButtons & Btn_MOVE) {
      if (! configuringPath) {
	rotateTowardGround();
	std::cout << "starting path definition" << std::endl;
	configuringPath = true;
      }
      else {
	// save path --> write path to file
	if (!path.empty()) {
	  std::ofstream output;
	  std::stringstream filename;

	  time_t now = time(0);
	  struct tm  tstruct;
	  char buf[80];
	  tstruct = *localtime(&now);

	  strftime(buf, sizeof(buf), "%Y-%m-%d.%X", &tstruct);

	  filename << "nodes/"
		   << buf
		   << ".txt";

	  output.open(filename.str().c_str());
	  std::vector<Vector3d>::iterator node;
	  for (node = path.begin(); node != path.end(); ++node) {
	    std::cout << "Node : " << node->x() << ", " << node->y() << ", " << node->z() << std::endl;
	    output << node->x() << "," << node->y() << "," << node->z() << "," << "0" << std::endl;
	  }
	  output.close();
	  std::stringstream msg;
	  msg << "new path wrote to " << filename.str();
	  std::cout << msg << std::endl;
	  broadcastMsg(msg.str());
	}
	path.clear();
	configuringPath = false;
      }
    }
    else if (configuringPath) {
      // update the position of the cursor.
      SimObj* cursor = getObj("pointer1");

      double xPosition = -300*move.tracker.y;
      double yPosition = 100;
      double zPosition = -300*move.tracker.x;

      cursor->setPosition(xPosition, yPosition, zPosition);


      if (move.releasedButtons & Btn_CROSS) {
	// add a step to the current path
	path.push_back(Vector3d(xPosition, yPosition, zPosition));
      }
      else if (move.releasedButtons & Btn_CIRCLE) {
	// remove the last step of the path
	if (! path.empty()) {
	  path.pop_back(); 
	}
      }

    }
  }

}
void MyController::onRecvMsg(RecvMsgEvent &evt)
{  
  // 送信者取得
  std::string sender = evt.getSender();
  std::cout << "sender: " << sender << std::endl;

	char *all_msg = (char*)evt.getMsg();		
	printf("all_msg: %s \n", all_msg);
	char *delim = (char *)(" ");
	char *ctx;
	char *header = strtok_r(all_msg, delim, &ctx);

	if (strcmp(header, "RESET") == 0) {
		printf("Received RESET \n");
		setRobotPosition(0, -50);	
		setRobotHeadingAngle(0);
		setCameraPosition(0, 3);
		printf("Reseted RobotPosition \n");
		//char* replyMsg = sendSceneInfo();
		sendSceneInfo("Start");
		m_executed = true;
		return;
	}



  // 送信者がゴミ認識サービスの場合
  if(sender == "RecogTrash") {

	  /*char *all_msg = (char*)evt.getMsg();		
		//printf("all_msg: %s \n", all_msg);
		char *delim = (char *)(" ");
		char *ctx;
		char *header = strtok_r(all_msg, delim, &ctx);*/

		double x = 0, y = 0, z = 0; // range = 0;
	
			
		if(strcmp(header, "ObjDir") == 0) {
			// 次に移動する座標を位置を取り出す			
			x = atof(strtok_r(NULL, delim, &ctx));
			y = atof(strtok_r(NULL, delim, &ctx));
			z = atof(strtok_r(NULL, delim, &ctx));
			m_range = atof(strtok_r(NULL, delim, &ctx));
			nextPos.set(x, y, z);
			//printf("m_range = %lf \n", m_range);
			printf("[ClientMess] ObjDir %lf %lf %lf range: %lf\n", nextPos.x(), nextPos.y(), nextPos.z(), m_range);		
			// ロボットのステートを更新

			m_state = 20;
			m_executed = false;
			return;
		}

		// 物体のある場所に到着し、アームを伸ばし、物体を掴む
		if(strcmp(header, "grab") == 0) {		
			printf("grab \n");	
			// 回転を止める
			m_my->setWheelVelocity(0.0, 0.0);
			m_state = 30;
			m_executed = false;
			return;
		}

	 if(strcmp(header, "TrashBoxDir") == 0) {
			printf("TrashBoxDir \n");
			// 次に移動する座標を位置を取り出す			
			x = atof(strtok_r(NULL, delim, &ctx));
			y = atof(strtok_r(NULL, delim, &ctx));
			z = atof(strtok_r(NULL, delim, &ctx));
			m_range = atof(strtok_r(NULL, delim, &ctx));
			//printf("range = %lf \n", m_range);
			nextPos.set(x, y, z);
			printf("[ClientMess] TrashBoxDir %lf %lf %lf range: %lf\n", nextPos.x(), nextPos.y(), nextPos.z(), m_range);
		  m_state = 40;
			m_executed = false;
			return;
	 }

	 if(strcmp(header, "ThrowTrash") == 0) {			
			m_state = 50;
			m_executed = false;
			return;
		}

		if(strcmp(header, "Finish") == 0) {	
			m_state = 100;
			m_executed = false;
			return;	
		}

		if(strcmp(header, "RandomRouteStart") == 0) {
			m_state = 800;
			m_executed = false;
			return;
		}

		if(strcmp(header, "RandomRouteArrived") == 0) {
			//printf("onRecv RandomRouteArrived \n");
			m_state = 800;
			m_executed = false;
			return;
		}

		if(strcmp(header, "RandomRoute") == 0) {
			x = atof(strtok_r(NULL, delim, &ctx));			
			z = atof(strtok_r(NULL, delim, &ctx));	
			m_range = atof(strtok_r(NULL, delim, &ctx));			
			nextPos.set(x, y, z);

			double lookingX = atof(strtok_r(NULL, delim, &ctx));
			double lookingZ = atof(strtok_r(NULL, delim, &ctx));
			m_lookingPos.set(lookingX, 0, lookingZ);

			m_state = 805;
			m_executed = false;
			return;
		} 

		if(strcmp(header, "GoForwardVelocity") == 0) {
			double wheelVel = atof(strtok_r(NULL, delim, &ctx));
			printf("GoForwardVelocity coef: %lf \n", wheelVel);
			wheelVel *= m_vel;
			m_my->setWheelVelocity(wheelVel * 10., wheelVel * 10.);				
			//sendSceneInfo();				
			return;
		}

		if(strcmp(header, "RotateVelocity") == 0) {
			double wheelVel = atof(strtok_r(NULL, delim, &ctx));
			printf("RotateVelocity coef: %lf \n", wheelVel);
			wheelVel *= m_vel;
			m_my->setWheelVelocity(-wheelVel, wheelVel);				
			//sendSceneInfo();				
			return;
		}

		if(strcmp(header, "Stop") == 0) {
			printf("Stop joyStick \n");
			m_my->setWheelVelocity(0.0, 0.0);				
			sendSceneInfo();			
			return;
		}

		if (strcmp(header, "CamID") == 0) {
			printf("Setting CameraID \n");
			m_CamID = atoi(strtok_r(NULL, delim, &ctx));
			return;
		}

		if (strcmp(header, "CaptureData") == 0) {
			printf("SetRobotPosition \n");
			double x = atof(strtok_r(NULL, delim, &ctx));		
			double z = atof(strtok_r(NULL, delim, &ctx));
			setRobotPosition(x, z);
			double angle = atof(strtok_r(NULL, delim, &ctx));
			printf("setRobotHeadingAngle: %lf \n", angle);
			setRobotHeadingAngle(angle);
			char* replyMsg = sendSceneInfo();
			//m_srv->sendMsgToSrv(replyMsg);
			m_executed = true;
			return;			
		} 

	} else if (sender == "SIGViewer") {
		printf("mess from SIGViewer \n");

		if(strcmp(header, "CameraAngle") == 0) {
			double angle = atof(strtok_r(NULL, delim, &ctx));
			printf("CameraAngle: %lf \n", angle);
			setCameraPosition(angle, 3);
			char* replyMsg = sendSceneInfo();
			m_executed = true;
			return;
		} else if (strcmp(header, "RobotAngle") == 0) {
			printf("rorate robor start \n");
			double angle = atof(strtok_r(NULL, delim, &ctx));
			printf("RobotHeadingAngle: %lf \n", angle);
			setRobotHeadingAngle(angle);
			char* replyMsg = sendSceneInfo();
			//m_srv->sendMsgToSrv(replyMsg);
			m_executed = true;
			return;			
		} else if (strcmp(header, "RotateDir") == 0) {
			// 次に移動する座標を位置を取り出す			
			double x = atof(strtok_r(NULL, delim, &ctx));
			double z = atof(strtok_r(NULL, delim, &ctx));
			nextPos.set(x, 0, z);
	
			printf("RotateDir %lf %lf \n", nextPos.x(), nextPos.z());		
			// ロボットのステートを更新
			m_state = 920;
			m_executed = false;
			return;
		} else if(strcmp(header, "RobotPosition") == 0) {
			double x = atof(strtok_r(NULL, delim, &ctx));		
			double z = atof(strtok_r(NULL, delim, &ctx));
			setRobotPosition(x, z);
			return;
		}
	}

}  
Пример #22
0
void MyController::onRecvMsg(RecvMsgEvent &evt)
{
	std::string sender = evt.getSender();

	char *all_msg = (char*)evt.getMsg();

	std::string ss = all_msg;
	int strPos1 = 0;
	int strPos2,strPos3;
	std::string headss,cmd;
	std::string tmpss;
	std::cout<<"the massage is :"<< ss <<std::endl;
	std::cout<<"the sender is :"<< sender <<std::endl;
	strPos2 = ss.find(" ", strPos1);
	headss.assign(ss, strPos1, strPos2-strPos1);
	int jj = ss.size();
	cmd.assign(ss, strPos2+1,jj);
	strPos3 = cmd.find("|", strPos1);
	id.assign(cmd, strPos1, strPos3-strPos1+1);
	//std::cout<<"The ID is :  "<< id <<std::endl;
	bool aff = true;

	if (headss == "MultiUsersMenu")
		{
			bool available = checkService(ss);
			if (!available && m_ref != NULL) m_ref = NULL;
			else if (available && m_ref == NULL){
				m_ref = connectToService(ss);
			}
			int nn = 0;
			for (int j = 0; j< list.size();j++) {
				if (id != list[j].ID) {
					nn++;
				}
				else {
					sendMsg(list[j].name,list[j].ID);
					m_state = 0;
					ind = j;
					current_avatar = list[j].name;
				}
			}
			if (nn == list.size()) {
				for (int i = 0; i< list.size();i++) {
					if (list[i].ID == "" && aff == true ) {
						list[i].ID = id;
						sendMsg(list[i].name,list[i].ID);
						m_state = 0;
						ind = i;
						current_avatar = list[i].name;
						aff = false;
					}
				}
			}
		}


	if (ss == "Info") {
		std::cout<<"Sending info to:"<< sender <<std::endl;

		std::string mess = "";
		mess +=  "AvatarInformation/" ;
		mess += avatars_number;
		mess +=  "/" ;
		for (int j = 0; j< list.size();j++) {
			mess += list[j].name ;
			mess +=  "/" ;
		}
		std::cout<<"The message is :"<< mess <<std::endl;
		sendMsg(sender,mess);
	}
}