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();
}
Man::Man() :
param("/home/nao/nbites/lib/parameters.json"),
playerNum(param.getParam<int>("playerNumber")),
teamNum(param.getParam<int>("teamNumber")),
robotName(param.getParam<std::string>("robotName")),
sensorsThread("sensors", SENSORS_FRAME_LENGTH_uS),
sensors(),
jointEnactor(),
motion(),
arms(),
guardianThread("guardian", GUARDIAN_FRAME_LENGTH_uS),
guardian(),
audio(),
commThread("comm", COMM_FRAME_LENGTH_uS),
comm(teamNum, playerNum),
cognitionThread("cognition", COGNITION_FRAME_LENGTH_uS),
topTranscriber(*new image::ImageTranscriber(Camera::TOP)),
bottomTranscriber(*new image::ImageTranscriber(Camera::BOTTOM)),
vision(640, 480, robotName),
localization(),
ballTrack(),
obstacle("/home/nao/nbites/Config/obstacleParams.txt", robotName),
gamestate(teamNum, playerNum),
behaviors(teamNum, playerNum),
sharedBall(playerNum)
{
/** Sensors **/
sensorsThread.addModule(sensors);
sensorsThread.addModule(jointEnactor);
sensorsThread.addModule(motion);
sensorsThread.addModule(arms);
sensors.printInput.wireTo(&guardian.printJointsOutput, true);
motion.jointsInput_.wireTo(&sensors.jointsOutput_);
motion.currentsInput_.wireTo(&sensors.currentsOutput_);
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_);
jointEnactor.ledsInput_.wireTo(&behaviors.ledCommandOut, true);
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);
guardian.motionStatusIn.wireTo(&motion.motionStatusOutput_, true);
audio.audioIn.wireTo(&guardian.audioOutput);
/** Comm **/
commThread.addModule(comm);
comm._worldModelInput.wireTo(&behaviors.myWorldModelOut, true);
comm._gcResponseInput.wireTo(&gamestate.gcResponseOutput, true);
/** 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(vision);
cognitionThread.addModule(localization);
cognitionThread.addModule(ballTrack);
cognitionThread.addModule(obstacle);
cognitionThread.addModule(gamestate);
cognitionThread.addModule(behaviors);
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);
vision.topIn.wireTo(&topTranscriber.imageOut);
vision.bottomIn.wireTo(&bottomTranscriber.imageOut);
vision.jointsIn.wireTo(&topTranscriber.jointsOut, true);
vision.inertsIn.wireTo(&topTranscriber.inertsOut, true);
localization.motionInput.wireTo(&motion.odometryOutput_, true);
localization.resetInput[0].wireTo(&behaviors.resetLocOut, true);
localization.resetInput[1].wireTo(&sharedBall.sharedBallReset, true);
localization.gameStateInput.wireTo(&gamestate.gameStateOutput);
localization.visionInput.wireTo(&vision.visionOut);
localization.ballInput.wireTo(&ballTrack.ballLocationOutput);
ballTrack.visionInput.wireTo(&vision.visionOut);
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);
}
sharedBall.locIn.wireTo(&localization.output);
sharedBall.ballIn.wireTo(&ballTrack.ballLocationOutput);
obstacle.armContactIn.wireTo(&arms.contactOut, true);
obstacle.visionIn.wireTo(&vision.robotObstacleOut, 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.linesOut);
// 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.sitDownIn.wireTo(&sensors.sitDownOutput_, true);
behaviors.visionIn.wireTo(&vision.visionOut, true);
behaviors.obstacleIn.wireTo(&obstacle.obstacleOut);
behaviors.sharedBallIn.wireTo(&sharedBall.sharedBallOutput);
behaviors.sharedFlipIn.wireTo(&sharedBall.sharedBallReset, true);
for (int i = 0; i < NUM_PLAYERS_PER_TEAM; ++i)
{
behaviors.worldModelIn[i].wireTo(comm._worldModels[i], true);
}
#ifdef USE_LOGGING
{ //brackets let us hide logging code in certain IDEs.
/*
log threads should have low CPU time if nothing is being logged.
That being said, should probably not init (i.e. start threads)
if not necessary.
*/
#ifdef V5_ROBOT
nblog::HOST_TYPE = nblog::V5ROBOT;
#else
nblog::HOST_TYPE = nblog::V4ROBOT;
#endif
printf("nblog::log_main_init()\n");
nblog::log_main_init();
printf("control::control_init()\n");
control::control_init();
#ifdef START_WITH_FILEIO
#ifndef USE_LOGGING
#error "option START_WITH_FILEIO defined WITHOUT option USE_LOGGING"
#endif
printf("CONTROL: Starting with fileio flag set!\n");
control::flags[control::fileio] = 1;
#endif
#ifdef START_WITH_THUMBNAIL
#ifndef USE_LOGGING
#error "option START_WITH_THUMBNAIL defined WITHOUT option USE_LOGGING"
#endif
printf("CONTROL: Starting with thumbnail flag set!\n");
control::flags[control::thumbnail] = 1;
#endif
/*
SPECIFIC MODULE LOGGING
*/
sensorsThread.log<messages::JointAngles>((control::SENSORS), &sensors.jointsOutput_,
"proto-JointAngles", "sensorsThread");
sensorsThread.log<messages::JointAngles>((control::SENSORS), &sensors.temperatureOutput_,
"proto-JointAngles", "sensorsThread");
sensorsThread.log<messages::ButtonState>((control::SENSORS), &sensors.chestboardButtonOutput_,
"proto-ButtonState", "sensorsThread");
sensorsThread.log<messages::FootBumperState>((control::SENSORS), &sensors.footbumperOutput_,
"proto-FootBumperState", "sensorsThread");
sensorsThread.log<messages::InertialState>((control::SENSORS), &sensors.inertialsOutput_,
"proto-InertialState", "sensorsThread");
sensorsThread.log<messages::SonarState>((control::SENSORS), &sensors.sonarsOutput_,
"proto-SonarState", "sensorsThread");
sensorsThread.log<messages::FSR>((control::SENSORS), &sensors.fsrOutput_,
"proto-FSR", "sensorsThread");
sensorsThread.log<messages::BatteryState>((control::SENSORS), &sensors.batteryOutput_,
"proto-BatteryState", "sensorsThread");
guardianThread.log<messages::StiffnessControl>((control::GUARDIAN), &guardian.stiffnessControlOutput,
"proto-StiffnessControl", "guardianThread");
guardianThread.log<messages::FeetOnGround>((control::GUARDIAN), &guardian.feetOnGroundOutput,
"proto-FeetOnGround", "guardianThread");
guardianThread.log<messages::FallStatus>((control::GUARDIAN), &guardian.fallStatusOutput,
"proto-FallStatus", "guardianThread");
guardianThread.log<messages::AudioCommand>((control::GUARDIAN), &guardian.audioOutput,
"proto-AudioCommand", "guardianThread");
// cognitionThread.log<messages::RobotLocation>((control::LOCATION), &localization.output, "proto-RobotLocation", "location");
// cognitionThread.log<messages::RobotLocation>((control::ODOMETRY), &motion.odometryOutput_, "proto-RobotLocation", "odometry");
// cognitionThread.log<messages::VisionField>((control::OBSERVATIONS), &vision.vision_field, "proto-VisionField", "observations");
// cognitionThread.log<messages::ParticleSwarm>((control::LOCALIZATION), &localization.particleOutput, "proto-ParticleSwarm", "localization");
// cognitionThread.log<messages::FilteredBall>((control::BALLTRACK), &ballTrack.ballLocationOutput, "proto-FilteredBall", "balltrack");
// cognitionThread.log<messages::VisionBall>((control::BALLTRACK), &vision.vision_ball, "proto-VisionBall", "balltrack");
cognitionThread.log<messages::Vision>((control::VISION), &vision.visionOut,
"proto-Vision", "vision");
// cognitionThread.log<messages::VisionField>((control::VISION), &vision.vision_field,
// "proto-VisionField", "vision");
// cognitionThread.log<messages::VisionBall>((control::VISION), &vision.vision_ball,
// "proto-VisionBall", "vision");
// cognitionThread.log<messages::VisionRobot>((control::VISION), &vision.vision_robot,
// "proto-VisionRobot", "vision");
// cognitionThread.log<messages::VisionObstacle>((control::VISION), &vision.vision_obstacle,
// "proto-VisionObstacle", "vision");
// cognitionThread.log<messages::JointAngles>((control::VISION), &vision.joint_angles_out,
// "proto-JointAngles", "vision");
// cognitionThread.log<messages::InertialState>((control::VISION), &vision.inertial_state_out,
// "proto-InertialState", "vision");
}
#endif //USE_LOGGING
#ifdef USE_TIME_PROFILING
Profiler::getInstance()->profileFrames(1400);
#endif
startSubThreads();
std::cout << "Man built" << std::endl;
}
