[TeamTalk 139]: [676] TeamTalk/Agents/boeingLib: 1) updates to boeingLib.
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Tue Jul 31 16:41:54 EDT 2007
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20070731/e8cadd2d/attachment.html
-------------- next part --------------
Modified: TeamTalk/Agents/boeingLib/boeing/boeing_converter.cc
===================================================================
--- TeamTalk/Agents/boeingLib/boeing/boeing_converter.cc 2007-07-30 20:55:57 UTC (rev 675)
+++ TeamTalk/Agents/boeingLib/boeing/boeing_converter.cc 2007-07-31 20:41:52 UTC (rev 676)
@@ -170,6 +170,24 @@
if (debug)
printf(" Got request image message\n");
result.hdr.type = REQ_IMAGE;
+ } else if (name == "eval") {
+ if (debug)
+ printf(" Eval message\n");
+ if (!params) {
+ fprintf(stderr, " Malformed eval message '%s'\n",mca->action);
+ return -1;
+ }
+
+ result.hdr.type = CMD_EVAL;
+ result.hdr.len = sizeof(MsgCmdEval);
+
+ memset(result.msg_eval.eval,0,MAX_STRING_LENGTH);
+
+ char temp[MAX_STRING_LENGTH];
+ rv=sscanf(params,"%s",temp);
+ strncpy(result.msg_eval.eval,temp,MAX_STRING_LENGTH-1);
+ fprintf(stderr," Eval string: '%s' ", result.msg_eval.eval);
+
} else{
if (debug)
printf(" Got some message, assume action message: '%s'\n",
Modified: TeamTalk/Agents/boeingLib/boeing/boeing_robot_client.cc
===================================================================
--- TeamTalk/Agents/boeingLib/boeing/boeing_robot_client.cc 2007-07-30 20:55:57 UTC (rev 675)
+++ TeamTalk/Agents/boeingLib/boeing/boeing_robot_client.cc 2007-07-31 20:41:52 UTC (rev 676)
@@ -129,6 +129,22 @@
}
+bool RobotClient::sendEval(Priority p,TaskID tid,char const *eval)
+{
+ if (eval==NULL)
+ return false;
+
+ MsgCmdEval mce;
+ mzero(mce);
+
+ mce.type = CMD_EVAL;
+ mce.taskid = tid;
+ mce.priority=p;
+ strncpy(mce.eval,eval,MAX_STRING_LENGTH-1);
+ mce.len = mce.getSize();
+ return sendPacket(&mce);
+}
+
// halt all robots
bool RobotClient::sendHalt(Priority p,TaskID tid,bool ignore_play_stop)
{
Modified: TeamTalk/Agents/boeingLib/boeing/boeing_robot_client.h
===================================================================
--- TeamTalk/Agents/boeingLib/boeing/boeing_robot_client.h 2007-07-30 20:55:57 UTC (rev 675)
+++ TeamTalk/Agents/boeingLib/boeing/boeing_robot_client.h 2007-07-31 20:41:52 UTC (rev 676)
@@ -48,6 +48,8 @@
bool sendAction(Priority p,TaskID tid,char const *action);
+ bool sendEval(Priority p,TaskID tid,char const *eval);
+
/// halt all robots
bool sendHalt(Priority p,TaskID tid,bool ignore_play_stop=false);
Modified: TeamTalk/Agents/boeingLib/boeing/boeing_robot_packet.h
===================================================================
--- TeamTalk/Agents/boeingLib/boeing/boeing_robot_packet.h 2007-07-30 20:55:57 UTC (rev 675)
+++ TeamTalk/Agents/boeingLib/boeing/boeing_robot_packet.h 2007-07-31 20:41:52 UTC (rev 676)
@@ -34,14 +34,16 @@
CMD_ACTION=0,
CMD_HALT,CMD_GOTO,CMD_HOME,CMD_FOLLOW,
CMD_COVER,CMD_SETPOS,CMD_PAUSE,CMD_RESUME,
- REQ_LOCATION,REQ_IMAGE
+ REQ_LOCATION,REQ_IMAGE,
+ CMD_EVAL
};
/// these are the IDs for messages being sent from the robot
enum RobotMsgID {
ROB_ACK=0x0100,ROB_DONE,ROB_LOCATION,
ROB_ACTION_ACK,ROB_ACTION_ECHO,
- ROB_IMAGE,ROB_PLAY_HALT
+ ROB_IMAGE,ROB_PLAY_HALT,
+ ROB_EVAL
};
static const uint16_t CmdMsgIdMask = 0x00FF;
@@ -164,10 +166,21 @@
}
} PACKED;
+ struct MsgCmdEval : public MsgCmdTask {
+ char eval[MAX_STRING_LENGTH];
+
+ int getSize() const {
+ return (int) strnlen(eval,MAX_STRING_LENGTH-1)+1+sizeof(MsgCmdTask);
+ }
+ } PACKED;
+
+
/** all the command messages. Note that action messages
are variable length so this union is inappropriate
to use as the receive buffer
*/
+ //eval messages also variable length
+
union MsgCmd {
MsgHeader hdr;
MsgCmdTask msg_cmdtask;
@@ -180,6 +193,7 @@
MsgCmdFollow msg_follow;
MsgCmdCover msg_cover;
MsgCmdAction msg_action;
+ MsgCmdEval msg_eval;
MsgMapReq msg_image;
MsgReqLocation req_location;
@@ -221,7 +235,16 @@
return m;
};
} PACKED;
+
+
+ /// evaluation message for play manager
+ //this should eventually be multiple data types, not just int16_t
+ struct MsgEvalResult : public MsgHeader {
+ TaskID taskid;
+ int16_t eresult;
+ } PACKED;
+
/** robot done status message
status - task status [Succeeded,Failed]
*/
@@ -271,6 +294,8 @@
MsgRobLocation location;
MsgActionEcho echo;
MsgActionAck action_ack;
+ MsgEvalResult eval_result;
+
MsgRobPlayHalt play_halt;
MsgMap image;
char buff[50000];
Modified: TeamTalk/Agents/boeingLib/boeing/boeing_robot_server.cc
===================================================================
--- TeamTalk/Agents/boeingLib/boeing/boeing_robot_server.cc 2007-07-30 20:55:57 UTC (rev 675)
+++ TeamTalk/Agents/boeingLib/boeing/boeing_robot_server.cc 2007-07-31 20:41:52 UTC (rev 676)
@@ -168,7 +168,9 @@
} break;
case CMD_HALT:
+ printf("Halt received\n");
if (!rxdata.msg_halt.ignore_play_stop) {
+ printf("Send play halt\n");
sendPlayHalt();
}
break;
@@ -242,6 +244,23 @@
return (doSend((MsgHeader *) &msg));
}
+ // sending operations
+ bool RobotServer::sendEvalResult(int taskid,int eval_result)
+ {
+ MsgEvalResult msg;
+
+ msg.taskid = taskid;
+ msg.eresult = eval_result;
+ msg.len = sizeof(msg);
+ msg.type = ROB_EVAL;
+
+ if (server_debug)
+ printf("sending robot eval result %d\n", eval_result);
+
+ return (doSend((MsgHeader *) &msg));
+ }
+
+
bool RobotServer::sendDone(int taskid,bool ok)
{
MsgRobDone msg;
Modified: TeamTalk/Agents/boeingLib/boeing/boeing_robot_server.h
===================================================================
--- TeamTalk/Agents/boeingLib/boeing/boeing_robot_server.h 2007-07-30 20:55:57 UTC (rev 675)
+++ TeamTalk/Agents/boeingLib/boeing/boeing_robot_server.h 2007-07-31 20:41:52 UTC (rev 676)
@@ -52,6 +52,9 @@
/// send an action acknowledgement and the current status
bool sendActionAck(int taskid,int status);
+ // send robot result of evaluation (robot world model information)
+ bool sendEvalResult(int taskid,int eval_result);
+
/** inform the playmanager that we need to halt the play
(Generally called internally in the server code */
bool sendPlayHalt();
Modified: TeamTalk/Agents/boeingLib/coralshared/error_check.cc
===================================================================
--- TeamTalk/Agents/boeingLib/coralshared/error_check.cc 2007-07-30 20:55:57 UTC (rev 675)
+++ TeamTalk/Agents/boeingLib/coralshared/error_check.cc 2007-07-31 20:41:52 UTC (rev 676)
@@ -57,3 +57,19 @@
AnsiColor::Reset(stderr);
}
+
+void _Notify(const char *fmt, ...)
+{
+ AnsiColor::SetFgColor(stderr,AnsiColor::Green);
+ AnsiColor::Bold(stderr);
+
+ va_list al;
+ va_start(al,fmt);
+ vfprintf(stderr, fmt, al);
+ va_end(al);
+
+ AnsiColor::Reset(stderr);
+}
+
+
+
Modified: TeamTalk/Agents/boeingLib/coralshared/error_check.h
===================================================================
--- TeamTalk/Agents/boeingLib/coralshared/error_check.h 2007-07-30 20:55:57 UTC (rev 675)
+++ TeamTalk/Agents/boeingLib/coralshared/error_check.h 2007-07-31 20:41:52 UTC (rev 676)
@@ -7,9 +7,12 @@
#define Fatal(str) _Fatal(__FILE__,__LINE__,"%s",str)
#define Warn(str) _Warn( __FILE__,__LINE__,"%s",str)
+#define Notify(str) _Notify( "%s",str)
+
#define FatalP(fmt, ...) _Fatal(__FILE__,__LINE__,fmt,__VA_ARGS__)
#define WarnP(fmt, ...) _Warn( __FILE__,__LINE__,fmt,__VA_ARGS__)
+#define NotifyP(fmt, ...) _Notify( fmt,__VA_ARGS__)
#ifdef NDEBUG
# define Assert(check,str)
@@ -35,6 +38,8 @@
void _Warn(const char *filename,int line,
const char *fmt, ...);
+void _Notify(const char *fmt, ...);
+
void _Assert(const char *filename,int line,const char *function,
const char *fmt, ...);
Modified: TeamTalk/Agents/boeingLib/utils/test_robot_server.cc
===================================================================
--- TeamTalk/Agents/boeingLib/utils/test_robot_server.cc 2007-07-30 20:55:57 UTC (rev 675)
+++ TeamTalk/Agents/boeingLib/utils/test_robot_server.cc 2007-07-31 20:41:52 UTC (rev 676)
@@ -73,34 +73,76 @@
while (run) {
while ((msg=bserver.getNextMessage())!=NULL) {
switch (msg->hdr.type) {
- case CMD_ACTION:
- printf("(RE->ROBOT) action message\n");
- printf(" Action '%s'\n", msg->msg_action.action);
-
- // sleep a random time
- sleep((rand()%10)+1);
-
- // send the response
- rv = ((rand()%10<1) ? FAILED : SUCCEEDED);
- printf(" Done with value %i\n",rv);
-
- bserver.sendActionAck(msg->msg_action.taskid,rv);
- break;
- case REQ_LOCATION:
- printf("Sending fake location: 1,2,0.1,false\n");
- bserver.sendLocation(1.0f,2.0f,0.1f,false);
- break;
- case REQ_IMAGE:
- printf("Sending fake image\n");
-
- printf("Opening JPEG writer with %i %i\n",(int)width,(int)height);
- jwriter.openMemGray(width,height);
- jwriter.writeData(image);
- bserver.sendJPEGImage((const unsigned char *)jwriter.getBuffer(),jwriter.getWrittenLength(),
- width,height,msg->msg_image.invoice);
- break;
- default:
- printf("(RE->ROBOT) Unknown mesasge %i\n", msg->hdr.type);
+ case CMD_ACTION:
+ printf("(RE->ROBOT) action message\n");
+ printf(" Action '%s'\n", msg->msg_action.action);
+
+ // sleep a random time
+ sleep((rand()%10)+1);
+
+ // send the response
+ rv = ((rand()%10<1) ? FAILED : SUCCEEDED);
+ printf(" Done with value %i, taskid %i\n",rv,msg->msg_action.taskid);
+ bserver.sendActionAck(msg->msg_action.taskid,rv);
+ break;
+ case CMD_EVAL:
+ printf("(RE->ROBOT) eval message\n");
+ printf(" Eval '%s'\n", msg->msg_eval.eval);
+
+ // sleep a random time
+ sleep((rand()%10)+1);
+
+ // send the response
+ rv = ((rand()%10<1) ? FAILED : SUCCEEDED);
+ printf(" Eval value %i\n",rv);
+
+ bserver.sendEvalResult(msg->msg_eval.taskid,rv);
+ break;
+ case REQ_LOCATION:
+ printf("Sending fake location: 1,2,0.1,false\n");
+ bserver.sendLocation(1.0f,2.0f,0.1f,false);
+ break;
+ case REQ_IMAGE:
+ printf("Sending fake image\n");
+
+ printf("Opening JPEG writer with %i %i\n",(int)width,(int)height);
+ jwriter.openMemGray(width,height);
+ jwriter.writeData(image);
+ bserver.sendJPEGImage((const unsigned char *)jwriter.getBuffer(),
+ jwriter.getWrittenLength(),
+ width,height,msg->msg_image.invoice);
+ break;
+ case CMD_HALT:
+ printf("(RE->ROBOT) halt message\n");
+ rv=SUCCEEDED;
+ printf(" RobDone with value %i, taskid %i\n",rv,
+ msg->msg_action.taskid);
+ bserver.sendDone(msg->msg_action.taskid, rv);
+ break;
+ case CMD_GOTO:
+ printf("(RE->ROBOT) cmd_goto message, nothing implemented\n");
+ break;
+ case CMD_HOME:
+ printf("(RE->ROBOT) home message, nothing implemented\n");
+ break;
+ case CMD_FOLLOW:
+ printf("(RE->ROBOT) follow message, nothing implemented\n");
+ break;
+ case CMD_COVER:
+ printf("(RE->ROBOT) cover message, nothing implemented\n");
+ break;
+ case CMD_SETPOS:
+ printf("(RE->ROBOT) setpos message, nothing implemented\n");
+ break;
+ case CMD_PAUSE:
+ printf("(RE->ROBOT) pause message, nothing implemented\n");
+ // bserver.sendPlayHalt();
+ break;
+ case CMD_RESUME:
+ printf("(RE->ROBOT) resume message, nothing implemented\n");
+ break;
+ default:
+ printf("(RE->ROBOT) Unknown mesasge %i\n", msg->hdr.type);
}
}
usleep(500*1000);
More information about the TeamTalk-developers
mailing list