void CBuzzController::SetBytecode(const std::string& str_fname) {
/* Reset the BuzzVM */
if(m_tBuzzVM) buzzvm_destroy(&m_tBuzzVM);
m_tBuzzVM = buzzvm_new(m_unRobotId);
/* Save the bytecode filename */
m_strBytecodeFName = str_fname;
/* Load the bytecode */
std::ifstream cBCodeFile(str_fname.c_str(), std::ios::binary | std::ios::ate);
if(cBCodeFile.fail()) {
THROW_ARGOSEXCEPTION("Can't open file \"" << str_fname << "\": " << strerror(errno));
}
std::ifstream::pos_type unFileSize = cBCodeFile.tellg();
m_cBytecode.Clear();
m_cBytecode.Resize(unFileSize);
cBCodeFile.seekg(0, std::ios::beg);
cBCodeFile.read(reinterpret_cast<char*>(m_cBytecode.ToCArray()), unFileSize);
/* Load the script */
buzzvm_set_bcode(m_tBuzzVM, m_cBytecode.ToCArray(), m_cBytecode.Size());
if(buzzvm_set_bcode(m_tBuzzVM, m_cBytecode.ToCArray(), m_cBytecode.Size()) != BUZZVM_STATE_READY) {
THROW_ARGOSEXCEPTION("Error loading Buzz script \"" << str_fname << "\": " << buzzvm_strerror(m_tBuzzVM));
}
/* Register basic function */
if(RegisterFunctions() != BUZZVM_STATE_READY) {
THROW_ARGOSEXCEPTION("Error while registering functions");
}
/* Execute the global part of the script */
buzzvm_execute_script(m_tBuzzVM);
/* Call the Init() function */
buzzvm_function_call(m_tBuzzVM, "init", 0);
}
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);
}
}
int main(int argc, char** argv) {
/* Parse command line */
if(argc != 2) {
fprintf(stderr, "Usage:\n\t%s <file.bo>\n\n", argv[0]);
return 0;
}
/* Open file */
int fd = open(argv[1], O_RDONLY);
if(fd < 0) perror(argv[1]);
/* Read data */
size_t bcode_size = lseek(fd, 0, SEEK_END);
lseek(fd, 0, SEEK_SET);
uint8_t* bcode_buf = (uint8_t*)malloc(bcode_size);
ssize_t rd;
size_t tot = 0;
while(tot < bcode_size) {
rd = read(fd, bcode_buf + tot, bcode_size - tot);
if(rd < 0) perror(argv[1]);
tot += rd;
}
close(fd);
/* Create new VM */
buzzvm_t vm = buzzvm_new(1);
/* Set byte code */
buzzvm_set_bcode(vm, bcode_buf, bcode_size);
/* Register hook function */
buzzvm_pushs(vm, buzzvm_string_register(vm, "hook"));
buzzvm_pushcc(vm, buzzvm_function_register(vm, hook));
buzzvm_gstore(vm);
/* Run byte code and dump state */
do dump(vm);
while(buzzvm_step(vm) == BUZZVM_STATE_READY);
if(vm->state == BUZZVM_STATE_DONE) {
dump(vm);
fprintf(stdout, "%s: execution terminated correctly\n\n",
argv[1]);
}
else {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
argv[1],
buzzvm_error_desc[vm->error]);
}
/* Destroy VM */
free(bcode_buf);
buzzvm_destroy(&vm);
return 0;
}
int main(int argc, char** argv) {
/* The bytecode filename */
char* bcfname;
/* The debugging information file name */
char* dbgfname;
/* Whether or not to show the assembly information */
int trace = 0;
/* Parse command line */
if(argc < 3 || argc > 4) usage(argv[0], 0);
if(argc == 3) {
bcfname = argv[1];
dbgfname = argv[2];
}
else {
bcfname = argv[2];
dbgfname = argv[3];
if(strcmp(argv[1], "--trace") != 0) {
fprintf(stderr, "error: %s: unrecognized option '%s'\n", argv[0], argv[1]);
usage(argv[0], 1);
}
trace = 1;
}
/* Read bytecode and fill in data structure */
FILE* fd = fopen(bcfname, "rb");
if(!fd) perror(bcfname);
fseek(fd, 0, SEEK_END);
size_t bcode_size = ftell(fd);
rewind(fd);
uint8_t* bcode_buf = (uint8_t*)malloc(bcode_size);
if(fread(bcode_buf, 1, bcode_size, fd) < bcode_size) {
perror(bcfname);
}
fclose(fd);
/* Read debug information */
buzzdebug_t dbg_buf = buzzdebug_new();
if(!buzzdebug_fromfile(dbg_buf, dbgfname)) {
perror(dbgfname);
}
/* Create new VM */
buzzvm_t vm = buzzvm_new(1);
/* Set byte code */
buzzvm_set_bcode(vm, bcode_buf, bcode_size);
/* Register hook functions */
buzzvm_pushs(vm, buzzvm_string_register(vm, "print", 1));
buzzvm_pushcc(vm, buzzvm_function_register(vm, print));
buzzvm_gstore(vm);
/* Run byte code */
do if(trace) buzzdebug_stack_dump(vm, 1, stdout);
while(buzzvm_step(vm) == BUZZVM_STATE_READY);
/* Done running, check final state */
int retval;
if(vm->state == BUZZVM_STATE_DONE) {
/* Execution terminated without errors */
if(trace) buzzdebug_stack_dump(vm, 1, stdout);
fprintf(stdout, "%s: execution terminated correctly\n\n",
bcfname);
retval = 0;
}
else {
/* Execution terminated with errors */
if(trace) buzzdebug_stack_dump(vm, 1, stdout);
buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(dbg_buf, &vm->pc);
if(dbg != NULL) {
fprintf(stderr, "%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n",
bcfname,
dbg->fname,
dbg->line,
dbg->col,
vm->errormsg);
}
else {
fprintf(stderr, "%s: execution terminated abnormally at bytecode offset %d: %s\n\n",
bcfname,
vm->pc,
vm->errormsg);
}
retval = 1;
}
/* Destroy VM */
free(bcode_buf);
buzzdebug_destroy(&dbg_buf);
buzzvm_destroy(&vm);
/* All done */
return retval;
}
