FGrab::FGrab(boost::shared_ptr<AL::ALBroker> broker,
const std::string& name)
: AL::ALModule(broker, name) {
// Describe the module here. This will appear on the webpage
setModuleDescription("Grab raw frames from the active camera.");
functionName("grab_to_file", getName(), "Grab a frame and write it in a file. "
"It is not necessary to call release() afterwards.");
addParam("file", "The path and name of the file to be written");
BIND_METHOD(FGrab::grab_to_file);
functionName("grab", getName(), "Returns a buffer containing a raw YUV422 picture. "
"This buffer is not to be freed manually. Call release() for cleanup.");
setReturn("unsigned char[]", "Buffer containing the YUV422 video frame");
BIND_METHOD(FGrab::grab);
functionName("getImageSize", getName(), "Returns the size in bytes of the buffer "
"currently on hold.");
setReturn("char[]", "Buffer containing the YUV422 video frame");
BIND_METHOD(FGrab::grab);
// Release the buffer, necessary before calling grab again
functionName("release", getName(), "Releases the buffer allocated by grab().");
BIND_METHOD(FGrab::release);
functionName("switchCamera", getName(), "Switch to the camera passed in parameter.");
addParam("camera", "The camera code");
BIND_METHOD(FGrab::release);
}
WhistelDetector(boost::shared_ptr<AL::ALBroker> broker, const std::string &name): AL::ALModule(broker, name), mWhistelCount(0) {
setModuleDescription("Whistle Detector");
functionName("setPaused", getName(), "pause / unpause whistel detection");
addParam("paused", "bool for paused");
BIND_METHOD(WhistelDetector::setPaused);
}
hal_access::hal_access(std::shared_ptr<AL::ALBroker> broker_ptr, const &broker_name_ptr) :
ALModule(broker_ptr, "Pineapple"),
shm(open_or_create, "PineappleJuice", sizeof(pineappleJuice)),
semaphore(open_or_create, "HAL_SEMAPHORE", 0, 0600)
{
setModuleDescription("Communicates between the Naoqi process and our Pineapple");
try {
//Initialize DCM and memory proxy
dcm_proxy = new DCMProxy(broker_ptr);
if(dcm_proxy == NULL) throw AL::ALError(name, "constructor", "dcm_proxy == NULL");
nao_memory_proxy = new AL::ALMemoryProxy(broker_ptr);
if(nao_memory_proxy == NULL) throw AL::ALError(name, "constructor", "nao_memory_proxy == NULL");
body_ID = (std::string) nao_memory_proxy->getData("Device/DeviceList/ChestBoard/BodyId", 0);
head_ID = (std::string) nao_memory_proxy->getData("RobotConfig/Head/FullHeadId", 0);
body_version = (std::string) nao_memory_proxy->getData("RobotConfig/Body/BaseVersion", 0);
head_version = (std::string) nao_memory_proxy->getData("RobotConfig/Head/BaseVersion", 0);
//This worked when I tried it this summer (I think). The hope is, is that
//Boost will properly map this struct into shared memory.
pineappleJuice = shm.construct<pineappleJuice>("PineappleJuice")();
}
}
Bumper::Bumper(boost::shared_ptr<AL::ALBroker> broker,const std::string& name): AL::ALModule(broker, name), fCallbackMutex(AL::ALMutex::createALMutex()){
std::cout<<"Bumper"<<std::endl;
setModuleDescription("This module presents how to subscribe to a simple event (here RightBumperPressed) and use a callback method.");
functionName("onRightBumperPressed", getName(), "Method called when the right bumper is pressed. Makes a LED animation.");
BIND_METHOD(Bumper::onRightBumperPressed)
}
ALSoundProcessing::ALSoundProcessing(boost::shared_ptr<ALBroker> pBroker,
std::string pName)
: ALSoundExtractor(pBroker, pName)
{
setModuleDescription("This module processes the data collected by the "
"microphones and output in the ALMemory the RMS power "
"of each of the four channels.");
}
HandVoiceControl(AL::ALBroker::Ptr broker, const std::string& name)
: AL::ALModule(broker, std::string("HandVoiceControl"))
, mutex(AL::ALMutex::createALMutex())
{
setModuleDescription("Control hand open/close with voice commands.");
functionName("onWordRecognized", getName(), "Handle voice command notifications.");
BIND_METHOD(HandVoiceControl::onWordRecognized);
}
//______________________________________________
// constructor
//______________________________________________
mainModule::mainModule(boost::shared_ptr<AL::ALBroker> broker, const std::string& name) :
AL::ALModule(broker, name)
{
std::string bodyId, headId;
setModuleDescription("This is the Kouretes Team root module ");
functionName("Start", "mainModule", "Method to start Talws");
BIND_METHOD(mainModule::Start);
functionName("Stop", "mainModule", "Method to stop Talws");
BIND_METHOD(mainModule::Stop);
boost::shared_ptr<AL::ALMemoryProxy> memory;
KAlBroker::Instance().SetBroker(broker);
try
{
memory = KAlBroker::Instance().GetBroker()->getMemoryProxy();
} catch (AL::ALError& e)
{
std::cerr << "Error in getting memory proxy" << std::endl;
std::cout << e.what() << std::endl;
}
try
{
bodyId = std::string(memory->getData("Device/DeviceList/ChestBoard/BodyId"));
headId = std::string(memory->getData("RobotConfig/Head/HeadId"));
if (bodyId.size() > 15)
{
bodyId = bodyId.substr(16, 19);/*bodyId.size()-5, bodyId.size()-2*/ //manually because aldebarab forgot to put a \0...
} else
{
try
{
bodyId = bodyId.substr(11, 14);
} catch (const std::out_of_range& oor)
{
std::cerr << "Out of Range error: " << oor.what() <<" " << bodyId << '\n';
}
}
} catch (AL::ALError& e)
{
std::cerr << "Error in getting body and/or head id`s" << std::endl;
bodyId = "";
headId = "";
}
std::cout << "KRobot - Found Head ID: '" << headId << "'" << std::endl;
//std::cout << "KRobot - Found Body ID: '" << _toString(KRobotConfig::Instance().getConfig(KDeviceLists::Interpret::BODY_ID).size()) << "'" << std::endl;
std::cout << "KRobot - Found Body ID: '" << bodyId << "'" << std::endl;
#ifndef KROBOT_IS_REMOTE
Configurator::Instance().initConfigurator("/home/nao/naoqi/config/", headId, bodyId);
#else
Configurator::Instance().initConfigurator("./config/", "", "");
#endif
tal = new Talws();
}
// declare constructor
OnFaceDetection::OnFaceDetection(boost::shared_ptr<AL::ALBroker> broker, const std::string& name):
AL::ALModule(broker, name),
fMemoryProxy(getParentBroker()),
fFaces(AL::ALValue()),
fFaceCount(0),
fCallbackMutex(AL::ALMutex::createALMutex()) {
setModuleDescription("Module that detects faces and acts accordingly.");
functionName("callback", getName(), "");
BIND_METHOD(OnFaceDetection::callback);
}
OnFaceDetection::OnFaceDetection(
boost::shared_ptr<AL::ALBroker> broker,
const std::string& name):
AL::ALModule(broker, name),
fMemoryProxy(getParentBroker()),
fFaces(AL::ALValue()),
fFacesCount(0),
fCallbackMutex(AL::ALMutex::createALMutex())
{
setModuleDescription("This is an autogenerated module, this descriptio needs to be updated.");
functionName("callback", getName(), "");
BIND_METHOD(OnFaceDetection::callback);
}
ResponseToNameInterface::ResponseToNameInterface(boost::shared_ptr<AL::ALBroker> pBroker, const std::string& pName) : AL::ALModule(pBroker, pName) {
setModuleDescription("Interface module, reacting to events generated by the Logger module, calling child by either name or by using special phrases");
functionName("startTask", getName(), "Function used to start/enable the task");
addParam("todo", "Tell the module either to start or enable the task");
BIND_METHOD(ResponseToNameInterface::startTask);
functionName("onTactilTouched", getName(), "FrontTactilTouched callback, starts the session");
BIND_METHOD(ResponseToNameInterface::onTactilTouched);
functionName("callChild", getName(), "CallChild callback, plays the sound");
BIND_METHOD(ResponseToNameInterface::callChild);
functionName("endSession", getName(), "EndSession callback, resets the Interface");
BIND_METHOD(ResponseToNameInterface::endSession);
}
/**
* Constructor for NaoPre object
* @param broker The parent broker
* @param name The name of the module
*/
NaoPre::NaoPre(
boost::shared_ptr<AL::ALBroker> broker,
const std::string& name): AL::ALModule(broker, name)
{
setModuleDescription("Preview controller to translate zmp trajectory into com trajectory");
functionName("update", "NaoPre", "run the preview controller for one step");
BIND_METHOD(NaoPre::update);
functionName("reset", "NaoPre", "reset the preview controller");
BIND_METHOD(NaoPre::reset);
functionName("setVerbo", getName(), "set verbose level");
addParam("verbo_d", "desired verbose level");
BIND_METHOD(NaoPre::setVerbo);
verbo = 1;
}
SubscribeGyro::SubscribeGyro(
boost::shared_ptr<AL::ALBroker> broker,
const std::string& name): AL::ALModule(broker, name),
fCallbackMutex(AL::ALMutex::createALMutex())
{
setModuleDescription("This module presents how to subscribe to a simple event (here RightSubscribeGyroPressed) and use a callback method.");
//functionName("onRightSubscribeGyroPressed", getName(), "Method called when the right subscribegyro is pressed. Makes a LED animation.");
functionName("onMoveBackward", getName(), "MoveBackward");
//BIND_METHOD(SubscribeGyro::onRightSubscribeGyroPressed)
BIND_METHOD(SubscribeGyro::onMoveBackward)
functionName("onMoveForward", getName(), "MoveForward");
BIND_METHOD(SubscribeGyro::onMoveForward)
functionName("GyroStart", getName(), "StartMovement");
BIND_METHOD(SubscribeGyro::GyroStart)
//std::fstream file;
file.open("/home/nao/data/gyroswingdata.txt");
}
NaoMarkServiceDetection::NaoMarkServiceDetection(
boost::shared_ptr<AL::ALBroker> broker,
const std::string& name):
AL::ALModule(broker, name),
fMemoryProxy(getParentBroker())
{
setModuleDescription("");
_naoMarkDetected = false;
_isAllowedToMove = true;
_isThreadRuning = false;
_isMarkFound = false;
_markToFind = -1;
motionProxy = new AL::ALMotionProxy(*_robotIp,9559);
functionName("callback", getName(), "");
BIND_METHOD(NaoMarkServiceDetection::callback)
_pBackGroundThread = new boost::thread(threadEntry,this);
//_pBackGroundThread->join();
//boost::thread::join
}
@module_name@::@module_name@(boost::shared_ptr<AL::ALBroker> broker,
const std::string& name)
: AL::ALModule(broker, name)
{
// Describe the module here. This will appear on the webpage
setModuleDescription("This is automatically generated comment. Please input short description on your own module.");
/**
* Define callable methods with their descriptions:
* This makes the method available to other cpp modules
* and to python.
* The name given will be the one visible from outside the module.
* This method has no parameters or return value to describe
* functionName(<method_name>, <class_name>, <method_description>);
* BIND_METHOD(<method_reference>);
*/
functionName("printHelloWorld", getName(), "Print hello to the world");
BIND_METHOD(@module_name@::printHelloWorld);
/**
* addParam(<attribut_name>, <attribut_descrption>);
* This enables to document the parameters of the method.
* It is not compulsory to write this line.
*
* setReturn(<return_name>, <return_description>);
* This enables to document the return of the method.
* It is not compulsory to write this line.
*/
functionName("echo", getName(), "Just echo back the argument.");
setReturn("ALValue", "return argument");
BIND_METHOD(@module_name@::echo);
// If you had other methods, you could bind them here...
/**
* Bound methods can only take const ref arguments of basic types,
* or AL::ALValue or return basic types or an AL::ALValue.
*/
}
/**
* Constructor for NaoSim object
* @param broker The parent broker
* @param name The name of the module
*/
NaoSim::NaoSim(
boost::shared_ptr<AL::ALBroker> broker,
const std::string& name): AL::ALModule(broker, name),fastMotionCommands(boost::shared_ptr<AL::ALMemoryFastAccess>(new AL::ALMemoryFastAccess()))
{
std::cout << "[NaoSim::NaoSim] initialize the simulation module" << std::endl;
setModuleDescription("Simulate the Robot Motion by kinematics");
functionName("reset", "NaoSim", "reset the preview controller");
BIND_METHOD(NaoSim::reset);
functionName("setVerbo", getName(), "set verbose level");
addParam("verbo_d", "desired verbose level");
BIND_METHOD(NaoSim::setVerbo);
functionName("ControlSteps", getName(), "run the simulation");
addParam("num", "number of steps");
BIND_METHOD(NaoSim::ControlSteps);
verbo = 9;
}
NaoIK::NaoIK(boost::shared_ptr<AL::ALBroker> broker,
const std::string& name)
: AL::ALModule(broker, name), fastMotionCommands(boost::shared_ptr<AL::ALMemoryFastAccess>(new AL::ALMemoryFastAccess())),fastSensorValues(boost::shared_ptr<AL::ALMemoryFastAccess>(new AL::ALMemoryFastAccess()))
{
// Describe the module here. This will appear on the webpage
setModuleDescription("This module is used to solve the inverse kinematics of the whole body. All the Cartesian markers should be relative to left foot.");
functionName("init", getName(), "initialize the module");
BIND_METHOD(NaoIK::init);
functionName("reset", getName(), "reset the module");
BIND_METHOD(NaoIK::reset);
functionName("calIK", getName(), "calculate the inverse kinematics with current joint values");
BIND_METHOD(NaoIK::calIK);
functionName("setVerbo", getName(), "set verbose level");
addParam("verbo_d", "desired verbose level");
BIND_METHOD(NaoIK::setVerbo);
// default debug information levels
verbo=1;
}
/**
* Constructor for oru_walk object
* @param broker The parent broker
* @param name The name of the module
*/
oru_walk::oru_walk(ALPtr<ALBroker> broker, const string& name) :
ALModule(broker, name),
access_sensor_values (ALPtr<ALMemoryFastAccess>(new ALMemoryFastAccess())),
wp (broker)
{
setModuleDescription("Orebro University: NAO walking module");
// advertise functions
functionName( "setStiffness" , getName(), "change stiffness of all joint");
addParam( "value", "new stiffness value from 0.0 to 1.0");
BIND_METHOD( oru_walk::setStiffness );
functionName( "initPosition", getName() , "initialize robot position");
BIND_METHOD( oru_walk::initPosition );
functionName( "walk", getName() , "walk");
BIND_METHOD( oru_walk::walk );
functionName( "stopWalking", getName() , "stopWalking");
BIND_METHOD( oru_walk::stopWalkingRemote );
solver = NULL;
}
Man::Man(boost::shared_ptr<AL::ALBroker> broker, const std::string &name)
: AL::ALModule(broker, name),
sensorsThread("sensors", SENSORS_FRAME_LENGTH_uS),
sensors(broker),
jointEnactor(broker),
motion(),
arms(),
guardianThread("guardian", GUARDIAN_FRAME_LENGTH_uS),
guardian(),
audio(),
commThread("comm", COMM_FRAME_LENGTH_uS),
comm(MY_TEAM_NUMBER, MY_PLAYER_NUMBER),
cognitionThread("cognition", COGNITION_FRAME_LENGTH_uS),
topTranscriber(*new image::ImageTranscriber(Camera::TOP)),
bottomTranscriber(*new image::ImageTranscriber(Camera::BOTTOM)),
topConverter(TOP_TABLE_PATHNAME),
bottomConverter(BOTTOM_TABLE_PATHNAME),
vision(),
localization(),
ballTrack(),
obstacle(),
gamestate(MY_TEAM_NUMBER, MY_PLAYER_NUMBER),
behaviors(MY_TEAM_NUMBER, MY_PLAYER_NUMBER),
leds(broker),
sharedBall()
{
setModuleDescription("The Northern Bites' soccer player.");
/** Sensors **/
sensorsThread.addModule(sensors);
#ifdef LOG_SENSORS
sensorsThread.log<messages::JointAngles>(&sensors.jointsOutput_,
"joints");
sensorsThread.log<messages::JointAngles>(&sensors.temperatureOutput_,
"temperatures");
sensorsThread.log<messages::ButtonState>(&sensors.chestboardButtonOutput_,
"chestbutton");
sensorsThread.log<messages::FootBumperState>(&sensors.footbumperOutput_,
"footbumper");
sensorsThread.log<messages::InertialState>(&sensors.inertialsOutput_,
"inertials");
sensorsThread.log<messages::SonarState>(&sensors.sonarsOutput_,
"sonars");
sensorsThread.log<messages::FSR>(&sensors.fsrOutput_,
"fsrs");
sensorsThread.log<messages::BatteryState>(&sensors.batteryOutput_,
"battery");
#endif
sensorsThread.addModule(jointEnactor);
sensorsThread.addModule(motion);
sensorsThread.addModule(arms);
sensors.printInput.wireTo(&guardian.printJointsOutput, true);
motion.jointsInput_.wireTo(&sensors.jointsOutput_);
motion.inertialsInput_.wireTo(&sensors.inertialsOutput_);
motion.fsrInput_.wireTo(&sensors.fsrOutput_);
motion.stiffnessInput_.wireTo(&guardian.stiffnessControlOutput, true);
motion.bodyCommandInput_.wireTo(&behaviors.bodyMotionCommandOut, true);
motion.headCommandInput_.wireTo(&behaviors.headMotionCommandOut, true);
motion.requestInput_.wireTo(&behaviors.motionRequestOut, true);
motion.fallInput_.wireTo(&guardian.fallStatusOutput, true);
jointEnactor.jointsInput_.wireTo(&motion.jointsOutput_);
jointEnactor.stiffnessInput_.wireTo(&motion.stiffnessOutput_);
arms.actualJointsIn.wireTo(&sensors.jointsOutput_);
arms.expectedJointsIn.wireTo(&motion.jointsOutput_);
arms.handSpeedsIn.wireTo(&motion.handSpeedsOutput_);
/** Guardian **/
guardianThread.addModule(guardian);
guardianThread.addModule(audio);
guardian.temperaturesInput.wireTo(&sensors.temperatureOutput_, true);
guardian.chestButtonInput.wireTo(&sensors.chestboardButtonOutput_, true);
guardian.footBumperInput.wireTo(&sensors.footbumperOutput_, true);
guardian.inertialInput.wireTo(&sensors.inertialsOutput_, true);
guardian.fsrInput.wireTo(&sensors.fsrOutput_, true);
guardian.batteryInput.wireTo(&sensors.batteryOutput_, true);
audio.audioIn.wireTo(&guardian.audioOutput);
#ifdef LOG_GUARDIAN
guardianThread.log<messages::StiffnessControl>(
&guardian.stiffnessControlOutput,
"stiffness");
guardianThread.log<messages::FeetOnGround>(
&guardian.feetOnGroundOutput,
"feetground");
guardianThread.log<messages::FallStatus>(
&guardian.fallStatusOutput,
"fall");
guardianThread.log<messages::AudioCommand>(
&guardian.audioOutput,
"audio");
#endif
/** Comm **/
commThread.addModule(comm);
comm._worldModelInput.wireTo(&behaviors.myWorldModelOut, true);
comm._gcResponseInput.wireTo(&gamestate.gcResponseOutput, true);
#ifdef LOG_COMM
commThread.log<messages::GameState>(&comm._gameStateOutput, "gamestate");
#endif
/** Cognition **/
// Turn ON the finalize method for images, which we've specialized
portals::Message<messages::YUVImage>::setFinalize(true);
portals::Message<messages::ThresholdImage>::setFinalize(true);
portals::Message<messages::PackedImage16>::setFinalize(true);
portals::Message<messages::PackedImage8>::setFinalize(true);
cognitionThread.addModule(topTranscriber);
cognitionThread.addModule(bottomTranscriber);
cognitionThread.addModule(topConverter);
cognitionThread.addModule(bottomConverter);
cognitionThread.addModule(vision);
cognitionThread.addModule(localization);
cognitionThread.addModule(ballTrack);
cognitionThread.addModule(obstacle);
cognitionThread.addModule(gamestate);
cognitionThread.addModule(behaviors);
cognitionThread.addModule(leds);
cognitionThread.addModule(sharedBall);
topTranscriber.jointsIn.wireTo(&sensors.jointsOutput_, true);
topTranscriber.inertsIn.wireTo(&sensors.inertialsOutput_, true);
bottomTranscriber.jointsIn.wireTo(&sensors.jointsOutput_, true);
bottomTranscriber.inertsIn.wireTo(&sensors.inertialsOutput_, true);
topConverter.imageIn.wireTo(&topTranscriber.imageOut);
bottomConverter.imageIn.wireTo(&bottomTranscriber.imageOut);
vision.topThrImage.wireTo(&topConverter.thrImage);
vision.topYImage.wireTo(&topConverter.yImage);
vision.topUImage.wireTo(&topConverter.uImage);
vision.topVImage.wireTo(&topConverter.vImage);
vision.botThrImage.wireTo(&bottomConverter.thrImage);
vision.botYImage.wireTo(&bottomConverter.yImage);
vision.botUImage.wireTo(&bottomConverter.uImage);
vision.botVImage.wireTo(&bottomConverter.vImage);
vision.joint_angles.wireTo(&topTranscriber.jointsOut, true);
vision.inertial_state.wireTo(&topTranscriber.inertsOut, true);
localization.visionInput.wireTo(&vision.vision_field);
localization.motionInput.wireTo(&motion.odometryOutput_, true);
localization.resetInput.wireTo(&behaviors.resetLocOut, true);
localization.gameStateInput.wireTo(&gamestate.gameStateOutput);
localization.ballInput.wireTo(&ballTrack.ballLocationOutput);
ballTrack.visionBallInput.wireTo(&vision.vision_ball);
ballTrack.odometryInput.wireTo(&motion.odometryOutput_, true);
ballTrack.localizationInput.wireTo(&localization.output, true);
for (int i = 0; i < NUM_PLAYERS_PER_TEAM; ++i)
{
sharedBall.worldModelIn[i].wireTo(comm._worldModels[i], true);
}
obstacle.armContactIn.wireTo(&arms.contactOut, true);
obstacle.sonarIn.wireTo(&sensors.sonarsOutput_, true);
gamestate.commInput.wireTo(&comm._gameStateOutput, true);
gamestate.buttonPressInput.wireTo(&guardian.advanceStateOutput, true);
gamestate.initialStateInput.wireTo(&guardian.initialStateOutput, true);
gamestate.switchTeamInput.wireTo(&guardian.switchTeamOutput, true);
gamestate.switchKickOffInput.wireTo(&guardian.switchKickOffOutput, true);
behaviors.localizationIn.wireTo(&localization.output);
behaviors.filteredBallIn.wireTo(&ballTrack.ballLocationOutput);
behaviors.gameStateIn.wireTo(&gamestate.gameStateOutput);
behaviors.visionFieldIn.wireTo(&vision.vision_field);
behaviors.visionRobotIn.wireTo(&vision.vision_robot);
behaviors.visionObstacleIn.wireTo(&vision.vision_obstacle);
behaviors.fallStatusIn.wireTo(&guardian.fallStatusOutput, true);
behaviors.motionStatusIn.wireTo(&motion.motionStatusOutput_, true);
behaviors.odometryIn.wireTo(&motion.odometryOutput_, true);
behaviors.jointsIn.wireTo(&sensors.jointsOutput_, true);
behaviors.stiffStatusIn.wireTo(&sensors.stiffStatusOutput_, true);
behaviors.obstacleIn.wireTo(&obstacle.obstacleOut);
for (int i = 0; i < NUM_PLAYERS_PER_TEAM; ++i)
{
behaviors.worldModelIn[i].wireTo(comm._worldModels[i], true);
}
leds.ledCommandsIn.wireTo(&behaviors.ledCommandOut);
#ifdef LOG_LOCATION
cognitionThread.log<messages::RobotLocation>(&localization.output, "location");
#endif
#ifdef LOG_ODOMETRY
cognitionThread.log<messages::RobotLocation>(&motion.odometryOutput_, "odometry");
#endif
#ifdef LOG_OBSERVATIONS
cognitionThread.log<messages::VisionField>(&vision.vision_field, "observations");
#endif
#ifdef LOG_LOCALIZATION
cognitionThread.log<messages::ParticleSwarm>(&localization.particleOutput, "particleSwarm");
#endif
#ifdef LOG_BALLTRACK
cognitionThread.log<messages::FilteredBall>(&ballTrack.ballLocationOutput, "filtered_ball");
cognitionThread.log<messages::VisionBall>(&vision.vision_ball, "vision_ball");
#endif
#ifdef LOG_IMAGES
cognitionThread.logImage<messages::YUVImage>(&topTranscriber.imageOut,
"top");
cognitionThread.logImage<messages::YUVImage>(&bottomTranscriber.imageOut,
"bottom");
#endif
#ifdef LOG_VISION
cognitionThread.log<messages::VisionField>(&vision.vision_field,
"field");
cognitionThread.log<messages::VisionBall>(&vision.vision_ball,
"ball");
cognitionThread.log<messages::VisionRobot>(&vision.vision_robot,
"robot");
cognitionThread.log<messages::VisionObstacle>(&vision.vision_obstacle,
"obstacle");
cognitionThread.log<messages::JointAngles>(&vision.joint_angles_out,
"vision_joints");
cognitionThread.log<messages::InertialState>(&vision.inertial_state_out,
"vision_inertials");
#endif
#ifdef USE_TIME_PROFILING
Profiler::getInstance()->profileFrames(1400);
#endif
startSubThreads();
}
/**
* The constructor sets up the structures required to communicate with NAOqi.
* @param pBroker A NAOqi broker that allows accessing other NAOqi modules.
*/
GameCtrl(boost::shared_ptr<AL::ALBroker> pBroker)
: ALModule(pBroker, "GameCtrl"),
proxy(0),
memory(0),
udp(0),
teamNumber(0)
{
setModuleDescription("A module that provides packets from the GameController.");
assert(numOfButtons == sizeof(buttonNames) / sizeof(*buttonNames));
assert(numOfLEDs == sizeof(ledNames) / sizeof(*ledNames));
init();
try
{
memory = new AL::ALMemoryProxy(pBroker);
proxy = new AL::DCMProxy(pBroker);
AL::ALValue params;
AL::ALValue result;
params.arraySetSize(1);
params.arraySetSize(2);
params[0] = std::string("leds");
params[1].arraySetSize(numOfLEDs);
for(int i = 0; i < numOfLEDs; ++i)
params[1][i] = std::string(ledNames[i]);
result = proxy->createAlias(params);
assert(result == params);
ledRequest.arraySetSize(6);
ledRequest[0] = std::string("leds");
ledRequest[1] = std::string("ClearAll");
ledRequest[2] = std::string("time-separate");
ledRequest[3] = 0;
ledRequest[4].arraySetSize(1);
ledRequest[5].arraySetSize(numOfLEDs);
for(int i = 0; i < numOfLEDs; ++i)
ledRequest[5][i].arraySetSize(1);
for(int i = 0; i < numOfButtons; ++i)
buttons[i] = (float*) memory->getDataPtr(buttonNames[i]);
// If no color was set, set it to black (no LED).
// This actually has a race condition.
if(memory->getDataList("GameCtrl/teamColour").empty())
memory->insertData("GameCtrl/teamColour", TEAM_BLACK);
playerNumber = (int*) memory->getDataPtr("GameCtrl/playerNumber");
teamNumberPtr = (int*) memory->getDataPtr("GameCtrl/teamNumber");
defaultTeamColour = (int*) memory->getDataPtr("GameCtrl/teamColour");
// register "onPreProcess" and "onPostProcess" callbacks
theInstance = this;
proxy->getGenericProxy()->getModule()->atPreProcess(&onPreProcess);
proxy->getGenericProxy()->getModule()->atPostProcess(&onPostProcess);
udp = new UdpComm();
if(!udp->setBlocking(false) ||
!udp->setBroadcast(true) ||
!udp->bind("0.0.0.0", GAMECONTROLLER_DATA_PORT) ||
!udp->setLoopback(false))
{
fprintf(stderr, "libgamectrl: Could not open UDP port\n");
delete udp;
udp = 0;
// continue, because button interface will still work
}
publish();
}
catch(AL::ALError& e)
{
fprintf(stderr, "libgamectrl: %s\n", e.what());
close();
}
}
Diagnostics(AL::ALBroker::Ptr broker, const std::string& name)
: AL::ALModule(broker, std::string("Diagnostics"))
, mutex(AL::ALMutex::createALMutex())
{
setModuleDescription("Report various diagnostics.");
}
/**
* The constructor initializes the shared memory for communicating with bhuman.
* It also establishes a communication with NaoQi and prepares all data structures
* required for this communication.
* @param pBroker A NaoQi broker that allows accessing other NaoQi modules.
*/
BHuman(boost::shared_ptr<AL::ALBroker> pBroker) :
ALModule(pBroker, "BHuman"),
data((LBHData*) MAP_FAILED),
sem(SEM_FAILED),
proxy(0),
memory(0),
dcmTime(0),
lastReadingActuators(-1),
actuatorDrops(0),
frameDrops(allowedFrameDrops + 1),
state(sitting),
phase(0.f),
ledIndex(0),
rightEarLEDsChangedTime(0),
startPressedTime(0),
lastBHumanStartTime(0)
{
setModuleDescription("A module that provides basic ipc NaoQi DCM access using shared memory.");
fprintf(stderr, "libbhuman: Starting.\n");
assert(lbhNumOfSensorIds == sizeof(sensorNames) / sizeof(*sensorNames));
assert(lbhNumOfActuatorIds == sizeof(actuatorNames) / sizeof(*actuatorNames));
assert(lbhNumOfTeamInfoIds == sizeof(teamInfoNames) / sizeof(*teamInfoNames));
// create shared memory
memoryHandle = shm_open(LBH_MEM_NAME, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR);
if(memoryHandle == -1)
perror("libbhuman: shm_open");
else if(ftruncate(memoryHandle, sizeof(LBHData)) == -1)
perror("libbhuman: ftruncate");
else
{
// map the shared memory
data = (LBHData*) mmap(NULL, sizeof(LBHData), PROT_READ | PROT_WRITE, MAP_SHARED, memoryHandle, 0);
if(data == MAP_FAILED)
perror("libbhuman: mmap");
else
{
memset(data, 0, sizeof(LBHData));
// open semaphore
sem = sem_open(LBH_SEM_NAME, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR, 0);
if(sem == SEM_FAILED)
perror("libbhuman: sem_open");
else
try
{
// get the robot name
memory = new AL::ALMemoryProxy(pBroker);
std::string robotName = (std::string) memory->getData("Device/DeviceList/ChestBoard/BodyNickName", 0);
strncpy(data->robotName, robotName.c_str(), sizeof(data->robotName));
// create "positionRequest" and "hardnessRequest" alias
proxy = new AL::DCMProxy(pBroker);
AL::ALValue params;
AL::ALValue result;
params.arraySetSize(1);
params.arraySetSize(2);
params[0] = std::string("positionActuators");
params[1].arraySetSize(lbhNumOfPositionActuatorIds);
for(int i = 0; i < lbhNumOfPositionActuatorIds; ++i)
params[1][i] = std::string(actuatorNames[i]);
result = proxy->createAlias(params);
params[0] = std::string("hardnessActuators");
params[1].arraySetSize(lbhNumOfHardnessActuatorIds);
for(int i = 0; i < lbhNumOfHardnessActuatorIds; ++i)
params[1][i] = std::string(actuatorNames[headYawHardnessActuator + i]);
result = proxy->createAlias(params);
params[0] = std::string("usRequest");
params[1].arraySetSize(1);
params[1][0] = std::string(actuatorNames[usActuator]);
result = proxy->createAlias(params);
// prepare positionRequest
positionRequest.arraySetSize(6);
positionRequest[0] = std::string("positionActuators");
positionRequest[1] = std::string("ClearAll");
positionRequest[2] = std::string("time-separate");
positionRequest[3] = 0;
positionRequest[4].arraySetSize(1);
positionRequest[5].arraySetSize(lbhNumOfPositionActuatorIds);
for(int i = 0; i < lbhNumOfPositionActuatorIds; ++i)
positionRequest[5][i].arraySetSize(1);
// prepare hardnessRequest
hardnessRequest.arraySetSize(6);
hardnessRequest[0] = std::string("hardnessActuators");
hardnessRequest[1] = std::string("ClearAll");
hardnessRequest[2] = std::string("time-separate");
hardnessRequest[3] = 0;
hardnessRequest[4].arraySetSize(1);
hardnessRequest[5].arraySetSize(lbhNumOfHardnessActuatorIds);
for(int i = 0; i < lbhNumOfHardnessActuatorIds; ++i)
hardnessRequest[5][i].arraySetSize(1);
// prepare usRequest
usRequest.arraySetSize(6);
usRequest[0] = std::string("usRequest");
usRequest[1] = std::string("Merge"); // doesn't work with "ClearAll"
usRequest[2] = std::string("time-separate");
usRequest[3] = 0;
usRequest[4].arraySetSize(1);
usRequest[5].arraySetSize(1);
usRequest[5][0].arraySetSize(1);
// prepare ledRequest
ledRequest.arraySetSize(3);
ledRequest[1] = std::string("ClearAll");
ledRequest[2].arraySetSize(1);
ledRequest[2][0].arraySetSize(2);
ledRequest[2][0][1] = 0;
// prepare sensor pointers
for(int i = 0; i < lbhNumOfSensorIds; ++i)
sensorPtrs[i] = (float*) memory->getDataPtr(sensorNames[i]);
resetUsMeasurements();
// initialize requested actuators
memset(requestedActuators, 0, sizeof(requestedActuators));
for(int i = faceLedRedLeft0DegActuator; i < chestBoardLedRedActuator; ++i)
requestedActuators[i] = -1.f;
// register "onPreProcess" and "onPostProcess" callbacks
theInstance = this;
proxy->getGenericProxy()->getModule()->atPreProcess(&onPreProcess);
proxy->getGenericProxy()->getModule()->atPostProcess(&onPostProcess);
fprintf(stderr, "libbhuman: Started!\n");
return; // success
}
catch(AL::ALError& e)
{
fprintf(stderr, "libbhuman: %s\n", e.toString().c_str());
}
}
}
close(); // error
}