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 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");
}
}
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 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;
}
}
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 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;
}
}
}
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;
}
}
}
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);
}
}
