void NaoMarkServiceDetection::callback(const std::string &key, const AL::ALValue &value, const AL::ALValue &msg) { AL::ALValue marks = fMemoryProxy.getData("LandmarkDetected"); if(marks.getSize() > 0 && !_isMarkFound) { int TimeStampField = marks[0][1]; if((int)marks[1][0][1][0] == _markToFind) { if((float)marks[1][0][0][3] < 0.2f) { if(_isAllowedToMove) motionProxy->moveToward(0.5f,0,marks[1][0][0][1]); _naoMarkDetected = true; } else { motionProxy->moveToward(0,0,0); _isMarkFound = true; } } else motionProxy->moveToward(0,0,0); } }
void SensorsModule::initializeSonarValues() { // Get a proxy to the DCM. boost::shared_ptr<AL::DCMProxy> dcmProxy = broker_->getDcmProxy(); if(dcmProxy != 0) { try { // For DCM::set() see: // http://www.aldebaran-robotics.com/documentation/naoqi/sensors/dcm-api.html#DCMProxy::set__AL::ALValueCR AL::ALValue dcmSonarCommand; dcmSonarCommand.arraySetSize(3); dcmSonarCommand[0] = std::string("Device/SubDeviceList/US/Actuator/Value"); // Device name. dcmSonarCommand[1] = std::string("ClearAll"); // Delete all timed commands before adding this one. dcmSonarCommand[2].arraySetSize(1); // A list of (1) timed-commands. dcmSonarCommand[2][0].arraySetSize(2); dcmSonarCommand[2][0][0] = 68.0; // The command itself. dcmSonarCommand[2][0][1] = dcmProxy->getTime(0); // The DCM time for the command to be applied. // Send the timed command to the sonars. dcmProxy->set(dcmSonarCommand); } catch(AL::ALError& e) { std::cout << "SensorsModule : Failed to initialize sonars, " << e.toString() << std::endl; } } }
void ParseWordRecognizedArray(std::vector<WordConfidencePair>& results, const AL::ALValue& value, float recognitionThreshold) { // unexpected data if (value.getType() != AL::ALValue::TypeArray) MODULE_ERROR("invalid array type"); // unexpected data if (value.getSize() % 2 != 0) MODULE_ERROR("invalid array size"); for (int i = 0; i < (int)value.getSize(); i += 2) { AL::ALValue wordValue = value[i]; AL::ALValue confidenceValue = value[i + 1]; float confidence = confidenceValue.operator float &(); if (confidence >= recognitionThreshold) { WordConfidencePair pair = { wordValue.toString(), confidence }; results.push_back(pair); } } std::sort(results.begin(), results.end(), WordConfidencePairComparison()); }
/** * \brief: Create the aliases for controlling the joint stiffnesses. **/ void hal_experimental::create_actuator_stiffness_aliases() { LOG("[create_actuator_stiffness]", "create actuator stiffness initializing..."); AL::ALValue jointAliases; try { jointAliases.clear(); jointAliases.arraySetSize(2); jointAliases[0] = std::string("jointStiffness"); jointAliases[1].arraySetSize(25); int j = 0; for(int i = NumOfPositionActuatorIds; i <= rAnkleRollStiffnessActuator; ++i) { jointAliases[1][j] = actuatorNames[i]; ++j; } // jointAliases[1][HEAD_PITCH] = std::string("Device/SubDeviceList/HeadPitch/Hardness/Actuator/Value"); // jointAliases[1][HEAD_YAW] = std::string("Device/SubDeviceList/HeadYaw/Hardness/Actuator/Value"); // jointAliases[1][L_ANKLE_PITCH] = std::string("Device/SubDeviceList/LAnklePitch/Hardness/Actuator/Value"); // jointAliases[1][L_ANKLE_ROLL] = std::string("Device/SubDeviceList/LAnkleRoll/Hardness/Actuator/Value"); // jointAliases[1][L_ELBOW_ROLL] = std::string("Device/SubDeviceList/LElbowRoll/Hardness/Actuator/Value"); // jointAliases[1][L_ELBOW_YAW] = std::string("Device/SubDeviceList/LElbowYaw/Hardness/Actuator/Value"); // jointAliases[1][L_HAND] = std::string("Device/SubDeviceList/LHand/Hardness/Actuator/Value"); // jointAliases[1][L_HIP_PITCH] = std::string("Device/SubDeviceList/LHipPitch/Hardness/Actuator/Value"); // jointAliases[1][L_HIP_ROLL] = std::string("Device/SubDeviceList/LHipRoll/Hardness/Actuator/Value"); // jointAliases[1][L_HIP_YAW_PITCH] = std::string("Device/SubDeviceList/LHipYawPitch/Hardness/Actuator/Value"); // jointAliases[1][L_KNEE_PITCH] = std::string("Device/SubDeviceList/LKneePitch/Hardness/Actuator/Value"); // jointAliases[1][L_SHOULDER_PITCH] = std::string("Device/SubDeviceList/LShoulderPitch/Hardness/Actuator/Value"); // jointAliases[1][L_SHOULDER_ROLL] = std::string("Device/SubDeviceList/LShoulderRoll/Hardness/Actuator/Value"); // jointAliases[1][L_WRIST_YAW] = std::string("Device/SubDeviceList/LWristYaw/Hardness/Actuator/Value"); // jointAliases[1][R_ANKLE_PITCH] = std::string("Device/SubDeviceList/RAnklePitch/Hardness/Actuator/Value"); // jointAliases[1][R_ANKLE_ROLL] = std::string("Device/SubDeviceList/RAnkleRoll/Hardness/Actuator/Value"); // jointAliases[1][R_ELBOW_ROLL] = std::string("Device/SubDeviceList/RElbowRoll/Hardness/Actuator/Value"); // jointAliases[1][R_ELBOW_YAW] = std::string("Device/SubDeviceList/RElbowYaw/Hardness/Actuator/Value"); // jointAliases[1][R_HAND] = std::string("Device/SubDeviceList/RHand/Hardness/Actuator/Value"); // jointAliases[1][R_HIP_PITCH] = std::string("Device/SubDeviceList/RHipPitch/Hardness/Actuator/Value"); // jointAliases[1][R_HIP_ROLL] = std::string("Device/SubDeviceList/RHipRoll/Hardness/Actuator/Value"); // jointAliases[1][R_KNEE_PITCH] = std::string("Device/SubDeviceList/RKneePitch/Hardness/Actuator/Value"); // jointAliases[1][R_SHOULDER_PITCH] = std::string("Device/SubDeviceList/RShoulderPitch/Hardness/Actuator/Value"); // jointAliases[1][R_SHOULDER_ROLL] = std::string("Device/SubDeviceList/RShoulderRoll/Hardness/Actuator/Value"); // jointAliases[1][R_WRIST_YAW] = std::string("Device/SubDeviceList/RWristYaw/Hardness/Actuator/Value"); } catch(const AL::ALError &e) { std::cout << "[create_actuator_stiffness][ERROR]: Error setting up the aliase: " << e.toString() << std::endl; } try { dcm_proxy->createAlias(jointAliases); log_file << "stiffness alias added to DCM!\n"; } catch(const AL::ALError &e) { LOG("[create_actuator_stiffness]","[ERROR]: An error occured while creating stiffness actuator aliases: " + e.toString()); } LOG("[create_actuator_stiffness]","create actuator stiffness initialized"); }
int main(int argc, char **argv) { std::string robotIp = "127.0.0.1"; if (argc < 2) { std::cerr << "Usage: almotion_setBreathConfig robotIp " << "(optional default \"127.0.0.1\")."<< std::endl; } else { robotIp = argv[1]; } AL::ALMotionProxy motion(robotIp); // Example showing how to change the breathing configuration // Setting a relaxed configuration: 5 breaths per minute at max amplitude AL::ALValue breathConfig; breathConfig.arraySetSize(2); AL::ALValue tmp; tmp.arraySetSize(2); tmp[0] = "Bpm"; tmp[1] = 5.0f; breathConfig[0] = tmp; tmp[0] = "Amplitude"; tmp[1] = 1.0f; breathConfig[1] = tmp; motion.setBreathConfig(breathConfig); return 0; }
gmVariable GMALProxy::CallReturnVariable(const char* function, gmVariable arg) { const std::string function_string = std::string(function); AL::ALValue result = AL::ALValue(); switch (arg.m_type) { case GM_INT: result = _proxy.call<AL::ALValue>(function_string, arg.GetInt()); break; case GM_FLOAT: result = _proxy.call<AL::ALValue>(function_string, arg.GetFloat()); break; case GM_VEC2: result = _proxy.call<AL::ALValue>(function_string, arg.GetVec2().x, arg.GetVec2().y); break; case GM_STRING: result = _proxy.call<AL::ALValue>(function_string, std::string(arg.GetCStringSafe())); break; default: result = _proxy.call<AL::ALValue>(function_string); break; } gmVariable result_variable; result_variable.Nullify(); switch (result.getType()) { case AL::ALValue::TypeBool: result_variable = gmVariable(int(result.operator bool &() ? 1 : 0)); break; case AL::ALValue::TypeInt: result_variable = gmVariable(int(result.operator int &())); break; case AL::ALValue::TypeFloat: result_variable = gmVariable(float(result.operator float &())); break; case AL::ALValue::TypeString: result_variable = gmVariable(VirtualMachine::Get()->GetVM().AllocStringObject(result.toString().c_str())); break; default: break; } return result_variable; }
Variant ALProxy::call(const std::string &method, const Variant &val) { AL::ALValue param; AL::ALValue result; Variant vResult(0); param.arrayPush(val.toALValue()); fProxy->genericCall(method, param,result); vResult.fromALValue(result); return vResult; }
/** * \brief: Creates an alias for all the position actuators. **/ void hal_experimental::create_actuator_position_aliases() { LOG("[create_actuator_pos]","create actuator position initializing..."); AL::ALValue jointAliases; jointAliases.arraySetSize(2); jointAliases[0] = std::string("jointActuators"); jointAliases[1].arraySetSize(NumOfPositionActuatorIds); for(int i = 0; i < NumOfPositionActuatorIds; ++i) { jointAliases[1][i] = std::string(actuatorNames[i]); } // Joints actuator list // jointAliases[1].arraySetSize(25); // jointAliases[1][HEAD_PITCH] = std::string("Device/SubDeviceList/HeadPitch/Position/Actuator/Value"); // jointAliases[1][HEAD_YAW] = std::string("Device/SubDeviceList/HeadYaw/Position/Actuator/Value"); // jointAliases[1][L_ANKLE_PITCH] = std::string("Device/SubDeviceList/LAnklePitch/Position/Actuator/Value"); // jointAliases[1][L_ANKLE_ROLL] = std::string("Device/SubDeviceList/LAnkleRoll/Position/Actuator/Value"); // jointAliases[1][L_ELBOW_ROLL] = std::string("Device/SubDeviceList/LElbowRoll/Position/Actuator/Value"); // jointAliases[1][L_ELBOW_YAW] = std::string("Device/SubDeviceList/LElbowYaw/Position/Actuator/Value"); // jointAliases[1][L_HAND] = std::string("Device/SubDeviceList/LHand/Position/Actuator/Value"); // jointAliases[1][L_HIP_PITCH] = std::string("Device/SubDeviceList/LHipPitch/Position/Actuator/Value"); // jointAliases[1][L_HIP_ROLL] = std::string("Device/SubDeviceList/LHipRoll/Position/Actuator/Value"); // jointAliases[1][L_HIP_YAW_PITCH] = std::string("Device/SubDeviceList/LHipYawPitch/Position/Actuator/Value"); // jointAliases[1][L_KNEE_PITCH] = std::string("Device/SubDeviceList/LKneePitch/Position/Actuator/Value"); // jointAliases[1][L_SHOULDER_PITCH] = std::string("Device/SubDeviceList/LShoulderPitch/Position/Actuator/Value"); // jointAliases[1][L_SHOULDER_ROLL] = std::string("Device/SubDeviceList/LShoulderRoll/Position/Actuator/Value"); // jointAliases[1][L_WRIST_YAW] = std::string("Device/SubDeviceList/LWristYaw/Position/Actuator/Value"); // jointAliases[1][R_ANKLE_PITCH] = std::string("Device/SubDeviceList/RAnklePitch/Position/Actuator/Value"); // jointAliases[1][R_ANKLE_ROLL] = std::string("Device/SubDeviceList/RAnkleRoll/Position/Actuator/Value"); // jointAliases[1][R_ELBOW_ROLL] = std::string("Device/SubDeviceList/RElbowRoll/Position/Actuator/Value"); // jointAliases[1][R_ELBOW_YAW] = std::string("Device/SubDeviceList/RElbowYaw/Position/Actuator/Value"); // jointAliases[1][R_HAND] = std::string("Device/SubDeviceList/RHand/Position/Actuator/Value"); // jointAliases[1][R_HIP_PITCH] = std::string("Device/SubDeviceList/RHipPitch/Position/Actuator/Value"); // jointAliases[1][R_HIP_ROLL] = std::string("Device/SubDeviceList/RHipRoll/Position/Actuator/Value"); // jointAliases[1][R_KNEE_PITCH] = std::string("Device/SubDeviceList/RKneePitch/Position/Actuator/Value"); // jointAliases[1][R_SHOULDER_PITCH] = std::string("Device/SubDeviceList/RShoulderPitch/Position/Actuator/Value"); // jointAliases[1][R_SHOULDER_ROLL] = std::string("Device/SubDeviceList/RShoulderRoll/Position/Actuator/Value"); // jointAliases[1][R_WRIST_YAW] = std::string("Device/SubDeviceList/RWristYaw/Position/Actuator/Value"); try { dcm_proxy->createAlias(jointAliases); log_file << "position alias added to DCM!\n"; } catch(const AL::ALError &e) { LOG("[create_actuaor_pos]","[ERROR]: Error creating actuator position aliases: " + e.toString()); } LOG("[create_actuator_pos]","create actuator position initialized"); }
Variant ALProxy::call(const std::string &method, const Variant &val1, const Variant &val2) { AL::ALValue param; AL::ALValue result; Variant resJava; param.arrayPush(val1.toALValue()); param.arrayPush(val2.toALValue()); fProxy->genericCall(method, param,result); resJava.fromALValue(result); return resJava; }
void NaoSim::ControlSteps(const int &num) { for(int i=0; i<num; i++) { step(); fastMotionCommands->GetValues(commandValues); //set angles //rearrange the command AL::ALValue dummyCommand; dummyCommand.arraySetSize(26); dummyCommand[HeadYaw] = commandValues[HEAD_YAW]; dummyCommand[HeadPitch] = commandValues[HEAD_PITCH]; dummyCommand[LShoulderPitch] = commandValues[L_SHOULDER_PITCH]; dummyCommand[LShoulderRoll] = commandValues[L_SHOULDER_ROLL]; dummyCommand[LElbowYaw] = commandValues[L_ELBOW_YAW]; dummyCommand[LElbowRoll] = commandValues[L_ELBOW_ROLL]; dummyCommand[LWristYaw] = commandValues[L_WRIST_YAW]; dummyCommand[LHand] = 0.0f; dummyCommand[LHipYawPitch] = commandValues[HIP_YAW_PITCH]; dummyCommand[LHipRoll] = commandValues[L_HIP_ROLL]; dummyCommand[LHipPitch] = commandValues[L_HIP_PITCH]; dummyCommand[LKneePitch] = commandValues[L_KNEE_PITCH]; dummyCommand[LAnklePitch] = commandValues[L_ANKLE_PITCH]; dummyCommand[LAnkleRoll] = commandValues[L_ANKLE_ROLL]; dummyCommand[RHipYawPitch] = commandValues[HIP_YAW_PITCH]; dummyCommand[RHipRoll] = commandValues[R_HIP_ROLL]; dummyCommand[RHipPitch] = commandValues[R_HIP_PITCH]; dummyCommand[RKneePitch] = commandValues[R_KNEE_PITCH]; dummyCommand[RAnklePitch] = commandValues[R_ANKLE_PITCH]; dummyCommand[RAnkleRoll] = commandValues[R_ANKLE_ROLL]; dummyCommand[RShoulderPitch] = commandValues[R_SHOULDER_PITCH]; dummyCommand[RShoulderRoll] = commandValues[R_SHOULDER_ROLL]; dummyCommand[RElbowYaw] = commandValues[R_ELBOW_YAW]; dummyCommand[RElbowRoll] = commandValues[R_ELBOW_ROLL]; dummyCommand[RWristYaw] = commandValues[R_WRIST_YAW]; dummyCommand[RHand] = 0.0f; motionProxy->setAngles("Body",dummyCommand, 1.0f); } }
/** * Creates the appropriate aliases with the DCM */ void NaoEnactor::initDCMAliases(){ AL::ALValue positionCommandsAlias; positionCommandsAlias.arraySetSize(3); positionCommandsAlias[0] = string("AllActuatorPosition"); positionCommandsAlias[1].arraySetSize(Kinematics::NUM_JOINTS); AL::ALValue hardCommandsAlias; hardCommandsAlias.arraySetSize(3); hardCommandsAlias[0] = string("AllActuatorHardness"); hardCommandsAlias[1].arraySetSize(Kinematics::NUM_JOINTS); for (unsigned int i = 0; i<Kinematics::NUM_JOINTS; i++){ positionCommandsAlias[1][i] = jointsP[i]; hardCommandsAlias[1][i] = jointsH[i]; } dcmProxy->createAlias(positionCommandsAlias); dcmProxy->createAlias(hardCommandsAlias); }
void KmeAction::DcmInit() { AL::ALValue jointAliasses; vector<std::string> PosActuatorStrings = KDeviceLists::getPositionActuatorKeys(); jointAliasses.arraySetSize(2); jointAliasses[0] = std::string("BodyWithoutHead"); // Alias for Body joint actuators without Head joins jointAliasses[1].arraySetSize(20); // Joints actuator list for (int i = KDeviceLists::L_ARM; i < KDeviceLists::NUMOFJOINTS; i++) { jointAliasses[1][i - KDeviceLists::L_ARM] = PosActuatorStrings[i]; } // Create alias try { dcm->createAlias(jointAliasses); } catch (const AL::ALError &e) { throw ALERROR("KmeAction", "createKmeActuatorAlias()", "Error when creating Alias : " + e.toString()); } // Create Commands commands.arraySetSize(6); commands[0] = string("BodyWithoutHead"); commands[1] = string("ClearAll"); // Erase all previous commands commands[2] = string("time-separate"); commands[3] = 0; commands[5].arraySetSize(20); // For all joints except head for (unsigned int i = 0; i < commands[5].getSize(); i++) { commands[5][i].arraySetSize(actionTimes[0].getSize()); //num of poses for (unsigned int j = 0; j < commands[5][i].getSize(); j++) { commands[5][i][j] = actionAngles[i + 2][j]; // actionAngles[joints][poses], commands[5][joints][poses] // Logger::Instance().WriteMsg("KmeACTION", "commands " + _toString(commands[5][i][j]) , Logger::ExtraInfo); } } }
/** * \brief: Set the stiffness value for every joint on the robot to a single value **/ void hal_experimental::set_all_actuator_stiffnesses(const float &stiffnessVal) { log_file << "[set_all_stiffness]: << Setting All stiffness values to: " << stiffnessVal << std::endl; AL::ALValue stiffnessCommands; int DCMTime; try { DCMTime = dcm_proxy->getTime(1000); //Time NAOQi has been running + 1000ms } catch(const AL::ALError &e) { LOG("[set_all_stiffness]","[ERROR] An error occured while setting stiffness value: " + e.toString()); } stiffnessCommands.arraySetSize(3); stiffnessCommands[0] = std::string("jointStiffness"); stiffnessCommands[1] = std::string("Merge"); stiffnessCommands[2].arraySetSize(1); stiffnessCommands[2][0].arraySetSize(2); stiffnessCommands[2][0][0] = stiffnessVal; stiffnessCommands[2][0][1] = DCMTime; log_file << "[set_all_stiffness] Alias set up..trying to set it now." << std::endl; std::cout << "[set_all_stiffness] Alias set up..trying to set it now." << std::endl; try { dcm_proxy->set(stiffnessCommands); LOG("[set_all_stiffness]","Stiffness Values Set!"); } catch(const AL::ALError &e) { LOG("[set_all_stiffness]","[ERROR] An error occured while setting stiffness value: " + e.toString()); } log_file << "[set_all_stiffness]: Done Setting all stiffness values to: " << stiffnessVal << std::endl; }
void NaoLights::sendLightCommand(AL::ALValue & command){ #ifdef DEBUG_NAOLIGHTS_COMMAND std::cout << " NaoLights::sendCommand() " <<command.serializeToText()<< std::endl; #endif try{ command[4][0] = dcmProxy->getTime(0); #ifdef LEDS_ENABLED dcmProxy->setAlias(command); #endif } catch(AL::ALError& e) { std::cout << "dcm value set error in sendLightCommand:" << e.toString() << std::endl; } }
void Variant::fromALValue(const AL::ALValue &val) { if (val.getType() == AL::ALValue::TypeInvalid) { // nothing to do } else if (val.getType() == AL::ALValue::TypeArray) { fType = VARRAY; fValue = val; } else if (val.getType()==AL::ALValue::TypeInt) { fType = VINT; fValue = val; } else if (val.getType()== AL::ALValue::TypeString) { fType == VSTRING; fValue = val; } else if (val.getType()== AL::ALValue::TypeBinary) { fType == VCHARARRAY; fValue = val; } else if (val.getType()== AL::ALValue::TypeBool) { fType == VBOOL; fValue = val; } else if (val.getType()== AL::ALValue::TypeFloat) { fType == VFLOAT; fValue = val; } else { printf("unknow type %d\n",val.getType()); } }
/** This method try to match a localized sound to an identified face, and update * accordingly the corresponding 'human'. * * If none is found, a 'virtual' human called 'unknown_speaker' is created, with an * approximate position. */ void InteractionMonitor::identifySpeaker(const AL::ALValue &sounds, map<string, Human>& humans) { // according to the doc // http://www.aldebaran-robotics.com/documentation/naoqi/audio/alaudiosourcelocalization.html // only one sound may be localized at a given step. if (sounds.getSize() == 0) return; map<string, Human>::iterator it; Human h = makeHuman("unknown_speaker", sounds[0][1][0], sounds[0][1][1]); for (it=humans.begin(); it!=humans.end(); ++it) { if (distance(h, it->second) < MAX_DISTANCE_HUMAN_SOUND) { it->second.speaking = true; return; } } humans[h.id] = h; }
/** * 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(); } }
void NaoqiJointStates::run() { ros::Rate r(m_rate); ros::Time stamp1; ros::Time stamp2; ros::Time stamp; std::vector<float> odomData; float odomX, odomY, odomZ, odomWX, odomWY, odomWZ; std::vector<float> memData; std::vector<float> positionData; ROS_INFO("Staring main loop. ros::ok() is %d nh.ok() is %d",ros::ok(),m_nh.ok()); while(ros::ok() ) { r.sleep(); ros::spinOnce(); stamp1 = ros::Time::now(); try { memData = m_memoryProxy->getListData(m_dataNamesList); // {SPACE_TORSO = 0, SPACE_WORLD = 1, SPACE_NAO = 2}. (second argument) odomData = m_motionProxy->getPosition("Torso", 1, true); positionData = m_motionProxy->getAngles("Body", true); } catch (const AL::ALError & e) { ROS_ERROR( "Error accessing ALMemory, exiting..."); ROS_ERROR( "%s", e.what() ); //ros.signal_shutdown("No NaoQI available anymore"); } stamp2 = ros::Time::now(); //ROS_DEBUG("dt is %f",(stamp2-stamp1).toSec()); % dt is typically around 1/1000 sec // TODO: Something smarter than this.. stamp = stamp1 + ros::Duration((stamp2-stamp1).toSec()/2.0); /****************************************************************** * IMU *****************************************************************/ if (memData.size() != m_dataNamesList.getSize()) { ROS_ERROR("memData length %zu does not match expected length %u",memData.size(),m_dataNamesList.getSize() ); continue; } // IMU data: m_torsoIMU.header.stamp = stamp; float angleX = memData[1]; float angleY = memData[2]; float angleZ = memData[3]; float gyroX = memData[4]; float gyroY = memData[5]; float gyroZ = memData[6]; float accX = memData[7]; float accY = memData[8]; float accZ = memData[9]; m_torsoIMU.orientation = tf::createQuaternionMsgFromRollPitchYaw( angleX, angleY, angleZ); // yaw currently always 0 m_torsoIMU.angular_velocity.x = gyroX; m_torsoIMU.angular_velocity.y = gyroY; m_torsoIMU.angular_velocity.z = gyroZ; // currently always 0 m_torsoIMU.linear_acceleration.x = accX; m_torsoIMU.linear_acceleration.y = accY; m_torsoIMU.linear_acceleration.z = accZ; // covariances unknown // cf http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html m_torsoIMU.orientation_covariance[0] = -1; m_torsoIMU.angular_velocity_covariance[0] = -1; m_torsoIMU.linear_acceleration_covariance[0] = -1; m_torsoIMUPub.publish(m_torsoIMU); /****************************************************************** * Joint state *****************************************************************/ m_jointState.header.stamp = stamp; m_jointState.header.frame_id = m_baseFrameId; m_jointState.position.resize(positionData.size()); for(unsigned i = 0; i<positionData.size(); ++i) { m_jointState.position[i] = positionData[i]; } // simulated model misses some joints, we need to fill: if (m_jointState.position.size() == 22) { m_jointState.position.insert(m_jointState.position.begin()+6,0.0); m_jointState.position.insert(m_jointState.position.begin()+7,0.0); m_jointState.position.push_back(0.0); m_jointState.position.push_back(0.0); } m_jointStatePub.publish(m_jointState); /****************************************************************** * Odometry *****************************************************************/ if (!m_paused) { // apply offset transformation: tf::Pose transformedPose; if (odomData.size()!=6) { ROS_ERROR( "Error getting odom data. length is %zu",odomData.size() ); continue; } double dt = (stamp.toSec() - m_lastOdomTime); odomX = odomData[0]; odomY = odomData[1]; odomZ = odomData[2]; odomWX = odomData[3]; odomWY = odomData[4]; odomWZ = odomData[5]; tf::Quaternion q; // roll and pitch from IMU, yaw from odometry: if (m_useIMUAngles) q.setRPY(angleX, angleY, odomWZ); else q.setRPY(odomWX, odomWY, odomWZ); m_odomPose.setOrigin(tf::Vector3(odomX, odomY, odomZ)); m_odomPose.setRotation(q); if(m_mustUpdateOffset) { if(!m_isInitialized) { if(m_initializeFromIMU) { // Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU m_targetPose.setOrigin(m_odomPose.getOrigin()); m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, odomWZ)); } else if(m_initializeFromOdometry) { m_targetPose.setOrigin(m_odomPose.getOrigin()); m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, odomWZ)); } m_isInitialized = true; } else { // Overwrite target pitch and roll angles with IMU data const double target_yaw = tf::getYaw(m_targetPose.getRotation()); if(m_initializeFromIMU) { m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, target_yaw)); } else if(m_initializeFromOdometry){ m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, target_yaw)); } } m_odomOffset = m_targetPose * m_odomPose.inverse(); transformedPose = m_targetPose; m_mustUpdateOffset = false; } else { transformedPose = m_odomOffset * m_odomPose; } // // publish the transform over tf first // m_odomTransformMsg.header.stamp = stamp; tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform); m_transformBroadcaster.sendTransform(m_odomTransformMsg); // // Fill the Odometry msg // m_odom.header.stamp = stamp; //set the velocity first (old values still valid) m_odom.twist.twist.linear.x = (odomX - m_odom.pose.pose.position.x) / dt; m_odom.twist.twist.linear.y = (odomY - m_odom.pose.pose.position.y) / dt; m_odom.twist.twist.linear.z = (odomZ - m_odom.pose.pose.position.z) / dt; // TODO: calc angular velocity! // m_odom.twist.twist.angular.z = vth; ?? //set the position from the above calculated transform m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation; m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x; m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y; m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z; m_odomPub.publish(m_odom); m_lastOdomTime = stamp.toSec(); } } ROS_INFO("naoqi_sensors stopped."); }
void WifiManager::UpdateList() { // reload config ParamEntry::Reload(); std::string serviceOff; std::string serviceOn; #if !WIFI_LOCAL_TEST GetConnectionProxy().scan(); AL::ALValue serviceList = GetConnectionProxy().services(); // Log() << serviceList << std::endl; #else // !LOCAL_TEST AL::ALValue serviceList; #endif // !LOCAL_TEST _services.clear(); for (int i = 0; i < serviceList.getSize(); i++) { // Log() << "item[" << i << "]: " << serviceList[i] << std::endl; std::map<std::string, std::string> details; for (int j = 0; j < serviceList[i].getSize(); j++) { //Log() << " -- " << serviceList[i][j] << std::endl; std::string name = serviceList[i][j][0]; std::string val; if (serviceList[i][j][1].isString()) val = (std::string) serviceList[i][j][1]; else val = serviceList[i][j][1].toString(); details[name] = val; } /*for (auto& item : details) { Log() << item.first << ": " << item.second << std::endl; }*/ WifiService service; service._id = details["ServiceId"]; service._name = details["Name"]; service._state = WifiStateUtils::StringToWifiState(details["State"]); service.FindConfig(); _services.push_back(service); if (service._knownConfig && service._knownConfig->_default && _selectedSSID.empty()) _selectedSSID = service._name; } #if WIFI_LOCAL_TEST WifiService service; service._id = "whatever"; service._name = "YETTI"; service._state = WifiStateUtils::StringToWifiState("Idle"); service.FindConfig(); _services.push_back(service); if (service._knownConfig && service._knownConfig->_default && _selectedSSID.empty()) _selectedSSID = service._name; #endif // WIFI_LOCAL_TEST }
int main(int argc, const char* argv[]) { std::string opt_ip = "131.254.10.126"; bool opt_language_english = true; bool opt_debug = false; for (unsigned int i=0; i<argc; i++) { if (std::string(argv[i]) == "--ip") opt_ip = argv[i+1]; else if (std::string(argv[i]) == "--fr") opt_language_english = false; else if (std::string(argv[i]) == "--debug") opt_debug = true; else if (std::string(argv[i]) == "--help") { std::cout << "Usage: " << argv[0] << "[--ip <robot address>] [--fr]" << std::endl; return 0; } } std::string camera_name = "CameraTopPepper"; // Open the grabber for the acquisition of the images from the robot vpNaoqiGrabber g; if (! opt_ip.empty()) g.setRobotIp(opt_ip); g.setFramerate(30); g.setCamera(0); g.open(); vpCameraParameters cam = vpNaoqiGrabber::getIntrinsicCameraParameters(AL::kQVGA,camera_name, vpCameraParameters::perspectiveProjWithDistortion); vpHomogeneousMatrix eMc = vpNaoqiGrabber::getExtrinsicCameraParameters(camera_name,vpCameraParameters::perspectiveProjWithDistortion); std::cout << "eMc:" << std::endl << eMc << std::endl; std::cout << "cam:" << std::endl << cam << std::endl; vpNaoqiRobot robot; // Connect to the robot if (! opt_ip.empty()) robot.setRobotIp(opt_ip); robot.open(); if (robot.getRobotType() != vpNaoqiRobot::Pepper) { std::cout << "ERROR: You are not connected to Pepper, but to a different Robot. Check the IP. " << std::endl; return 0; } std::vector<std::string> jointNames_head = robot.getBodyNames("Head"); // Open Proxy for the speech AL::ALTextToSpeechProxy tts(opt_ip, 9559); std::string phraseToSay; if (opt_language_english) { tts.setLanguage("English"); phraseToSay = " \\emph=2\\ Hi,\\pau=200\\ How are you ?"; } else { tts.setLanguage("French"); phraseToSay = " \\emph=2\\ Bonjour,\\pau=200\\ comment vas tu ?"; } // Inizialize PeoplePerception AL::ALPeoplePerceptionProxy people_proxy(opt_ip, 9559); AL::ALMemoryProxy m_memProxy(opt_ip, 9559); people_proxy.subscribe("People", 30, 0.0); std::cout << "period: " << people_proxy.getCurrentPeriod() << std::endl; // Open Proxy for the recognition speech AL::ALSpeechRecognitionProxy asr(opt_ip, 9559); // asr.unsubscribe("Test_ASR"); // return 0 ; asr.setVisualExpression(false); asr.setLanguage("English"); std::vector<std::string> vocabulary; vocabulary.push_back("follow me"); vocabulary.push_back("stop"); // Set the vocabulary asr.setVocabulary(vocabulary,false); // Start the speech recognition engine with user Test_ASR asr.subscribe("Test_ASR"); std::cout << "Speech recognition engine started" << std::endl; // Proxy to control the leds AL::ALLedsProxy led_proxy(opt_ip, 9559); //Declare plots vpPlot * plotter_diff_vel; vpPlot * plotter_vel; vpPlot * plotter_error; vpPlot * plotter_distance; if (opt_debug) { // Plotting plotter_diff_vel = new vpPlot (2); plotter_diff_vel->initGraph(0, 2); plotter_diff_vel->initGraph(1, 2); plotter_diff_vel->setTitle(0, "HeadYaw"); plotter_diff_vel->setTitle(1, "HeadPitch"); plotter_vel= new vpPlot (1); plotter_vel->initGraph(0, 5); plotter_vel->setLegend(0, 0, "vx"); plotter_vel->setLegend(0, 1, "vy"); plotter_vel->setLegend(0, 2, "wz"); plotter_vel->setLegend(0, 3, "q_yaw"); plotter_vel->setLegend(0, 4, "q_pitch"); plotter_error = new vpPlot(1); plotter_error->initGraph(0, 3); plotter_error->setLegend(0, 0, "x"); plotter_error->setLegend(0, 1, "y"); plotter_error->setLegend(0, 2, "Z"); plotter_distance = new vpPlot (1); plotter_distance->initGraph(0, 1); plotter_distance->setLegend(0, 0, "dist"); } try { vpImage<unsigned char> I(g.getHeight(), g.getWidth()); vpDisplayX d(I); vpDisplay::setTitle(I, "ViSP viewer"); vpFaceTrackerOkao face_tracker(opt_ip,9559); double dist = 0.0; // Distance between person detected from peoplePerception and faceDetection // Set Visual Servoing: vpServo task; task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ; task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE); // vpAdaptiveGain lambda_adapt; // lambda_adapt.initStandard(1.6, 1.8, 15); vpAdaptiveGain lambda_base(1.2, 1.0, 10); // 2.3, 0.7, 15 vpAdaptiveGain lambda_nobase(5, 2.9, 15); // 4, 0.5, 15 task.setLambda(lambda_base) ; double Z = 0.9; double Zd = 0.9; bool stop_vxy = false; bool move_base = true; bool move_base_prev = true; // Create the desired visual feature vpFeaturePoint s; vpFeaturePoint sd; vpImagePoint ip(I.getHeight()/2, I.getWidth()/2); // Create the current x visual feature vpFeatureBuilder::create(s, cam, ip); vpFeatureBuilder::create(sd, cam, ip); // sd.buildFrom( I.getWidth()/2, I.getHeight()/2, Zd); AL::ALValue limit_yaw = robot.getProxy()->getLimits("HeadYaw"); std::cout << limit_yaw[0][0] << " " << limit_yaw[0][1] << std::endl; // Add the feature task.addFeature(s, sd) ; vpFeatureDepth s_Z, s_Zd; s_Z.buildFrom(s.get_x(), s.get_y(), Z , 0); // log(Z/Z*) = 0 that's why the last parameter is 0 s_Zd.buildFrom(sd.get_x(), sd.get_y(), Zd , 0); // log(Z/Z*) = 0 that's why the last parameter is 0 // Add the feature task.addFeature(s_Z, s_Zd); // Jacobian 6x5 (vx,vy,wz,q_yaq,q_pitch) vpMatrix tJe(6,5); tJe[0][0]= 1; tJe[1][1]= 1; tJe[5][2]= 1; vpMatrix eJe(6,5); double servo_time_init = 0; vpImagePoint head_cog_cur; vpImagePoint head_cog_des(I.getHeight()/2, I.getWidth()/2); vpColVector q_dot; bool reinit_servo = true; unsigned long loop_iter = 0; std::vector<std::string> recognized_names; std::map<std::string,unsigned int> detected_face_map; bool detection_phase = true; unsigned int f_count = 0; // AL::ALValue leg_names = AL::ALValue::array("HipRoll","HipPitch", "KneePitch" ); // AL::ALValue values = AL::ALValue::array(0.0, 0.0, 0.0 ); // robot.getProxy()->setMoveArmsEnabled(false,false); std::vector<std::string> arm_names = robot.getBodyNames("LArm"); std::vector<std::string> rarm_names = robot.getBodyNames("RArm"); std::vector<std::string> leg_names(3); leg_names[0]= "HipRoll"; leg_names[1]= "HipPitch"; leg_names[2]= "KneePitch"; arm_names.insert( arm_names.end(), rarm_names.begin(), rarm_names.end() ); arm_names.insert( arm_names.end(), leg_names.begin(), leg_names.end() ); std::vector<float> arm_values(arm_names.size(),0.0); for (unsigned int i = 0; i < arm_names.size(); i++ ) std::cout << arm_names[i]<< std::endl; robot.getPosition(arm_names, arm_values,false); vpImagePoint best_cog_face_peoplep; bool person_found = false; double t_prev = vpTime::measureTimeSecond(); while(1) { if (reinit_servo) { servo_time_init = vpTime::measureTimeSecond(); t_prev = vpTime::measureTimeSecond(); reinit_servo = false; led_proxy.fadeRGB("FaceLeds","white",0.1); } double t = vpTime::measureTimeMs(); if (0) // (opt_debug) { g.acquire(I); } vpDisplay::display(I); // Detect face bool face_found = face_tracker.detect(); stop_vxy = false; //std::cout << "Loop time face_tracker: " << vpTime::measureTimeMs() - t << " ms" << std::endl; // Check speech recognition result AL::ALValue result_speech = m_memProxy.getData("WordRecognized"); if ( ((result_speech[0]) == vocabulary[0]) && (double (result_speech[1]) > 0.4 )) //move { //std::cout << "Recognized: " << result_speech[0] << "with confidence of " << result_speech[1] << std::endl; task.setLambda(lambda_base) ; move_base = true; } else if ( (result_speech[0] == vocabulary[1]) && (double(result_speech[1]) > 0.4 )) //stop { //std::cout << "Recognized: " << result_speech[0] << "with confidence of " << result_speech[1] << std::endl; task.setLambda(lambda_nobase) ; move_base = false; } if (move_base != move_base_prev) { if (move_base) { phraseToSay = "Ok, I will follow you."; tts.post.say(phraseToSay); } else { phraseToSay = "Ok, I will stop."; tts.post.say(phraseToSay); } } //robot.setStiffness(arm_names,0.0); //robot.getProxy()->setAngles(arm_names,arm_values,1.0); //robot.getProxy()->setAngles("HipRoll",0.0,1.0); //std::cout << "Loop time check_speech: " << vpTime::measureTimeMs() - t << " ms" << std::endl; move_base_prev = move_base; if (face_found) { std::ostringstream text; text << "Found " << face_tracker.getNbObjects() << " face(s)"; vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red); for(size_t i=0; i < face_tracker.getNbObjects(); i++) { vpRect bbox = face_tracker.getBBox(i); if (i == 0) vpDisplay::displayRectangle(I, bbox, vpColor::red, false, 2); else vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 1); vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), face_tracker.getMessage(i) , vpColor::red); } led_proxy.post.fadeRGB("FaceLeds","blue",0.1); double u = face_tracker.getCog(0).get_u(); double v = face_tracker.getCog(0).get_v(); if (u<= g.getWidth() && v <= g.getHeight()) head_cog_cur.set_uv(u,v); vpRect bbox = face_tracker.getBBox(0); std::string name = face_tracker.getMessage(0); //std::cout << "Loop time face print " << vpTime::measureTimeMs() - t << " ms" << std::endl; } AL::ALValue result = m_memProxy.getData("PeoplePerception/VisiblePeopleList"); //std::cout << "Loop time get Data PeoplePerception " << vpTime::measureTimeMs() - t << " ms" << std::endl; person_found = false; if (result.getSize() > 0) { AL::ALValue info = m_memProxy.getData("PeoplePerception/PeopleDetected"); int num_people = info[1].getSize(); std::ostringstream text; text << "Found " << num_people << " person(s)"; vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red); person_found = true; if (face_found) // Try to find the match between two detection { vpImagePoint cog_face; double dist_min = 1000; unsigned int index_person = 0; for (unsigned int i = 0; i < num_people; i++) { float alpha = info[1][i][2]; float beta = info[1][i][3]; //Z = Zd; // Centre of face into the image float x = g.getWidth()/2 - g.getWidth() * beta; float y = g.getHeight()/2 + g.getHeight() * alpha; cog_face.set_uv(x,y); dist = vpImagePoint::distance(cog_face, head_cog_cur); if (dist < dist_min) { dist_min = dist; best_cog_face_peoplep = cog_face; index_person = i; } } vpDisplay::displayCross(I, best_cog_face_peoplep, 10, vpColor::white); if (dist_min < 55.) { Z = info[1][index_person][1]; // Current distance } } else // Take the first one on the list { float alpha = info[1][0][2]; float beta = info[1][0][3]; //Z = Zd; // Centre of face into the image float x = g.getWidth()/2 - g.getWidth() * beta; float y = g.getHeight()/2 + g.getHeight() * alpha; head_cog_cur.set_uv(x,y); Z = info[1][0][1]; // Current distance } } else { std::cout << "No distance computed " << std::endl; stop_vxy = true; robot.getProxy()->setAngles("HipRoll",0.0,1.0); //Z = Zd; } // float alpha = info[1][0][2]; // float beta = info[1][0][3]; // //Z = Zd; // // Centre of face into the image // float x = g.getWidth()/2 - g.getWidth() * beta; // float y = g.getHeight()/2 + g.getHeight() * alpha; // vpImagePoint cog_face(y,x); // dist = vpImagePoint::distance(cog_face,head_cog_cur); // if (dist < 55.) // Z = info[1][0][1]; // Current distance // else // stop_vxy = true; // } // else // { // std::cout << "No distance computed " << std::endl; // stop_vxy = true; // //Z = Zd; // } //std::cout << "Loop time before VS: " << vpTime::measureTimeMs() - t << " ms" << std::endl; if (face_found || person_found ) { // Get Head Jacobian (6x2) vpMatrix torso_eJe_head; robot.get_eJe("Head",torso_eJe_head); // Add column relative to the base rotation (Wz) // vpColVector col_wz(6); // col_wz[5] = 1; for (unsigned int i = 0; i < 6; i++) for (unsigned int j = 0; j < torso_eJe_head.getCols(); j++) tJe[i][j+3] = torso_eJe_head[i][j]; // std::cout << "tJe" << std::endl << tJe << std::endl; // vpHomogeneousMatrix torsoMHeadPith( robot.getProxy()->getTransform(jointNames_head[jointNames_head.size()-1], 0, true));// get transformation matrix between torso and HeadRoll vpHomogeneousMatrix torsoMHeadPith( robot.getProxy()->getTransform("HeadPitch", 0, true));// get transformation matrix between torso and HeadRoll vpVelocityTwistMatrix HeadPitchVLtorso(torsoMHeadPith.inverse()); for(unsigned int i=0; i< 3; i++) for(unsigned int j=0; j< 3; j++) HeadPitchVLtorso[i][j+3] = 0; //std::cout << "HeadPitchVLtorso: " << std::endl << HeadPitchVLtorso << std::endl; // Transform the matrix eJe = HeadPitchVLtorso *tJe; // std::cout << "eJe" << std::endl << eJe << std::endl; task.set_eJe( eJe ); task.set_cVe( vpVelocityTwistMatrix(eMc.inverse()) ); vpDisplay::displayCross(I, head_cog_des, 10, vpColor::blue); vpDisplay::displayCross(I, head_cog_cur, 10, vpColor::green); // std::cout << "head_cog_des:" << std::endl << head_cog_des << std::endl; // std::cout << "head_cog_cur:" << std::endl << head_cog_cur << std::endl; // Update the current x feature double x,y; vpPixelMeterConversion::convertPoint(cam, head_cog_cur, x, y); s.buildFrom(x, y, Z); //s.set_xyZ(head_cog_cur.get_u(), head_cog_cur.get_v(), Z); // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix s_Z.buildFrom(s.get_x(), s.get_y(), Z, log(Z/Zd)) ; q_dot = task.computeControlLaw(vpTime::measureTimeSecond() - servo_time_init); //std::cout << "Loop time compute VS: " << vpTime::measureTimeMs() - t << " ms" << std::endl; vpMatrix P = task.getI_WpW(); double alpha = -3.3; double min = limit_yaw[0][0]; double max = limit_yaw[0][1]; vpColVector z_q2 (q_dot.size()); vpColVector q_yaw = robot.getPosition(jointNames_head[0]); z_q2[3] = 2 * alpha * q_yaw[0]/ pow((max - min),2); vpColVector q3 = P * z_q2; //if (q3.euclideanNorm()<10.0) q_dot = q_dot + q3; std::vector<float> vel(jointNames_head.size()); vel[0] = q_dot[3]; vel[1] = q_dot[4]; // Compute the distance in pixel between the target and the center of the image double distance = vpImagePoint::distance(head_cog_cur, head_cog_des); //if (distance > 0.03*I.getWidth()) std::cout << "q:" << std::endl << q_dot << std::endl; // std::cout << "vel" << std::endl << q_dot << std::endl; //std::cout << "distance" << std::endl << distance <<" -- " << 0.03*I.getWidth() << " ++ " << I.getWidth() << std::endl; // if (distance > 0.1*I.getWidth()) robot.setVelocity(jointNames_head,vel); // else // { // std::cout << "Setting hipRoll to zero" << std::endl; // robot.getProxy()->setAngles("HipRoll",0.0,1.0); // } // std::cout << "errorZ: " << task.getError()[2] << std::endl; // std::cout << "stop_vxy: " << stop_vxy << std::endl; if (std::fabs(Z -Zd) < 0.05 || stop_vxy || !move_base) robot.setBaseVelocity(0.0, 0.0, q_dot[2]); else robot.setBaseVelocity(q_dot[0], q_dot[1], q_dot[2]); if (opt_debug) { vpColVector vel_head = robot.getJointVelocity(jointNames_head); for (unsigned int i=0 ; i < jointNames_head.size() ; i++) { plotter_diff_vel->plot(i, 1, loop_iter, q_dot[i+3]); plotter_diff_vel->plot(i, 0, loop_iter, vel_head[i]); } plotter_error->plot(0,loop_iter,task.getError()); plotter_vel->plot(0,loop_iter, q3); plotter_distance->plot(0,0,loop_iter,Z); } // if (detection_phase) // { // //if (score >= 0.4 && distance < 0.06*I.getWidth() && bbox.getSize() > 3000) // if (distance < 0.06*I.getWidth() && bbox.getSize() > 3000) // { // if (opt_debug) // { // vpDisplay::displayRectangle(I, bbox, vpColor::red, false, 1); // vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), name, vpColor::red); // } // detected_face_map[name]++; // f_count++; // } // else // { // if (opt_debug) // { // vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 1); // vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), name, vpColor::green); // } // } // if (f_count>10) // { // detection_phase = false; // f_count = 0; // } // } // else // { // std::string recognized_person_name = std::max_element(detected_face_map.begin(), detected_face_map.end(), pred)->first; // unsigned int times = std::max_element(detected_face_map.begin(), detected_face_map.end(), pred)->second; // if (!in_array(recognized_person_name, recognized_names) && recognized_person_name != "Unknown") { // if (opt_language_english) // { // phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\" + recognized_person_name + "\\pau=200\\ How are you ?"; // } // else // { // phraseToSay = "\\emph=2\\ Salut \\wait=200\\ \\emph=2\\" + recognized_person_name + "\\pau=200\\ comment vas tu ?";; // } // std::cout << phraseToSay << std::endl; // tts.post.say(phraseToSay); // recognized_names.push_back(recognized_person_name); // } // if (!in_array(recognized_person_name, recognized_names) && recognized_person_name == "Unknown" // && times > 15) // { // if (opt_language_english) // { // phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\. I don't know you! \\emph=2\\ What's your name?"; // } // else // { // phraseToSay = " \\emph=2\\ Salut \\wait=200\\ \\emph=2\\. Je ne te connais pas! \\emph=2\\ Comment t'appelles-tu ?"; // } // std::cout << phraseToSay << std::endl; // tts.post.say(phraseToSay); // recognized_names.push_back(recognized_person_name); // } // detection_phase = true; // detected_face_map.clear(); // } } else { robot.stop(jointNames_head); robot.stopBase(); std::cout << "Stop!" << std::endl; reinit_servo = true; } //if (opt_debug) vpDisplay::flush(I); if (vpDisplay::getClick(I, false)) break; loop_iter ++; std::cout << "Loop time: " << vpTime::measureTimeMs() - t << " ms" << std::endl; } robot.stop(jointNames_head); robot.stopBase(); tts.setLanguage("French"); // tts.exit(); people_proxy.unsubscribe("People"); // people_proxy.exit(); asr.unsubscribe("Test_ASR"); asr.setVisualExpression(true); // asr.exit(); tts.setLanguage("French"); // tts.exit(); led_proxy.fadeRGB("FaceLeds","white",0.1); // led_proxy.exit(); vpDisplay::getClick(I, true); } catch(vpException &e) { std::cout << e.getMessage() << std::endl; } catch (const AL::ALError& e) { std::cerr << "Caught exception " << e.what() << std::endl; } std::cout << "The end: stop the robot..." << std::endl; //tts.setLanguage("French"); // tts.exit(); robot.stop(jointNames_head); robot.stopBase(); return 0; }
/** * 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 }
/** * The method reads all sensors. It also detects if the chest button was pressed * for at least three seconds. In that case, it shuts down the robot. */ void readSensors() { // get new sensor values and copy them to the shared memory block try { // copy sensor values into the shared memory block int writingSensors = 0; if(writingSensors == data->newestSensors) ++writingSensors; if(writingSensors == data->readingSensors) if(++writingSensors == data->newestSensors) ++writingSensors; assert(writingSensors != data->newestSensors); assert(writingSensors != data->readingSensors); float* sensors = data->sensors[writingSensors]; for(int i = 0; i < lbhNumOfSensorIds; ++i) sensors[i] = *sensorPtrs[i]; AL::ALValue value = memory->getData("GameCtrl/RoboCupGameControlData"); if(value.isBinary() && value.getSize() == sizeof(RoboCup::RoboCupGameControlData)) memcpy(&data->gameControlData[writingSensors], value, sizeof(RoboCup::RoboCupGameControlData)); data->newestSensors = writingSensors; // detect shutdown request via chest-button if(*sensorPtrs[chestButtonSensor] == 0.f) startPressedTime = dcmTime; else if(state != shuttingDown && startPressedTime && dcmTime - startPressedTime > 3000) { if(*sensorPtrs[rBumperRightSensor] != 0.f || *sensorPtrs[rBumperLeftSensor] != 0.f || *sensorPtrs[lBumperRightSensor] != 0.f || *sensorPtrs[lBumperLeftSensor] != 0.f) (void) !system("( /home/nao/bin/bhumand stop && sudo shutdown -r now ) &"); else (void) !system("( /home/nao/bin/bhumand stop && sudo shutdown -h now ) &"); state = preShuttingDown; } } catch(AL::ALError& e) { fprintf(stderr, "libbhuman: %s\n", e.toString().c_str()); } // raise the semaphore if(sem != SEM_FAILED) { int sval; if(sem_getvalue(sem, &sval) == 0) { if(sval < 1) { sem_post(sem); frameDrops = 0; } else { if(frameDrops == 0) fprintf(stderr, "libbhuman: dropped sensor data.\n"); ++frameDrops; } } } }
void InteractionMonitor::processFaces(const AL::ALValue &faces, map<string, Human>& humans) { try { /** Check that there are faces effectively detected. */ if (faces.getSize() < 2 ) { if (facesCount != 0) { ROS_INFO("No face detected"); humanBuffer = NB_VIEWS_BEFORE_LEARNING; facesCount = 0; } return; } /** Check the number of faces from the FaceInfo field, and check that it has * changed from the last event.*/ if (faces[1].getSize() - 1 != facesCount) { ROS_INFO("%d face(s) detected.", faces[1].getSize() - 1); /** Update the current number of detected faces. */ facesCount = faces[1].getSize() - 1; } } catch (const AL::ALError& e) { ROS_ERROR(e.what()); } for (int i = 0; i < facesCount; ++i) { int naoqiFaceId = faces[1][i][1][0]; float confidence = faces[1][i][1][1]; // Face > [FaceInfo] > FaceInfo > ShapeInfo > yaw float yaw = faces[1][i][0][1]; // Face > [FaceInfo] > FaceInfo > ShapeInfo > pitch float pitch = faces[1][i][0][2]; ROS_INFO("Face (id: %d) confidence:%.2f yaw: %.2f, pitch: %.2f", naoqiFaceId, confidence, yaw, pitch); //ROS_INFO("Face (id: %d) reco confidence: %.2f", naoqiFaceId, confidence); //ROS_INFO("Time_Filtered_Reco_Info: %s", faces[1][i].toString().c_str()); int candidateId = facesCloseTo(yaw, pitch); if (naoqiFaceId == -1 && candidateId == -1) // seen unknown new face { if (humanBuffer > 0) { humanBuffer--; } else { humanBuffer = NB_VIEWS_BEFORE_LEARNING; char faceId[6]; gen_random(faceId, 5); ROS_INFO("Unknown face (confidence: %.2f). Learning it as <%s>", confidence, faceId); if(!m_faceProxy->learnFace(faceId)) { ROS_ERROR("Could not learn the new face!"); } } } else { // face recognized! humanBuffer = NB_VIEWS_BEFORE_LEARNING; // too close to an existing face? re-use the existing face! if (candidateId != -1) { naoqiFaceId = candidateId; } else { assert(naoqiFaceId != -1); string label = faces[1][i][1][2]; id2label[naoqiFaceId] = label; } string label = id2label[naoqiFaceId]; // save the yaw,pitch pose of the recognized face, to // prevent jitter at next round of detections m_lastSeenHumans[naoqiFaceId] = make_pair(yaw, pitch); humans[label] = makeHuman(label, yaw, pitch); } } }
/*! Connect to Romeo robot, grab, display images using ViSP and start face detection with Okao library running on the robot. By default, this example connect to a robot with ip address: 198.18.0.1. */ int main(int argc, const char* argv[]) { try { std::string opt_ip = "198.18.0.1";; if (argc == 3) { if (std::string(argv[1]) == "--ip") opt_ip = argv[2]; } vpNaoqiGrabber g; if (! opt_ip.empty()) g.setRobotIp(opt_ip); g.setCamera(0); g.open(); vpImage<unsigned char> I(g.getHeight(), g.getWidth()); vpDisplayX d(I); vpDisplay::setTitle(I, "ViSP viewer"); // Open Proxy for the speech AL::ALTextToSpeechProxy tts(opt_ip, 9559); tts.setLanguage("English"); std::string phraseToSay = "Hi!"; // int id = tts.post.say(phraseToSay); // tts.wait(id,2000); // Open Face detection proxy AL::ALFaceDetectionProxy df(opt_ip, 9559); // Start the face recognition engine const int period = 1; df.subscribe("Face", period, 0.0); AL::ALMemoryProxy memProxy(opt_ip, 9559); float mImageHeight = g.getHeight(); float mImageWidth = g.getWidth(); std::vector<std::string> names; while (1) { g.acquire(I); vpDisplay::display(I); AL::ALValue result = memProxy.getData("FaceDetected"); if (result.getSize() >=2) { //std::cout << "face" << std::endl; AL::ALValue info_face_array = result[1]; for (unsigned int i = 0; i < info_face_array.getSize()-1; i++ ) { // AL::ALValue info_face = info_face_array[i]; //Extract face info //AL::ALValue shape_face =info_face[0]; //std::cout << "alpha "<< shape_face[1] <<". Beta:" << shape_face[1] << std::endl; // Face Detected [1]/ First face [0]/ Shape Info [0]/ Alpha [1] float alpha = result[1][i][0][1]; float beta = result[1][i][0][2]; float sx = result[1][i][0][3]; float sy = result[1][i][0][4]; std::string name = result[1][i][1][2]; float score = result[1][i][1][1]; // sizeX / sizeY are the face size in relation to the image float sizeX = mImageHeight * sx; float sizeY = mImageWidth * sy; // Centre of face into the image float x = mImageWidth / 2 - mImageWidth * alpha; float y = mImageHeight / 2 + mImageHeight * beta; vpDisplay::displayCross(I, y, x, 10, vpColor::red); vpDisplay::displayRectangle(I,y,x,0.0,sizeX,sizeY,vpColor::cyan,1); std::cout << i << "- size: " << sizeX*sizeY << std::endl; if (score >= 0.4) { std::cout << i << "- Name: " << name <<" score " << score << std::endl; // std::cout << "NAMES: " << names << std::endl; // std::cout << "Esite: " << in_array(name, names) << std::endl; // std::cout << "==================================== " << std::endl; if (!in_array(name, names) && sizeX*sizeY > 4000) { std::string phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\" + name; std::cout << phraseToSay << std::endl; tts.post.say(phraseToSay); names.push_back(name); } } else std::cout << i << "- Face not recognized. " << std::endl; // cv::Point p1(x - (sizeX / 2), y - (sizeY / 2)); // cv::Point p2(x + (sizeX / 2), y + (sizeY / 2)); // cv::rectangle(I, p1, p2, cv::Scalar(255,255,255)); } } //vpTime::sleepMs(60); vpDisplay::flush(I); if (vpDisplay::getClick(I, false)) break; } df.unsubscribe("Face"); } catch (const vpException &e) { std::cerr << "Caught exception: " << e.what() << std::endl; } catch (const AL::ALError &e) { std::cerr << "Caught exception: " << e.what() << std::endl; } return 0; }
bool vpFaceTrackerOkao::detect() { m_message.clear(); m_polygon.clear(); m_nb_objects = 0; m_faces.clear(); m_scores.clear(); bool target_found = false; AL::ALValue result = m_mem_proxy.getData("FaceDetected"); //-- Detect faces if (result.getSize() >=2) { //std::cout << "face" << std::endl; AL::ALValue info_face_array = result[1]; target_found = true; double min_dist = m_image_width*m_image_height; unsigned int index_closest_cog = 0; vpImagePoint closest_cog; for (unsigned int i = 0; i < info_face_array.getSize()-1; i++ ) { //Extract face info // Face Detected [1]/ First face [0]/ Shape Info [0]/ Alpha [1] float alpha = result[1][i][0][1]; float beta = result[1][i][0][2]; float sx = result[1][i][0][3]; float sy = result[1][i][0][4]; std::string name = result[1][i][1][2]; float score = result[1][i][1][1]; std::ostringstream message; if (score > 0.6) message << name; else message << "Unknown"; m_message.push_back( message.str() ); m_scores.push_back(score); // sizeX / sizeY are the face size in relation to the image float h = m_image_height * sx; float w = m_image_width * sy; // Center of face into the image float x = m_image_width / 2 - m_image_width * alpha; float y = m_image_height / 2 + m_image_height * beta; vpImagePoint cog(x,y); double dist = vpImagePoint::distance(m_previuos_cog,cog); if (dist< min_dist) { closest_cog = cog; index_closest_cog = i; min_dist = dist; } std::vector<vpImagePoint> polygon; double x_corner = x - h/2; double y_corner = y - w/2; polygon.push_back(vpImagePoint(y_corner , x_corner )); polygon.push_back(vpImagePoint(y_corner+w, x_corner )); polygon.push_back(vpImagePoint(y_corner+w, x_corner+h)); polygon.push_back(vpImagePoint(y_corner , x_corner+h)); m_polygon.push_back(polygon); m_nb_objects ++; } if (index_closest_cog !=0) std::swap(m_polygon[0], m_polygon[index_closest_cog]); m_previuos_cog = closest_cog; } return target_found; }
/*! Connect toRomeo robot, and apply some motion. By default, this example connect to a robot with ip address: 198.18.0.1. If you want to connect on an other robot, run: ./motion --ip <robot ip address> Example: ./motion --ip 169.254.168.230 */ int main(int argc, const char* argv[]) { try { std::string opt_ip = "198.18.0.1";; if (argc == 3) { if (std::string(argv[1]) == "--ip") opt_ip = argv[2]; } // Create a general proxy to motion to use new functions not implemented in the specialized proxy std::string myIP = ""; // IP du portable (voir /etc/hosts) int myPort = 0 ; // Default broker port // AL::ALProxy *m_proxy; boost::shared_ptr<AL::ALBroker> broker = AL::ALBroker::createBroker("Broker", myIP, myPort, opt_ip, 9559); // m_proxy = new AL::ALProxy(broker, "ALVideoDevice"); // m_proxy->callVoid("setCameraGroup",1 ,true); //AL::ALProxy* dcm = new AL::ALProxy(broker, "DCM_video"); boost::shared_ptr<AL::ALProxy> proxy = boost::shared_ptr<AL::ALProxy>(new AL::ALProxy(broker, "DCM_video")); AL::DCMProxy* dcm = new AL::DCMProxy(proxy); //boost::shared_ptr<AL::ALProxy> dcm = boost::shared_ptr<AL::ALProxy>(new AL::ALProxy(broker, "DCM_video")); AL::ALValue commands; commands.arraySetSize(3); commands[0] = std::string("FaceBoard/CameraSwitch/Value"); commands[1] = std::string("Merge"); commands[2].arraySetSize(1); commands[2][0].arraySetSize(2); commands[2][0][0] = 1; //commands[2][0][1] = dcm->call<int>("getTime",0);// dcm->getTime(0); commands[2][0][1] = dcm->getTime(0); //dcm->callVoid("set",commands); dcm->set(commands); //int time = dcm->call<int>("getTime",0); int time = dcm->getTime(10); std::cout <<"Time " << time << std::endl; //dcm = naoqitools.myGetProxy( "DCM_video" );^M //dcm.set( ["FaceBoard/CameraSwitch/Value", "Merge", [[rGroupNumber, dcm.getTime( 0 ) ]] ] );^M //dcm.set(\["ChestBoard/Led/Red/Actuator/Value", "Merge", \[[1.0, dcm.getTime(10000)]] ]) } catch (const vpException &e) { std::cerr << "Caught exception: " << e.what() << std::endl; } catch (const AL::ALError &e) { std::cerr << "Caught exception: " << e.what() << std::endl; } return 0; }