test-RobotMessage.C

00001 #include "Component/ModelManager.H"
00002 #include "Component/ModelComponent.H"
00003 #include "Component/ModelOptionDef.H"
00004 #include "Ice/SimEventsUtils.H"
00005 #include <Ice/Ice.h>
00006 #include "Ice/RobotSimEvents.ice.H"
00007 
00008 int main(int argc, char *argv[]) {
00009 
00010     Ice::CommunicatorPtr ic;
00011     ic = Ice::initialize(argc, argv);
00012 
00013     ModelManager itsMgr("TestRobotMessageManager");
00014     itsMgr.parseCommandLine((const int)argc, (const char**)argv, "<ActionMessage=1|GPSMessage=2|GoalMessage=3|TrainImageMessage=4>", 1, -1);
00015 
00016     int messageType = atoi(itsMgr.getExtraArg(0).c_str());
00017     switch(messageType) {
00018       case 1:
00019         {
00020           float transVel = atof(itsMgr.getExtraArg(1).c_str());
00021           float rotVel   = atof(itsMgr.getExtraArg(2).c_str());
00022           LINFO("TransVel = %f, RotVel = %f", transVel, rotVel);
00023 
00024           RobotSimEvents::EventsPrx actionPublisher =
00025             RobotSimEvents::EventsPrx::uncheckedCast(
00026                 SimEventsUtils::getPublisher(ic, "ActionMessageTopic")
00027                 );
00028 
00029           RobotSimEvents::ActionMessagePtr actionMessage =
00030             new RobotSimEvents::ActionMessage;
00031 
00032           actionMessage->transVel = transVel;
00033           actionMessage->rotVel = rotVel;
00034 
00035           actionPublisher->updateMessage(actionMessage);
00036         }
00037         break;
00038       case 2:
00039         {
00040           float xPos = atof(itsMgr.getExtraArg(1).c_str());
00041           float yPos   = atof(itsMgr.getExtraArg(2).c_str());
00042           float ori   = atof(itsMgr.getExtraArg(3).c_str());
00043           LINFO("posX = %f, posY = %f, ori = %f", xPos, yPos, ori);
00044 
00045           RobotSimEvents::EventsPrx actionPublisher =
00046             RobotSimEvents::EventsPrx::uncheckedCast(
00047                 SimEventsUtils::getPublisher(ic, "GPSMessageTopic")
00048                 );
00049 
00050           RobotSimEvents::GPSMessagePtr gpsMessage =
00051             new RobotSimEvents::GPSMessage;
00052 
00053           gpsMessage->xPos = xPos;
00054           gpsMessage->yPos = yPos;
00055           gpsMessage->orientation = ori;
00056 
00057 
00058           actionPublisher->updateMessage(gpsMessage);
00059         }
00060         break;
00061       case 3:
00062         {
00063           float xPos = itsMgr.getExtraArgAs<float>(1);
00064           float yPos   = itsMgr.getExtraArgAs<float>(2);
00065           float ori   = itsMgr.getExtraArgAs<float>(3);
00066           LINFO("Setting goal posX = %f, posY = %f, ori = %f", xPos, yPos, ori);
00067 
00068           RobotSimEvents::EventsPrx actionPublisher =
00069             RobotSimEvents::EventsPrx::uncheckedCast(
00070                 SimEventsUtils::getPublisher(ic, "GoalStateMessageTopic")
00071                 );
00072 
00073           RobotSimEvents::GoalStateMessagePtr goalMessage =
00074             new RobotSimEvents::GoalStateMessage;
00075 
00076           goalMessage->xPos = xPos;
00077           goalMessage->yPos = yPos;
00078           goalMessage->orientation = ori;
00079 
00080           actionPublisher->updateMessage(goalMessage);
00081         }
00082         break;
00083       case 4:
00084         {
00085 
00086         }
00087         break;
00088 
00089     }
00090 
00091     return 0;
00092 }
00093 
Generated on Sun May 8 08:41:32 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3