void reportStatus() { char json[JSON_MAX_SIZE]; if (nextEntryReport == 0) { // start reporting nextEntryReport = SENSOR_TEMP; } switch (nextEntryReport) { case SENSOR_TEMP: jsonifySensorValue(SENSOR_TEMP, tempRoom, json, JSON_MAX_SIZE); broadcastMsg(json); nextEntryReport = SENSOR_HUM; break; case SENSOR_HUM: jsonifySensorValue(SENSOR_HUM, humRoom, json, JSON_MAX_SIZE); broadcastMsg(json); nextEntryReport = 0; break; default: break; } }
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::onCollision(CollisionEvent &evt) { std::string msg = "Collision"; broadcastMsg(msg); }
double MyController::onAction(ActionEvent &evt) { if(first == false){ std::string msg = "start"; broadcastMsg(msg); first = true; } int count=0; double r=0.0; //2点間の直線距離 double angle; if(end==false){ if(start==true){ Vector3d pos; Vector3d npos; if(first==false){ FILE* fp; x=0; y=0; z=0; w=0; //チェックポイント dx=0; dy=0; dz=0; if((fp = fopen("node.txt", "r")) == NULL) { printf("File do not exist.\n"); exit(0); } while(fscanf(fp, "%lf,%lf,%lf,%lf", &x, &y, &z,&w) != EOF) { temp.x[i]=x; temp.y[i]=y; temp.z[i]=z; temp.w[i]=w; i++; } fclose(fp); first = true; i=0; } if(stop==false){ my->getPosition(pos); npos.x(temp.x[i]); npos.y(temp.y[i]); npos.z(temp.z[i]); angle = rotateTowardObj(npos); if(angle < 0.0){ angle = -1.0 * angle; } /*ここに相当する部分を書く*/ //my->setAxisAndAngle(0, 1.0, 0, -angle); // 回転すべき円周距離 double distance = m_distance*PI*fabs(targetAngle)/(2*PI); if(targetAngle > 0.0){ m_my->setWheelVelocity(velocity, -velocity); } else{ m_my->setWheelVelocity(-velocity, velocity); } // 車輪の半径から移動速度を得る double vel = m_radius*velocity; // 回転時間(u秒) double time = (distance / vel) + evt.time(); if(evt.time>=time){ m_my->setWheelVelocity(0, 0); } dx=(temp.x[i]-pos.x()); dy=(temp.y[i]-pos.y()); dz=(temp.z[i]-pos.z()); r=sqrt(pow(dx,2)+pow(dz,2)); //ここまでが現在地から次の座標までの距離と角度の計算 double vel = m_radius*velocity; // 移動開始 m_my->setWheelVelocity(velocity, velocity); // 到着時間取得 double time2 = r / vel; count = 0; count2= 0; i++; } if(temp.w[i-1] == 1.0){ std::string msg = "point1"; //"robot_000"にメッセージを送信します sendMsg("man_001", msg); }else if(temp.w[i-1] == 2.0){ stop=true; broadcastMsgToSrv("elevator"); if(elevator==true){ stop=false; } } } } /***************************************************************************/ /* if(strstr(myname() , "robot_004")!= NULL){ m_my->setWheelVelocity(m_vel*2, m_vel*2); } */ return 0.1; }
void SendBroadcastForm::onSendBtn(){ emit broadcastMsg(ui->textEdit->toPlainText()); ui->textEdit->clear(); }
void QRoseComponent::broadcastCache(int groupId) { if (m_cache.empty()) return; SgNodesMsg msg(&m_cache); broadcastMsg(&msg, groupId); }
void QRoseComponent::broadcastNode(SgNode *node, int groupId) { SgNodeMsg msg(node); broadcastMsg(&msg, groupId); }
void OnMessage(const khaki::TcpClientPtr& con) { khaki::Buffer tmp(con->getBuf()); broadcastMsg(tmp); }
double RobotController::onAction(ActionEvent &evt) { //std::cout << "m_state " << m_state << std::endl; //std::cout << "the size of Vector " << Record_Postures.size() << std::endl; switch(m_state) { case 10: { Robot_speed = Change_Robot_speed; choose_task_arm(5, LEFT_ARM); choose_task_arm(5, RIGHT_ARM); // printf("got it in case!1 flag1 \n"); if (goTo(m_relayPoint1, 0) == true && moveArm(LEFT_ARM) == true && moveArm(RIGHT_ARM) == true) { m_state = 20; // printf("got it in case!1 \n"); } break; } case 20: { Robot_speed = Change_Robot_speed; if (goTo(m_relayPoint2, 0) == true) m_state = 30; break; } case 30: { Robot_speed = Change_Robot_speed; if (goTo(m_relayFrontTable, 0) == true) m_state = 40; break; } case 40: // Test if the cycle is finished or not { if (cycle > 0) { m_state = 41; broadcastMsg("Show_me"); get_Kinect_Data(); m_time = evt.time() + 5; break; } else { m_state = 49; break; } } case 41: { if(evt.time() > m_time) m_state = 42; break; } case 42: { Kinect_Data_Off(); // finished analysing data m_pointedObject = "petbottle"; m_pointedtrash = "recycle"; std::cout << "Task started Robot ........ " << std::endl; //PrintPosture(); m_state = 50; } case 49: { break; } case 50: //Optional case !!! { Robot_speed = Change_Robot_speed; if (m_pointedObject=="petbottle") { if (goTo(m_BottleFront, 0) == true) m_state = 60; } else if (m_pointedObject=="mugcup") { if (goTo(m_MuccupFront, 0) == true) m_state = 60; } else if (m_pointedObject=="can") { if (goTo(m_CanFront, 0) == true) m_state = 60; } break; } case 60: //preparation of the arm for grasp { Robot_speed = Change_Robot_speed; recognizeObjectPosition(m_Object_togo, m_pointedObject); if (goTo(m_Object_togo, 70) == true) { m_state = 70; } break; } case 70: //preparation of the arm for grasp { choose_task_arm(1, LEFT_ARM); if (moveArm(LEFT_ARM) == true) m_state = 80; break; } case 80: //move to the object { Robot_speed = 1; if (goTo(m_Object_togo, 38) == true) m_state = 90; break; } case 90: //move arm to grasp the object { choose_task_arm(2, LEFT_ARM); grasp_left_hand(); if (moveArm(LEFT_ARM) == true) m_state = 100; break; } case 100: { m_state = 110; break; } case 110: //move arm to place in good position for moving { grasp_left_hand(); choose_task_arm(3, LEFT_ARM); if (moveArm(LEFT_ARM) == true) m_state = 120; break; } case 120: { my->setWheelVelocity(-1.4,-1.4); choose_task_arm(5, LEFT_ARM); if (moveArm(LEFT_ARM) == true) { Robot_speed = Change_Robot_speed; m_state = 130; sleep(1); } break; } case 130: //move to the object { // Robot_speed = 1; if (goTo(m_relayFrontTrash, 0) == true) { m_state = 140; } break; } case 140: { if (goTo(m_relayFrontTable_reset, 0) == true) { sleep(2); m_state = 150; } break; } case 990: //move to the object { // Robot_speed = 1; break; } case 150: //Optional case !!! { Robot_speed = Change_Robot_speed; if (m_pointedtrash=="recycle") { if (goTo(m_RecycleFront, 0) == true) m_state = 160; } else if (m_pointedtrash=="burnable") { if (goTo(m_BurnableFront, 0) == true) m_state = 160; } else if (m_pointedtrash=="unburnable") { if (goTo(m_UnburnableFront, 0) == true) m_state = 160; } break; } case 160: //preparation of the arm for grasp { recognizeObjectPosition(m_Trash_togo, m_pointedtrash); if (goTo(m_Trash_togo, 50) == true) { m_state = 170; } break; } case 170: //preparation of the arm for grasp { choose_task_arm(1, LEFT_ARM); if (moveArm(LEFT_ARM) == true) m_state = 180; break; } case 180: { Robot_speed = 1; if (goTo(m_Trash_togo, 60) == true) m_state = 190; break; } case 190: //move arm to grasp the object { choose_task_arm(3, LEFT_ARM); if (moveArm(LEFT_ARM) == true) { m_state = 200; release_left_hand(); } break; } case 200: { my->setWheelVelocity(-2.5,-2.5); choose_task_arm(2, LEFT_ARM); if (moveArm(LEFT_ARM) == true) { Robot_speed = Change_Robot_speed; m_state = 210; } break; } case 210: //move to the object { Robot_speed = Change_Robot_speed; if (goTo(m_relayFrontTrash, 0) == true) m_state = 220; break; } case 220: //preparation of the arm for grasp { choose_task_arm(5, LEFT_ARM); if (moveArm(LEFT_ARM) == true) m_state = 230; break; } case 230: //move to the object { Robot_speed = Change_Robot_speed; if (goTo(m_relayFrontTable, 0) == true) { cycle = cycle-1; m_state = 40; broadcastMsg("Task_finished"); m_pointedtrash = ""; m_pointedObject = ""; m_state = 0; } break; } } return m_onActionReturn; }
double DemoRobotController::onAction(ActionEvent &evt) { switch(m_state) { case 0: { break; } case 1: { this->stopRobotMove(); break; } case 50: { // detour: rotate toward relay point 1 if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_relayPoint1); m_time = l_moveTime+evt.time(); m_state = 60; } break; } case 60: { // detour: go toward relay point 1 if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = goToObj(m_relayPoint1, 0.0); m_time = l_moveTime+evt.time(); m_state = 70; } break; } case 70: { // rotate toward the position in front of trash if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = rotateTowardObj(m_frontDesk1); m_time = l_moveTime+evt.time(); m_state = 80; } break; } case 80: { // go toward the position in front of trash if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime = goToObj(m_frontDesk1, 0.0); m_time = l_moveTime+evt.time(); m_state = 90; } break; } case 90: { // rotate toward the trash if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 100; } break; } case 100: { // prepare the robot arm to grasping the trash if(evt.time() >= m_time) { this->stopRobotMove(); this->neutralizeArms(evt.time()); m_state = 105; } break; } case 105: { // fix robot direction for grasping if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime+evt.time(); m_state = 110; } break; } case 110: { // approach to the trash if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goToObj(l_tpos, 30.0); m_time = l_moveTime+evt.time(); m_state = 120; } break; } case 120: { // try to grasp trash if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashName1); else this->recognizeObjectPosition(l_tpos, m_trashName2); double l_moveTime = goGraspingObject(l_tpos); m_time = l_moveTime+evt.time(); m_state = 125; } break; } case 125: { if(evt.time() >= m_time) { m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); this->neutralizeArms(evt.time()); m_state = 130; } break; } case 130: { if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 20./m_movingSpeed + evt.time(); m_state = 150; } break; } case 150: { if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime; if(m_task == 1) l_moveTime = rotateTowardObj(m_frontTrashBox1); else l_moveTime = rotateTowardObj(m_frontTrashBox2); m_time = l_moveTime + evt.time(); m_state = 160; } break; } case 160: { if(evt.time() >= m_time) { this->stopRobotMove(); double l_moveTime; if(m_task == 1) l_moveTime = goToObj(m_frontTrashBox1,0.0); else l_moveTime = goToObj(m_frontTrashBox2,0.0); m_time = l_moveTime + evt.time(); m_state = 161; } break; } case 161: { if(evt.time() >= m_time) { this->stopRobotMove(); this->prepareThrowing(evt.time()); m_state = 165; } break; } case 165: { if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashBoxName1); else this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 170; } break; } case 170: { if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashBoxName1); else this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = goToObj(l_tpos, 50.0); m_time = l_moveTime + evt.time(); m_state = 180; } break; } case 180: { if(evt.time() >= m_time) { this->stopRobotMove(); Vector3d l_tpos; if(m_task == 1) this->recognizeObjectPosition(l_tpos, m_trashBoxName1); else this->recognizeObjectPosition(l_tpos, m_trashBoxName2); double l_moveTime = rotateTowardObj(l_tpos); m_time = l_moveTime + evt.time(); m_state = 200; } break; } case 200: { // throw trash and get back a bit if(evt.time() >= m_time) { this->stopRobotMove(); this->throwTrash(); sleep(1); m_robotObject->setWheelVelocity(-m_angularVelocity, -m_angularVelocity); m_time = 50.0/m_movingSpeed + evt.time(); m_state = 225; } break; } case 225: { // recover robot arms if(evt.time() >= m_time) { this->stopRobotMove(); this->neutralizeArms(evt.time()); m_state = 240; } break; } case 240: { // go next if(evt.time() >= m_time1) m_robotObject->setJointVelocity("RARM_JOINT1", 0.0, 0.0); if(evt.time() >= m_time4) m_robotObject->setJointVelocity("RARM_JOINT4", 0.0, 0.0); if(evt.time() >= m_time1 && evt.time() >= m_time4) { this->stopRobotMove(); broadcastMsg("Task_finished"); //broadcastMsg("Give_up"); m_state = 0; } break; } } return refreshRateOnAction; }