bool Desktop::OnCompleteCommand(const string_t &cmd, int &pos, string_t &result)
{
assert(pos >= 0);
lua_getglobal(g_env.L, "autocomplete");
if( lua_isnil(g_env.L, -1) )
{
lua_pop(g_env.L, 1);
GetConsole().WriteLine(1, "There was no autocomplete module loaded");
return false;
}
lua_pushlstring(g_env.L, cmd.substr(0, pos).c_str(), pos);
HRESULT hr = S_OK;
if( lua_pcall(g_env.L, 1, 1, 0) )
{
GetConsole().WriteLine(1, lua_tostring(g_env.L, -1));
}
else
{
const char *str = lua_tostring(g_env.L, -1);
string_t insert = str ? str : "";
result = cmd.substr(0, pos) + insert + cmd.substr(pos);
pos += insert.length();
if( g_client && !g_client->IsLocal() && !result.empty() && result[0] != '/' )
{
result = string_t("/") + result;
++pos;
}
}
lua_pop(g_env.L, 1); // pop result or error message
return true;
}
bool MachineBridge::registerListener() //TODO select only events that I'm interested to
{
nsresult rc;
if(session == nsnull)
return false;
NS_CHECK_AND_DEBUG_ERROR(session, GetConsole(getter_AddRefs(console)), rc);
if(NS_FAILED(rc))
return false;
NS_CHECK_AND_DEBUG_ERROR(console, GetEventSource(getter_AddRefs(eventSource)), rc);
if(NS_FAILED(rc))
return false;
// QVector<VBoxEventType> events;
uint32_t events[1];
// events << VBoxEventType::Any;
events[0] = VBoxEventType::Any;
NS_CHECK_AND_DEBUG_ERROR(eventSource, RegisterListener(eventListener, /*events.count()*/ 1, events, (PRBool) true), rc);
if(NS_FAILED(rc))
return false;
return true;
}
static ConfVar* ArrayElementFromLua(lua_State *L, ConfVarArray *parent, size_t key)
{
ConfVar* result = NULL;
int valueType = lua_type(L, -1);
switch( valueType )
{
case LUA_TSTRING:
result = parent->GetVar(key, ConfVar::typeString).first;
break;
case LUA_TBOOLEAN:
result = parent->GetVar(key, ConfVar::typeBoolean).first;
break;
case LUA_TNUMBER:
result = parent->GetVar(key, ConfVar::typeNumber).first;
break;
case LUA_TTABLE:
result = parent->GetVar(key, lua_objlen(L,-1) ? ConfVar::typeArray : ConfVar::typeTable).first;
break;
default:
GetConsole().Format(1) << "Unknown lua type - " << lua_typename(L, valueType);
return NULL;
}
return result;
}
static int vlclua_console_wait( lua_State *L )
{
int i_timeout = luaL_optint( L, 1, 0 );
DWORD status = WaitForSingleObject( GetConsole( L ), i_timeout );
lua_pushboolean( L, status == WAIT_OBJECT_0 );
return 1;
}
void Log::Write(eLogType logType, const char * fmt, ...) {
char buf[2048] = { 0 };
va_list va_alist;
va_start(va_alist, fmt);
vsprintf_s(buf, fmt, va_alist);
va_end(va_alist);
GetConsole()->SetTextColor(logTypeToColorMap[logType]);
char buff2[2048] = { 0 };
sprintf_s(buff2, "%s %s\n", GetTimeFormatted().c_str(), buf);
// Print to console
printf(buff2);
#ifndef _DEBUG
if (logType == LogTypeDebug) {
return;
}
#endif
sprintf_s(buff2, "%s%s %s\n", GetTimeFormatted().c_str(), logTypeToFormatMap[logType].c_str(), buf);
// Write to log file
LogToFile(buff2);
}
RobotInterface::Status ExampleOpenLoopPIDControlModule::RobotInit(){
// Sensors and actuators
mSensorsGroup.SetSensorsList(mRobot->GetSensors());
mActuatorsGroup.SetActuatorsList(mRobot->GetActuators());
mJointTorques.Resize(mRobot->GetDOFCount());
mJointTarget.Resize(mRobot->GetDOFCount());
//mJointTorques.Print();
if(mInternalRobot.Load(mRobot->GetType(),mRobot->GetSubType(),"")){
cout << "Internal robot loaded as a copy of main robbot"<<endl;
}else{
cout << "Failed loading internal robot"<<endl;
exit(0);
}
mInternalSensorsGroup.SetSensorsList(mInternalRobot.GetSensors());
mInternalActuatorsGroup.SetActuatorsList(mInternalRobot.GetActuators());
// Inverse dynamics
mInvDynamics.SetRobot(mRobot);
mInvDynamics.Init();
mInvDynamics.SetGravityCompensationOnly(true);
XmlTree tree;
tree.LoadFromFile("./data/packages/WAMRobotModel/Misc/WAMDefaultPID.xml");
mPIDCtrl.Init(&tree);
mState = 0;
if(GetConsole()){
AddConsoleCommand("Rest");
AddConsoleCommand("Hit");
AddConsoleCommand("GComp");
GetConsole()->Print("Available commands are GComp Rest and Hit");
}
return STATUS_OK;
}
static int vlclua_console_read( lua_State *L )
{
char psz_buffer[MAX_LINE_LENGTH+1];
int i_size = 0;
ReadWin32( GetConsole( L ), psz_buffer, &i_size );
lua_pushlstring( L, psz_buffer, i_size );
return 1;
}
static int vlclua_console_read( lua_State *L )
{
char psz_buffer[MAX_LINE_LENGTH+1];
int i_size = 0;
HANDLE consol = GetConsole( L ); // sunqueen add
ReadWin32( &consol/*GetConsole( L )*/, psz_buffer, &i_size ); // sunqueen modify
lua_pushlstring( L, psz_buffer, i_size );
return 1;
}
size_t TextureManager::FindSprite(const string_t &name) const
{
std::map<string_t, size_t>::const_iterator it = _mapName_to_Index.find(name);
if( _mapName_to_Index.end() != it )
return it->second;
// flood the console
GetConsole().Printf(1, "texture '%s' not found!", name.c_str());
return 0; // index of checker texture
}
void MessageArea::WriteLine(const string_t &text)
{
GetConsole().WriteLine(0, text);
Line line;
line.time = 5; // timeout
line.str = text;
_lines.push_front(line);
SetTimeStep(true);
}
Desktop::Desktop(LayoutManager* manager)
: Window(NULL, manager)
, _font(GetManager()->GetTextureManager()->FindSprite("font_default"))
{
SetTexture("ui/window", false);
_msg = new MessageArea(this, 100, 100);
_editor = new EditorLayout(this);
_editor->SetVisible(false);
_con = Console::Create(this, 10, 0, 100, 100, &GetConsole());
_con->eventOnSendCommand = std::bind( &Desktop::OnCommand, this, _1 );
_con->eventOnRequestCompleteCommand = std::bind( &Desktop::OnCompleteCommand, this, _1, _2, _3 );
_con->SetVisible(false);
_con->SetTopMost(true);
SpriteColor colors[] = {0xffffffff, 0xffff7fff};
_con->SetColors(colors, sizeof(colors) / sizeof(colors[0]));
_con->SetHistory(&_history);
_score = new ScoreTable(this);
_score->SetVisible(false);
_fps = new FpsCounter(this, 0, 0, alignTextLB);
g_conf.ui_showfps.eventChange = std::bind(&Desktop::OnChangeShowFps, this);
OnChangeShowFps();
_time = new TimeElapsed(this, 0, 0, alignTextRB);
g_conf.ui_showtime.eventChange = std::bind(&Desktop::OnChangeShowTime, this);
OnChangeShowTime();
if( g_conf.dbg_graph.Get() )
{
float xx = 200;
float yy = 3;
float hh = 50;
for( size_t i = 0; i < CounterBase::GetMarkerCountStatic(); ++i )
{
Oscilloscope *os = new Oscilloscope(this, xx, yy);
os->Resize(400, hh);
os->SetRange(-1/15.0f, 1/15.0f);
os->SetTitle(CounterBase::GetMarkerInfoStatic(i).title);
CounterBase::SetMarkerCallbackStatic(i, CreateDelegate(&Oscilloscope::Push, os));
yy += hh+5;
}
}
OnRawChar(VK_ESCAPE); // to invoke main menu dialog
}
void FConsoleHelper::VExec(const TArray<FString>& Args)
{
FString Cmd = "vexec ";
uint32 NumArgs = Args.Num();
if (NumArgs == 0) return;
for (uint32 ArgIndex = 0; ArgIndex < NumArgs - 1; ArgIndex++)
{
Cmd += Args[ArgIndex] + " ";
}
Cmd += Args[NumArgs - 1];
FExecStatus ExecStatus = CommandDispatcher->Exec(Cmd);
GetConsole()->Log(ExecStatus.GetMessage());
}
void PropertySet::MyExchange(bool applyToObject)
{
if( applyToObject )
{
const char *name = _propName.GetStringValue().c_str();
GC_Object* found = g_level->FindObject(name);
if( found && GetObject() != found )
{
GetConsole().Format(1) << "object with name \"" << name << "\" already exists";
}
else
{
GetObject()->SetName(name);
}
}
else
{
const char *name = GetObject()->GetName();
_propName.SetStringValue(name ? name : "");
}
}
void FConsoleHelper::VRun(const TArray<FString>& Args)
{
if (CommandDispatcher == nullptr)
{
UE_LOG(LogUnrealCV, Error, TEXT("CommandDispatcher not set"));
}
FString Cmd = "vrun ";
uint32 NumArgs = Args.Num();
if (NumArgs == 0) return;
for (uint32 ArgIndex = 0; ArgIndex < NumArgs-1; ArgIndex++)
{
Cmd += Args[ArgIndex] + " ";
}
Cmd += Args[NumArgs-1]; // Maybe a more elegant implementation for joining string
FUE4CVServer::Get().InitWorld();
FExecStatus ExecStatus = CommandDispatcher->Exec(Cmd);
UE_LOG(LogUnrealCV, Warning, TEXT("vrun helper function, the real command is %s"), *Cmd);
// In the console mode, output should be writen to the output log.
UE_LOG(LogUnrealCV, Warning, TEXT("%s"), *ExecStatus.GetMessage());
GetConsole()->Log(ExecStatus.GetMessage());
}
void FConsoleHelper::VSet(const TArray<FString>& Args)
{
if (CommandDispatcher == nullptr)
{
UE_LOG(LogUnrealCV, Error, TEXT("CommandDispatcher not set"));
}
FString Cmd = "vset ";
uint32 NumArgs = Args.Num();
if (NumArgs == 0) return;
for (uint32 ArgIndex = 0; ArgIndex < NumArgs-1; ArgIndex++)
{
Cmd += Args[ArgIndex] + " ";
}
Cmd += Args[NumArgs-1];
FUE4CVServer::Get().InitWorld();
FExecStatus ExecStatus = CommandDispatcher->Exec(Cmd);
// Output result to the console
UE_LOG(LogUnrealCV, Warning, TEXT("vset helper function, the real command is %s"), *Cmd);
UE_LOG(LogUnrealCV, Warning, TEXT("%s"), *ExecStatus.GetMessage());
GetConsole()->Log(ExecStatus.GetMessage());
}
bool ConfVarTable::Load(const char *filename)
{
lua_State *L = lua_open();
// try to read and execute the file
if( luaL_loadfile(L, filename) || lua_pcall(L, 0, 0, 0) )
{
GetConsole().WriteLine(1, lua_tostring(L, -1));
lua_close(L);
return false;
}
// get global table
lua_pushvalue(L, LUA_GLOBALSINDEX);
bool result = Assign(L);
lua_close(L);
FireValueUpdate(this);
return result;
}
bool ConfVarTable::Assign(lua_State *L)
{
// enumerate all fields of the table
for( lua_pushnil(L); lua_next(L, -2); lua_pop(L, 1) )
{
// now 'key' is at index -2 and 'value' at index -1
// use only string keys
if( LUA_TSTRING == lua_type(L, -2) )
{
const char *key = lua_tostring(L, -2);
if( ConfVar *v = TableElementFromLua(L, this, key) )
{
if( !v->Assign(L) )
return false;
}
else
{
GetConsole().Printf(1, "variable '%s' was dropped", key);
}
}
}
return true;
}
void CApplication::PrintError(const tstring& sText)
{
tstring sTrimmedText = trim(sText);
GetConsole()->PrintConsole(tstring("[color=FF0000]ERROR: ") + sTrimmedText + "[/color]" + (sText.endswith("\n")?"\n":""));
}
RobotInterface::Status RobotStatePublisher::RobotInit(){
mLWRRobot = (LWRRobot*)mRobot;
nh = mRobot->InitializeROS();
string topicName;
topicName = mRobot->GetName();
GetConsole()->Print("Robot Init Publisher");
float new_sampling_time = 0.002;
mLWRRobot->SetSamplingTime(new_sampling_time);
dt = mLWRRobot->GetSamplingTime();
std::ostringstream ss;
ss << "DT: " << dt;
std::string msg(ss.str());
GetConsole()->Print(msg);
// topicName += "/JointState";
string JointStateName = "/joint_states";
jointStatePublisher = nh->advertise<sensor_msgs::JointState>(JointStateName,100);
topicName = mRobot->GetName();
topicName += "/Pose";
posePublisher = nh->advertise<geometry_msgs::PoseStamped>(topicName,100);
topicName = mRobot->GetName();
topicName += "/FT";
ftPublisher = nh->advertise<geometry_msgs::WrenchStamped>(topicName,3);
topicName = mRobot->GetName();
topicName += "/Stiff";
stiffPublisher = nh->advertise<geometry_msgs::TwistStamped>(topicName,3);
mSensorsGroup.SetSensorsList(mRobot->GetSensors());
jointStateMsg.position.resize(mRobot->GetDOFCount());
jointStateMsg.velocity.resize(mRobot->GetDOFCount());
jointStateMsg.effort.resize(mRobot->GetDOFCount());
jointStateMsg.name.resize(mRobot->GetDOFCount());
char buf[255];
pXmlTree options = GetOptionTree();
string which_arm;
if(options) {
which_arm = options->CGet("Options.Arm", string("right"));
} else {
which_arm = "right";
}
cout<<"Using as "<<which_arm<<" arm";
for(int i=0; i<mRobot->GetDOFCount(); ++i) {
sprintf(buf, "%s_arm_%d_joint",which_arm.c_str(), i);
jointStateMsg.name[i] = buf;
}
topicName = mRobot->GetName();
topicName += "/CoreRate";
//rosrt::init();
//ratePublisher = new rosrt::Publisher<std_msgs::Empty>(*nh,topicName,1,10,std_msgs::Empty());
//emptyMsg = ratePublisher->allocate();
return STATUS_OK;
}
static void guard_irq_handler(irq_t source, irq_context_t *context) {
dbglog(DBG_INFO, "\n=============== CATCHING EXCEPTION ===============\n");
irq_context_t *irq_ctx = irq_get_context();
if (source == EXC_FPU) {
/* Display user friendly informations */
export_sym_t * symb;
symb = export_lookup_by_addr(irq_ctx->pc);
if (symb) {
dbglog(DBG_INFO, "FPU EXCEPTION PC = %s + 0x%08x (0x%08x)\n",
symb->name,
((int)irq_ctx->pc) - ((int)symb->ptr),
(int)irq_ctx->pc);
}
/* skip the offending FPU instruction */
int *ptr = (int *)&irq_ctx->r[0x40/4];
*ptr += 4;
return;
}
/* Display user friendly informations */
export_sym_t * symb;
int i;
uint32 *stk = (uint32 *)irq_ctx->r[15];
for (i = 15; i >= 0; i--) {
if((stk[i] < 0x8c000000) || (stk[i] > 0x8d000000) ||
!(symb = export_lookup_by_addr(stk[i])) ||
((int)stk[i] - ((int)symb->ptr) > 0x800)) {
dbglog(DBG_INFO, "STACK#%2d = 0x%08x\n", i, (int)stk[i]);
} else {
dbglog(DBG_INFO, "STACK#%2d = 0x%08x (%s + 0x%08x)\n",
i, (int)stk[i], symb->name, (int)stk[i] - ((int)symb->ptr));
}
}
symb = export_lookup_by_addr(irq_ctx->pc);
if (symb && (int)stk[i] - ((int)symb->ptr) < 0x800) {
dbglog(DBG_INFO, " PC = %s + 0x%08x (0x%08x)\n",
symb->name,
((int)irq_ctx->pc) - ((int)symb->ptr),
(int)irq_ctx->pc);
} else {
dbglog(DBG_INFO, " PC = 0x%08x\n", (int)irq_ctx->pc);
}
symb = export_lookup_by_addr(irq_ctx->pr);
if (symb && (int)stk[i] - ((int)symb->ptr) < 0x800) {
dbglog(DBG_INFO, " PR = %s + 0x%08x (0x%08x)\n",
symb->name,
((int)irq_ctx->pr) - ((int)symb->ptr),
(int)irq_ctx->pr);
} else {
dbglog(DBG_INFO, " PR = 0x%08x\n", (int)irq_ctx->pr);
}
uint32 *regs = irq_ctx->r;
dbglog(DBG_INFO, " R0-R3 = %08lx %08lx %08lx %08lx\n", regs[0], regs[1], regs[2], regs[3]);
dbglog(DBG_INFO, " R4-R7 = %08lx %08lx %08lx %08lx\n", regs[4], regs[5], regs[6], regs[7]);
dbglog(DBG_INFO, " R8-R11 = %08lx %08lx %08lx %08lx\n", regs[8], regs[9], regs[10], regs[11]);
dbglog(DBG_INFO, " R12-R15 = %08lx %08lx %08lx %08lx\n", regs[12], regs[13], regs[14], regs[15]);
//arch_stk_trace_at(regs[14], 0);
for (i = 0; exceptions_code[i].code; i++) {
if (exceptions_code[i].code == source) {
dbglog(DBG_INFO, " EVENT = %s (0x%08x)\n", exceptions_code[i].name, (int)source);
break;
}
}
expt_quard_stack_t *s = NULL;
s = (expt_quard_stack_t *) kthread_getspecific(expt_key);
if (s && s->pos >= 0) {
// Simulate a call to longjmp by directly changing stored
// context of the exception
irq_ctx->pc = (uint32)longjmp;
irq_ctx->r[4] = (uint32)s->jump[s->pos];
irq_ctx->r[5] = (uint32)(void *) -1;
} else {
//malloc_stats();
//texture_memstats();
/* not handled --> panic !! */
//irq_dump_regs(0, source);
dbgio_set_dev_fb();
vid_clear(0, 0, 0);
ConsoleInformation *con = GetConsole();
for(i = 16; i > 0; i--) {
dbglog(DBG_INFO, "%s\n", con->ConsoleLines[i]);
}
dbglog(DBG_ERROR, "Unhandled Exception. Reboot after 10 seconds.");
//panic("Unhandled IRQ/Exception");
timer_spin_sleep(10000);
arch_reboot();
// asic_sys_reset();
}
}
int TextureManager::LoadPackage(const string_t &packageName, const SafePtr<FS::MemMap> &file)
{
TRACE("Loading texture package '%s'", packageName.c_str());
lua_State *L = lua_open();
if( 0 != (luaL_loadbuffer(L, file->GetData(), file->GetSize(), packageName.c_str()) || lua_pcall(L, 0, 1, 0)) )
{
GetConsole().WriteLine(1, lua_tostring(L, -1));
lua_close(L);
return 0;
}
if( !lua_istable(L, 1) )
{
lua_close(L);
return 0;
}
// loop over files
for( lua_pushnil(L); lua_next(L, -2); lua_pop(L, 1) )
{
// now 'key' is at index -2 and 'value' at index -1
// check that value is a table
if( !lua_istable(L, -1) )
{
TRACE("WARNING: value is not a table; skipping.");
}
while( lua_istable(L, -1) )
{
TexDescIterator td;
// get a file name; load
lua_getfield(L, -1, "file");
std::string f = lua_tostring(L, -1);
lua_pop(L, 1); // pop result of lua_getfield
try
{
LoadTexture(td, f);
}
catch( const std::exception &e )
{
TRACE("WARNING: could not load texture '%s' - %s", f.c_str(), e.what());
break;
}
// get 'content' field
lua_getfield(L, -1, "content");
if( lua_istable(L, -1) )
{
// loop over textures in 'content' table
for( lua_pushnil(L); lua_next(L, -2); lua_pop(L, 1) )
{
if( !lua_istable(L, -1) )
{
TRACE("WARNING: element of 'content' is not a table; skipping");
continue;
}
lua_pushvalue(L, -2); // create copy of the key
if( const char *texname = lua_tostring(L, -1) )
{
// now 'value' at index -2
float scale_x = auxgetfloat(L, -2, "xscale", 1);
float scale_y = auxgetfloat(L, -2, "yscale", 1);
LogicalTexture tex;
tex.dev_texture = td->id;
// texture bounds
tex.uvLeft = (float) floorf(auxgetfloat(L, -2, "left", 0)) / (float) td->width;
tex.uvRight = (float) floorf(auxgetfloat(L, -2, "right", (float) td->width)) / (float) td->width;
tex.uvTop = (float) floorf(auxgetfloat(L, -2, "top", 0)) / (float) td->height;
tex.uvBottom = (float) floorf(auxgetfloat(L, -2, "bottom", (float) td->height)) / (float) td->height;
// frames count
tex.xframes = auxgetint(L, -2, "xframes", 1);
tex.yframes = auxgetint(L, -2, "yframes", 1);
// frame size
tex.uvFrameWidth = (tex.uvRight - tex.uvLeft) / (float) tex.xframes;
tex.uvFrameHeight = (tex.uvBottom - tex.uvTop) / (float) tex.yframes;
// original size
tex.pxFrameWidth = (float) td->width * scale_x * tex.uvFrameWidth;
tex.pxFrameHeight = (float) td->height * scale_y * tex.uvFrameHeight;
// pivot position
tex.uvPivot.x = (float) auxgetfloat(L, -2, "xpivot", (float) td->width * tex.uvFrameWidth / 2) / ((float) td->width * tex.uvFrameWidth);
tex.uvPivot.y = (float) auxgetfloat(L, -2, "ypivot", (float) td->height * tex.uvFrameHeight / 2) / ((float) td->height * tex.uvFrameHeight);
// frames
tex.uvFrames.reserve(tex.xframes * tex.yframes);
for( int y = 0; y < tex.yframes; ++y )
{
for( int x = 0; x < tex.xframes; ++x )
{
FRECT rt;
rt.left = tex.uvLeft + tex.uvFrameWidth * (float) x;
rt.right = tex.uvLeft + tex.uvFrameWidth * (float) (x + 1);
rt.top = tex.uvTop + tex.uvFrameHeight * (float) y;
rt.bottom = tex.uvTop + tex.uvFrameHeight * (float) (y + 1);
tex.uvFrames.push_back(rt);
}
}
//---------------------
if( tex.xframes > 0 && tex.yframes > 0 )
{
td->refCount++;
//---------------------------------------------
std::map<string_t, size_t>::iterator it =
_mapName_to_Index.find(texname);
if( _mapName_to_Index.end() != it )
{
// replace existing logical texture
LogicalTexture &existing = _logicalTextures[it->second];
TexDescIterator tmp =
_mapDevTex_to_TexDescIter[existing.dev_texture];
existing = tex;
tmp->refCount--;
assert(tmp->refCount >= 0);
}
else
{
// define new texture
_mapName_to_Index[texname] = _logicalTextures.size();
_logicalTextures.push_back(tex);
}
} // end if( xframes > 0 && yframes > 0 )
} // end if( texname )
lua_pop(L, 1); // remove copy of the key
} // end loop over 'content'
} // end if 'content' is table
else
{
TRACE("WARNING: 'content' field is not a table.");
}
lua_pop(L, 1); // pop the result of getfield("content")
break;
} // end of while( lua_istable(L, -1) )
}
lua_close(L);
//
// unload unused textures
//
TexDescIterator it = _textures.begin();
while( _textures.end() != it )
{
TexDescIterator tmp = it++;
assert(tmp->refCount >= 0);
if( 0 == tmp->refCount )
Unload(tmp);
}
TRACE("Total number of loaded textures: %d", _logicalTextures.size());
return _logicalTextures.size();
}
void CApplication::PrintConsole(const tstring& sText)
{
GetConsole()->PrintConsole(sText);
}
