void CBuzzController::Init(TConfigurationNode& t_node) {
try {
/* Get pointers to devices */
m_pcRABA = GetActuator<CCI_RangeAndBearingActuator>("range_and_bearing");
m_pcRABS = GetSensor <CCI_RangeAndBearingSensor >("range_and_bearing");
try {
m_pcPos = GetSensor <CCI_PositioningSensor>("positioning");
}
catch(CARGoSException& ex) {}
/* Get the script name */
std::string strBCFName;
GetNodeAttributeOrDefault(t_node, "bytecode_file", strBCFName, strBCFName);
/* Get the script name */
std::string strDbgFName;
GetNodeAttributeOrDefault(t_node, "debug_file", strDbgFName, strDbgFName);
/* Initialize the rest */
bool bIDSuccess = false;
m_unRobotId = 0;
/* Find Buzz ID */
size_t tStartPos = GetId().find_last_of("_");
if(tStartPos != std::string::npos){
/* Checks for ID after last "_" ie. footbot_group3_10 -> 10 */
m_unRobotId = FromString<UInt16>(GetId().substr(tStartPos+1));
bIDSuccess = true;
}
/* FromString() returns 0 if passed an invalid string */
if(!m_unRobotId || !bIDSuccess){
/* Checks for ID after first number footbot_simulated10 -> 10 */
tStartPos = GetId().find_first_of("0123456789");
if(tStartPos != std::string::npos){
m_unRobotId = FromString<UInt16>(GetId().substr(tStartPos));
bIDSuccess = true;
}
}
if(!bIDSuccess) {
THROW_ARGOSEXCEPTION("Error in finding Buzz ID from name \"" << GetId() << "\"");
}
if(strBCFName != "" && strDbgFName != "")
SetBytecode(strBCFName, strDbgFName);
else
m_tBuzzVM = buzzvm_new(m_unRobotId);
UpdateSensors();
/* Set initial robot message (id and then all zeros) */
CByteArray cData;
cData << m_tBuzzVM->robot;
while(cData.Size() < m_pcRABA->GetSize()) cData << static_cast<UInt8>(0);
m_pcRABA->SetData(cData);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the Buzz controller", ex);
}
}
void CBuzzController::Reset() {
/* Reset debug information */
m_sDebug.Clear();
m_sDebug.TrajectoryClear();
m_sDebug.TrajectoryDisable();
m_sDebug.RayClear();
try {
/* Set the bytecode again */
if(m_strBytecodeFName != "" && m_strDbgInfoFName != "")
SetBytecode(m_strBytecodeFName, m_strDbgInfoFName);
}
catch(CARGoSException& ex) {
LOGERR << ex.what();
}
}
void CBuzzController::Init(TConfigurationNode& t_node) {
try {
/* Get pointers to devices */
m_pcRABA = GetActuator<CCI_RangeAndBearingActuator>("range_and_bearing");
m_pcRABS = GetSensor <CCI_RangeAndBearingSensor >("range_and_bearing");
/* Get the script name */
std::string strFName;
GetNodeAttribute(t_node, "bytecode_file", strFName);
/* Initialize the rest */
m_tBuzzVM = NULL;
m_unRobotId = FromString<UInt16>(GetId().substr(2));
SetBytecode(strFName);
UpdateSensors();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the Buzz controller", ex);
}
}
void CBuzzController::Reset() {
SetBytecode(m_strBytecodeFName);
}
