// Move the real block!
void feedbackCb( const InteractiveMarkerFeedbackConstPtr &feedback )
{
if (!action_server_.isActive())
{
ROS_INFO("[block logic] Got feedback but not active!");
return;
}
switch ( feedback->event_type )
{
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
ROS_INFO_STREAM("[block logic] Staging " << feedback->marker_name);
old_pose_ = feedback->pose;
break;
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
ROS_INFO_STREAM("[block logic] Now moving " << feedback->marker_name);
moveBlock(old_pose_, feedback->pose);
break;
}
interactive_m_server_.applyChanges();
}
/*
* returns TRUE if a move was made
*/
int moveBlocks(Block* blocks, Direction d, int* score, int* blocksLeft, Outcome* outcome) {
int result = FALSE;
switch (d) {
case UP:
for (int r = 0; r < size - 1; r++) {
for (int c = 0; c < size; c++) {
result = moveBlock(r, c, blocks, UP, score, blocksLeft, outcome) || result;
}
}
break;
case DOWN:
for (int r = size - 1; r >= 1; r--) {
for (int c = 0; c < size; c++) {
result = moveBlock(r, c, blocks, DOWN, score, blocksLeft, outcome) || result;
}
}
break;
case LEFT:
for (int c = 0; c < size - 1; c++) {
for (int r = 0; r < size; r++) {
result = moveBlock(r, c, blocks, LEFT, score, blocksLeft, outcome) || result;
}
}
break;
case RIGHT:
for (int c = size - 1; c >= 1; c--) {
for (int r = 0; r < size; r++) {
result = moveBlock(r, c, blocks, RIGHT, score, blocksLeft, outcome) || result;
}
}
break;
}
return result;
}
void AllegroFlasher::run()
{
const int zero = 0;
timer = al_create_timer(1.0 / FPS.at(zero));
display = al_create_display(width, height);
event_queue = al_create_event_queue();
al_register_event_source(event_queue, al_get_display_event_source(display));
al_register_event_source(event_queue, al_get_timer_event_source(timer));
al_register_event_source(event_queue, al_get_keyboard_event_source());
al_register_event_source(event_queue, al_get_mouse_event_source());
al_start_timer(timer); // Start the timer
int counter = 0;
while(!done) // Main loop
{
EVENT Event = EventType(); // Wait for an event to occur
if (Event == ESCAPE) { // Quit the program
counter = frames.at(zero);
break;
}
else if (Event == UPDATE) { // A timer event
if(useBlankStart && counter < frames.at(zero)/2)
displayBlankScreen(); // Simply display a black screen
else
updateDisplay();
counter++;
}
else if (Event == UP) {
moveBlock(UP);
}
else if (Event == DOWN) {
moveBlock(DOWN);
}
else if (Event == LEFT) {
moveBlock(LEFT);
}
else if (Event == RIGHT) {
moveBlock(RIGHT);
}
if(counter == frames.at(zero))
done = true;
}
al_destroy_timer(timer);
al_destroy_display(display);
al_destroy_event_queue(event_queue);
done = false; // Set again for the next time
}
VOID DumpAtomTable(
PRTL_ATOM_TABLE *ppat,
ATOM a
)
{
RTL_ATOM_TABLE at, *pat;
RTL_ATOM_TABLE_ENTRY ate, *pate;
int iBucket;
LPWSTR pwsz;
BOOL fFirst;
move(pat, ppat);
if (pat == NULL) {
dprintf("is not initialized.\n");
return;
}
move(at, pat);
if (a) {
dprintf("\n");
} else {
dprintf("at %x\n", pat);
}
for (iBucket = 0; iBucket < (int)at.NumberOfBuckets; iBucket++) {
move(pate, &pat->Buckets[iBucket]);
if (pate != NULL && !a) {
dprintf("Bucket %2d:", iBucket);
}
fFirst = TRUE;
while (pate != NULL) {
if (!fFirst && !a) {
dprintf(" ");
}
fFirst = FALSE;
move(ate, pate);
pwsz = (LPWSTR)LocalAlloc(LPTR, (ate.NameLength + 1) * sizeof(WCHAR));
moveBlock(*pwsz, &pate->Name, ate.NameLength * sizeof(WCHAR));
pwsz[ate.NameLength ] = L'\0';
if (a == 0 || a == (ATOM)(ate.HandleIndex | MAXINTATOM)) {
dprintf("%hx(%2d) = %ls (%d)%s\n",
(ATOM)(ate.HandleIndex | MAXINTATOM),
ate.ReferenceCount,
pwsz, ate.NameLength,
ate.Flags & RTL_ATOM_PINNED ? " pinned" : "");
if (a) {
LocalFree(pwsz);
return;
}
}
LocalFree(pwsz);
if (pate == ate.HashLink) {
dprintf("Bogus hash link at %x\n", pate);
break;
}
pate = ate.HashLink;
}
}
if (a)
dprintf("\n");
}
// Convert
int PuzzleTV::draw(YUV* src_yuv, RGB32* dst_rgb, char* dst_msg)
{
LOGI("%s(L=%d)", __func__, __LINE__);
RGB32* src_rgb = mUtils->yuv_YUVtoRGB(src_yuv);
memset(dst_rgb, 0, video_area * PIXEL_SIZE);
if (autoSolveTimer == 0) {
autoSolve();
} else {
autoSolveTimer--;
}
for (int i=0; i<blockNum; i++) {
if (i == movingBlock || i == spaceBlock) {
RGB32* q = dst_rgb + blocks[i].destOffset;
for (int y=0; y<blockSizeH; y++) {
memset(q, 0, blockSizeW * PIXEL_SIZE);
q += video_width;
}
} else {
copyBlockImage(src_rgb + blocks[i].srcOffset, dst_rgb + blocks[i].destOffset);
}
if (hint) {
copyHintImage(src_rgb, dst_rgb, blocks[i].destOffset);
}
}
if (movingBlock >= 0) {
moveBlock(src_rgb, dst_rgb);
}
return 0;
}
///////////////////////////////////////////////////////////////////////////////////////////////////
// scrolls the world (all the blocks) at the given velocity
//
void scrollWorld() {
TEntity *ce = &g_Character.entity;
TPhysics *p = &ce->phys;
u8 i;
// Scroll all the given block entities
for(i=0; i < g_lastBlock; ++i) {
// Move the block and check if the block has been destroyed
if ( moveBlock(i) ) {
// The block has been destroyed:
// 1. Eliminate floor from the main character to force recalculation
// 2. As blocks have been moved after destroy, repeat this last
// block by discounting 1 from the index and continuing.
p->floor = 0;
i--;
}
}
// If the floor of the Character has moved, it would have been set to be drawn.
// In that case, the character has to be moved along with its floor
if (p->floor && p->floor->draw) {
// Get height of the current sprite
TAnimation *anim = &ce->graph.anim;
u8 height = anim->frames[anim->frame_id]->height;
// Set new physics y coordinate and vy ~= 0 (less than minVel)
ce->phys.y = (p->floor->ny - height) * SCALE;
ce->phys.vy = G_minVel - 1;
ce->draw = 1;
}
// Expand a new block, when required, at Y coordinate G_minY-3,
// 3 pixels high and with ce->nx random increment
if ( g_blocks[0].draw && randomCreateNewBlock(G_minY-3, 3, ce->nx) ) {
// If the block was created, increment score and check if we arrive
// to a new zone (every 16 blocks, new zone)
if ( !(++G_score & 0x0F) ) {
// Change platform colour
if ( ++G_platfColour > 15)
G_platfColour = 1;
// Upgrade velocity
if (G_scrollVel < G_maxScrollVel)
++G_scrollVel;
}
}
}
static int draw(RGB32 *src, RGB32 *dest)
{
int y, i;
RGB32 *p, *q;
if(autoSolveTimer == 0) {
autoSolve();
} else {
autoSolveTimer--;
}
for(i=0; i<blockNum; i++) {
if(i == movingBlock || i == spaceBlock) {
q = dest + blocks[i].destOffset;
for(y=0; y<blockSize; y++) {
memset(q, 0, blockSize * PIXEL_SIZE);
q += video_width;
}
} else {
copyBlockImage( src + blocks[i].srcOffset,
dest + blocks[i].destOffset);
}
}
if(movingBlock >= 0) {
moveBlock(src, dest);
}
if(marginW) {
p = src + blockW * blockSize;
q = dest + blockW * blockSize;
for(y=0; y<blockH * blockSize; y++) {
memcpy(q, p, marginW * PIXEL_SIZE);
p += video_width;
q += video_width;
}
}
if(marginH) {
p = src + (blockH * blockSize) * video_width;
q = dest + (blockH * blockSize) * video_width;
memcpy(q, p, marginH * video_width * PIXEL_SIZE);
}
return 0;
}
BOOL dch(
HANDLE hCurrentProcess,
HANDLE hCurrentThread,
DWORD dwCurrentPc,
PWINDBG_EXTENSION_APIS lpExtensionApis,
LPSTR lpArgumentString)
{
PNTSD_OUTPUT_ROUTINE Print;
PNTSD_GET_EXPRESSION EvalExpression;
PNTSD_GET_SYMBOL GetSymbol;
PCOMMAND_HISTORY pCmdHist;
COMMAND_HISTORY CmdHist;
PCOMMAND pCmd;
union {
COMMAND Cmd;
WCHAR awch[80];
} DbgCmd;
int i;
char ach[120];
Print = lpExtensionApis->lpOutputRoutine;
EvalExpression = lpExtensionApis->lpGetExpressionRoutine;
GetSymbol = lpExtensionApis->lpGetSymbolRoutine;
/*
* Skip spaces
*/
while (*lpArgumentString == ' ')
lpArgumentString++;
/*
* If no COMMAND_HISTORY is specified, use something ????
*/
if (*lpArgumentString == 0) {
Print("must specify address of COMMAND_HISTORY struct\n");
Print("(a breakpoint on winsrv!AddCommand() may help)\n");
return FALSE;
}
pCmdHist = (PCOMMAND_HISTORY)EvalExpression(lpArgumentString);
move(CmdHist, pCmdHist);
Print("COMMAND_HISTORY @ 0x%lX\n", pCmdHist);
Print(" Flags = 0x%08lX%s\n",
CmdHist.Flags, GetFlags(GF_CMDHIST, CmdHist.Flags, NULL));
Print(" ListLink.F B = 0x%08lX 0x%08lX\n",
CmdHist.ListLink.Flink, CmdHist.ListLink.Blink);
DebugConvertToAnsi(hCurrentProcess,lpExtensionApis,CmdHist.AppName, ach);
Print(" AppName = %s\n", ach);
Print(" NumberOfCommands = 0x%lx\n"
" LastAdded = 0x%lx\n"
" LastDisplayed = 0x%lx\n"
" FirstCommand = 0x%lx\n"
" MaximumNumberOfCommands = 0x%lx\n",
CmdHist.NumberOfCommands,
CmdHist.LastAdded,
CmdHist.LastDisplayed,
CmdHist.FirstCommand,
CmdHist.MaximumNumberOfCommands);
Print(" ProcessHandle = 0x%08lX\n",
CmdHist.ProcessHandle);
Print(" PopupList.F B = 0x%08lX 0x%08lX\n",
CmdHist.PopupList.Flink, CmdHist.PopupList.Blink);
for (i = 0; i < CmdHist.NumberOfCommands; i++) {
move(pCmd, &(pCmdHist->Commands[i]));
// first get the count of bytes in the string...
moveBlock(DbgCmd, pCmd, sizeof(DbgCmd.Cmd.CommandLength));
// ...then get the string (and the count of bytes again)
moveBlock(DbgCmd, pCmd,
DbgCmd.Cmd.CommandLength + sizeof(DbgCmd.Cmd.CommandLength));
// DebugConvertToAnsi(hCurrentProcess,lpExtensionApis,
// DbgCmd.Cmd.Command, ach);
DbgCmd.Cmd.CommandLength /= sizeof(WCHAR);
DbgCmd.Cmd.Command[DbgCmd.Cmd.CommandLength] = L'\0';
Print(" %03d: %d chars = \"%S\"\n", i,
DbgCmd.Cmd.CommandLength,
DbgCmd.Cmd.Command);
if (i == CmdHist.LastAdded) {
Print(" (Last Added)\n");
} else if (i == CmdHist.LastDisplayed) {
Print(" (Last Displayed)\n");
} else if (i == CmdHist.FirstCommand) {
Print(" (First Command)\n");
}
}
return TRUE;
}
/***************************************************************************\
* Idso
*
* !dso StructName [FieldName] [Address]
*
* 06/17/96 Created Gerardob
\***************************************************************************/
BOOL Idso(DWORD opts, LPSTR pszCmdLine)
{
BOOL fOneField = FALSE;
DWORD dwOptions;
DWORD dwValue, dwSize, dwBytesRead, dwOffset, dwOffsetNext, dwFieldsPerRow, dwMoveSize;
DWORD dwBuffer [20]; /* Make sure it has an even number of elemnts and at least 4*/
const DWORD *pcdwLimit = dwBuffer + (sizeof(dwBuffer) / sizeof(*dwBuffer));
DWORD *pdwValue;
LPSTR pszField, pszAddress;
PBYTE pBufferOffset;
PSTRUCTURESTABLE pst;
PSTRUCTUREOFFSETSTABLE psot;
PVOID pAddress = NULL;
if (pszCmdLine == NULL) {
return FALSE;
}
/*
* NULL terminate first argument and get a pointer to
* second one (presumably the field name)
*/
/*
* Get the options, if any
*/
if (*pszCmdLine == '-') {
dwOptions = GetOpts(&pszCmdLine, NULL);
}
/*
* Find the struct table
*/
pszField = dsoTerminateString(pszCmdLine, &dwSize);
pst = dosGetStruct (pszCmdLine, dwSize);
if (pst == NULL) {
Print("Structure not found: %s\n", pszCmdLine);
return TRUE;
}
/*
* Got a table
*/
psot = pst->psot;
/*
* If there is another argument, let's assume a field name follows
*/
if (*pszField != 0) {
/*
* Find the field
*/
pszAddress = dsoTerminateString(pszField, &dwSize);
psot = dosGetField (psot, pszField, dwSize);
/*
* If it didn't find the field and an address was provided, game over.
* Otherwise, the second parameter might be the address
*/
if (psot == NULL) {
if (*pszAddress != 0) {
Print("Field not found: %s. Struct: %s\n", pszField, pst->pszName);
return TRUE;
} else {
pszAddress = pszField;
/*
* Reset psot since this argument was not a field
*/
psot = pst->psot;
}
} else {
fOneField = TRUE;
}
/*
* Get the pointer to the struct
*/
if (*pszAddress != 0) {
pAddress = EvalExp(pszAddress);
if (pAddress == NULL) {
/*
* EvalExp displayed the error message, so return silently
*/
return TRUE;
}
}
} /* if (*pszField != 0) */
/*
* If a field name was specified, dump that field only
* Otherwise, dump the whole table.
*/
if (fOneField) {
/*
* If no address available, just display the field name and offset
*/
dwOffset = dsoGetOffset(psot);
Print ("Structure %s - Size: %#lx\n", pst->pszName, pst->dwSize);
Print("Field: %s - Offset: %#lx\n", psot->pszField, dwOffset);
if (pAddress == NULL) {
return TRUE;
}
/*
* Printing field value
*/
/*123456789 1*/
Print("Address Value\n");
dwBytesRead = 0;
dwSize = dsoGetSize(psot, dwOffset);
/*
* Print 4 DWORDS per row; one row per loop
*/
do { /* while ((int)dwSize > 0) */
/*
* Read values for next row
*/
if (4 * sizeof(DWORD) >= dwSize) {
dwMoveSize = dwSize;
} else {
dwMoveSize = 4 * sizeof(DWORD);
}
moveBlock(dwBuffer, (PBYTE)pAddress + dwOffset + dwBytesRead, dwMoveSize);
pBufferOffset = (PBYTE)dwBuffer;
/*
* Print the address
*/
Print("%08lx ", (DWORD)((PBYTE)pAddress + dwOffset + dwBytesRead));
/*
* Keep track of bytes read (dwBytesRead) and bytes
* remaining to be read (dwSize)
*/
dwBytesRead += dwMoveSize;
dwSize -= dwMoveSize;
/*
* Print the values, one dword at the time
*/
while (dwMoveSize >= sizeof(DWORD)) {
Print("%08lx ", *((DWORD *)pBufferOffset));
pBufferOffset += sizeof(DWORD);
dwMoveSize -= sizeof(DWORD);
}
/*
* If less than a DWORD left, zero extend and print a DWORD
*/
if (dwMoveSize > 0) {
dwValue = 0;
memcpy(&dwValue, pBufferOffset, dwMoveSize);
Print("%0*lx", dwMoveSize * 2, dwValue);
}
Print("\n");
} while ((int)dwSize > 0);
return TRUE;
} /* if (fOneField) */
/*
* Printing all the fields.
*/
Print ("Structure %s - Size: %#lx\n", pst->pszName, pst->dwSize);
dwOffset = 0;
pBufferOffset = NULL; /* Forces the local buffer to be loaded */
dwFieldsPerRow = 0;
/*
* Loop through all fields in the table. Print one field per loop
*/
while (psot->pszField != NULL) {
/*
* Print two fields per row
*/
if (dwFieldsPerRow == 2) {
Print("\n");
dwFieldsPerRow = 1;
} else {
dwFieldsPerRow++;
}
/*
* If no address provided, Print field name(s) and offset(s) only
*/
if (pAddress == NULL) {
Print("%03lx %-34.33s", dsoGetOffset(psot), psot->pszField);
} else {
/*
* Printing offsets and values.
*
* Get the size of the value and max it to one DWORD
*/
dwOffsetNext = dsoGetOffset(psot + 1);
if (dwOffsetNext > dwOffset) {
dwSize = dwOffsetNext - dwOffset;
} else {
dwSize = dsoGetSize(psot, dwOffset);
}
if (dwSize > sizeof(DWORD)) {
dwSize = sizeof(DWORD);
}
/*
* Get a pointer to the value in the local buffer
* If the value is not in the buffer, load it
*/
pdwValue = (PDWORD)(pBufferOffset + dwOffset);
if ((pdwValue < dwBuffer) || (pdwValue + dwSize > pcdwLimit)) {
pBufferOffset = (PBYTE)dwBuffer - dwOffset;
pdwValue = dwBuffer;
if (sizeof(dwBuffer) >= pst->dwSize - dwOffset) {
dwMoveSize = pst->dwSize - dwOffset;
} else {
dwMoveSize = sizeof(dwBuffer);
}
moveBlock((PBYTE)dwBuffer, (PBYTE)pAddress + dwOffset, dwMoveSize);
}
/*
* Copy the value and print it
*/
dwValue = 0; /* in case size < sizeof(DWORD) */
memcpy(&dwValue, pdwValue, dwSize);
Print("(%03lx) %08lx %-24.23s", dwOffset, dwValue, psot->pszField);
} /* if (pAddress == NULL) */
dwOffset = dwOffsetNext;
psot++;
} /* while (psot->pszField != NULL) */
Print("\n");
return TRUE;
}
void goalCB()
{
// ---------------------------------------------------------------------------------------------
// Accept the new goal
goal_ = action_server_.acceptNewGoal();
ROS_INFO("[block logic] Received goal! %f, %s", goal_->block_size, goal_->frame.c_str());
block_size = goal_->block_size;
arm_link = goal_->frame;
if (initialized_)
{
addBlocks(msg_);
}
// --------------------------------------------------------------------------------------------
// Start pose - choose one that is preferrably not in the goal region
geometry_msgs::Pose start_pose;
bool found_pose = false;
// Check if there is only 1 block detected
if( !msg_->poses.size() )
{
// no blocks, what to do?
ROS_WARN("[block logic] No blocks found");
}
else if( msg_->poses.size() == 1 )
{
start_pose = msg_->poses[0];
found_pose = true;
ROS_INFO("[block logic] Only 1 block, using it");
}
else
{
// Search for block that meets our criteria
for(int i = 0; i < msg_->poses.size(); ++i)
{
if( msg_->poses[i].position.y > -0.12 && msg_->poses[i].position.y < 0.2 ) // start of goal region
{
start_pose = msg_->poses[i];
found_pose = true;
ROS_INFO_STREAM("[block logic] Chose this block:\n" << start_pose);
break;
}
}
if( !found_pose )
{
ROS_INFO("[block logic] Did not find a good block, default to first");
start_pose = msg_->poses[0];
found_pose = true;
}
}
// --------------------------------------------------------------------------------------------
// End pose is just chosen place on board
geometry_msgs::Pose end_pose;
end_pose.orientation = msg_->poses[0].orientation; // keep the same orientation
end_pose.position.x = 0.225;
end_pose.position.y = 0.18;
end_pose.position.z = msg_->poses[0].position.z;
// --------------------------------------------------------------------------------------------
// Move that block
if( found_pose )
{
moveBlock(start_pose, end_pose);
}
}
