void ProcInvoke( void )
{
char *fstart;
unsigned flen;
char *start;
char_ring *parmlist;
char_ring **owner;
char_ring *path;
unsigned len;
if( !ScanItem( TRUE, &fstart, &flen ) ) Error( ERR_LOC, LIT( ERR_WANT_FILENAME ) );
parmlist = NULL;
owner = &parmlist;
while( !ScanEOC() ) {
ScanItem( TRUE, &start, &len );
_Alloc( path, sizeof( char_ring ) + len );
if( path == NULL ) {
FreeRing( parmlist );
Error( ERR_NONE, LIT( ERR_NO_MEMORY ) );
}
memcpy( path->name, start, len );
path->name[ len ] = NULLCHAR;
path->next = NULL;
*owner = path;
owner = &path->next;
}
Invoke( fstart, (int)flen, parmlist );
}
void SourceSet( void )
{
char_ring **owner;
char *start;
unsigned len;
if( CurrToken == T_DIV ) {
Scan();
if( ScanCmd( AddTab ) == 0 ) {
Error( ERR_LOC, LIT( ERR_BAD_SUBCOMMAND ), GetCmdName( CMD_SET ) );
}
owner = RingEnd( &SrcSpec );
} else {
owner = &SrcSpec;
FiniSource();
}
while( ScanItem( TRUE, &start, &len ) ) {
while( len > 0 && *start == ' ' ) {
++start;
--len;
}
InsertRing( owner, start, len );
}
DbgUpdate( UP_NEW_SRC );
}
OVL_EXTERN brkp *ImageBreak( memory_expr def_seg )
{
const char *start;
size_t len;
bool clear = false;
int cmd;
def_seg=def_seg;
while( CurrToken == T_DIV ) {
Scan();
cmd = ScanCmd( PointNameTab );
if( cmd < 0 )
break;
switch( cmd ) {
case B_CLEAR:
clear = true;
break;
default:
Error( ERR_LOC, LIT_ENG( ERR_BAD_OPTION ), GetCmdName( CMD_BREAK ) );
break;
}
}
if( !ScanItem( true, &start, &len ) ) {
BadPoint( def_seg );
}
BreakOnImageLoad( start, len, clear );
Scan();
ReqEOC();
return( NULL );
}
OVL_EXTERN brkp *ImageBreak( memory_expr def_seg )
{
char *start;
size_t len;
bool clear = FALSE;
def_seg=def_seg;
while( CurrToken == T_DIV ) {
Scan();
switch( ScanCmd( PointNameTab ) ) {
case 0:
goto done;
case B_CLEAR:
clear = TRUE;
break;
default:
Error( ERR_LOC, LIT( ERR_BAD_OPTION ), GetCmdName( CMD_BREAK ) );
break;
}
}
done:;
if( !ScanItem( TRUE, &start, &len ) ) {
BadPoint( def_seg );
}
BreakOnImageLoad( start, len, clear );
Scan();
ReqEOC();
return( NULL );
}
extern void ProcCapture( void )
{
cmd_list *cmds;
const char *start;
size_t len;
const char *old;
char *p;
char *end;
if( !ScanItem( false, &start, &len ) )
Error( ERR_NONE, LIT_ENG( ERR_WANT_COMMAND_LIST ) );
ReqEOC();
cmds = AllocCmdList( start, len );
end = FileName + sizeof( FileName );
p = FileName + PREFIX_LEN;
p = CnvULongHex( TaskId, p, end - p );
*p++ = RemFile.ext_separator;
p = CnvULongDec( FileNum++, p, end - p );
*p = NULLCHAR;
old = ReScan( FileName+PREFIX_LEN );
StdOutNew();
ReScan( old );
cmds->use++;
CaptureOk = true;
PushInpStack( cmds, DoneCapture, false );
TypeInpStack( INP_CAPTURED );
FreeCmdList( cmds );
}
static void LangSet( void )
{
const char *start;
size_t len;
ScanItem( TRUE, &start, &len );
ReqEOC();
NewLang( start );
}
static char *ScanName( void )
/***************************/
{
char *start;
unsigned len;
ScanItem( TRUE, &start, &len );
memcpy( TxtBuff, start, len );
TxtBuff[len] = '\0';
return( TxtBuff );
}
void ProcError( void )
{
char *start;
unsigned len;
ScanItem( FALSE, &start, &len );
ReqEOC();
memcpy( TxtBuff, start, len );
TxtBuff[ len ] = NULLCHAR;
Error( ERR_NONE, LIT( ERR_GENERIC ), TxtBuff );
}
void ProcError( void )
{
const char *start;
size_t len;
ScanItem( false, &start, &len );
ReqEOC();
memcpy( TxtBuff, start, len );
TxtBuff[len] = NULLCHAR;
Error( ERR_NONE, LIT_ENG( ERR_GENERIC ), TxtBuff );
}
static gui_menu_struct *FindMainMenu( gui_menu_struct *main, int size )
{
char *start;
unsigned len;
if( !ScanItem( TRUE, &start, &len ) ) return( NULL );
while( --size >= 0 ) {
if( StrAmpEqual( start, main->label, len ) ) break;
++main;
}
if( size < 0 ) return( NULL );
return( main );
}
OVL_EXTERN void GetModName( void *value )
{
mod_handle mod;
char *start;
size_t len;
RawScanInit();
ScanItem( FALSE, &start, &len );
mod = LookupModName( NO_MOD, start, len );
if( mod == NO_MOD ) {
Error( ERR_NONE, LIT( ERR_NO_MODULE ), start, len );
}
Scan();
ReqEOC();
*(mod_handle *)value = mod;
}
void WndDumpFile( a_window *wnd )
{
const char *start;
size_t len;
bool got;
got = ScanItem( TRUE, &start, &len );
ReqEOC();
if( !got ) {
WndDumpPrompt( wnd );
} else {
memcpy( TxtBuff, start, len );
TxtBuff[len] = '\0';
DoWndDumpFile( TxtBuff, wnd );
}
}
void ProcSystem()
{
char *start;
size_t len;
int loc;
loc = 0;
if( CurrToken == T_DIV ) {
Scan();
loc = ScanCmd( SystemOps );
if( loc == 0 ) {
Error( ERR_LOC, LIT( ERR_BAD_OPTION ), GetCmdName( CMD_SYSTEM ) );
}
if( loc == 2 ) loc = -1;
}
ScanItem( FALSE, &start, &len );
ReqEOC();
DoSystem( start, len, loc );
}
void ProcFont( void )
{
wnd_class_wv wndclass;
const char *start;
size_t len;
wnd_class_wv wndclass1;
wndclass = ReqWndName();
if( !ScanItem( false, &start, &len ) )
return;
ReqEOC();
if( wndclass == WND_ALL ) {
for( wndclass1 = 0; wndclass1 < WND_NUM_CLASSES; ++wndclass1 ) {
SetFont( wndclass1, NULL );
}
}
SetFont( wndclass, GUIDupStrLen( start, len ) );
_SwitchOn( SW_PENDING_REPAINT );
}
static void OpenLog( open_access mode )
{
const char *start;
size_t len;
if( LogHndl != NIL_HANDLE ) {
Error( ERR_NONE, LIT_ENG( ERR_LOG_STARTED ) );
} else if( !ScanItem( TRUE, &start, &len ) ) {
Error( ERR_LOC, LIT_ENG( ERR_WANT_FILENAME ) );
} else {
if( len > TXT_LEN ) len = TXT_LEN;
ReqEOC();
memcpy( TxtBuff, start, len );
TxtBuff[len] = NULLCHAR;
LogHndl = FileOpen( TxtBuff, mode );
if( LogHndl == NIL_HANDLE ) {
Error( ERR_NONE, LIT_ENG( ERR_FILE_NOT_OPEN ), TxtBuff );
}
}
}
static brkp *SetPoint( memory_expr def_seg, mad_type_handle th )
{
brkp *bp;
const char *start;
size_t len;
address loc;
cmd_list *cmds;
char *condition;
long countdown;
bool resume;
bool active;
int index;
mad_type_info mti;
unsigned old;
bool unmapped;
bool mapaddress;
bool symaddress;
char *image_name;
char *mod_name;
char *sym_name;
long cue_diff;
long addr_diff;
int cmd;
resume = false;
index = 0;
active = true;
unmapped = false;
mapaddress = false;
symaddress = false;
while( CurrToken == T_DIV ) {
Scan();
cmd = ScanCmd( PointNameTab );
if( cmd < 0 )
break;
switch( cmd ) {
case B_RESUME:
resume = true;
break;
case B_UNRESUME:
resume = false;
break;
case B_ACTIVATE:
active = true;
break;
case B_DEACTIVATE:
active = false;
break;
case B_UNMAPPED:
unmapped = true;
break;
case B_MAPADDRESS:
mapaddress = true;
ScanItem( true, &start, &len );
image_name = DupStrLen( start, len );
loc.mach.segment = ReqLongExpr();
loc.mach.offset = ReqLongExpr();
ReqComma();
break;
case B_SYMADDRESS:
symaddress = true;
ScanItem( true, &start, &len );
image_name = DupStrLen( start, len );
ScanItem( true, &start, &len );
mod_name = DupStrLen( start, len );
ScanItem( true, &start, &len );
sym_name = DupStrLen( start, len );
cue_diff = ReqLongExpr();
addr_diff = ReqLongExpr();
loc = NilAddr;
ReqComma();
break;
case B_INDEX:
old = NewCurrRadix( 10 );
index = ReqExpr();
NewCurrRadix( old );
ReqComma();
break;
/* fall thru */
default:
Error( ERR_LOC, LIT_ENG( ERR_BAD_OPTION ), GetCmdName( CMD_BREAK ) );
break;
}
}
if( !unmapped ) {
ReqMemAddr( def_seg, &loc );
}
for( bp = BrkList; bp != NULL; bp = bp->next ) {
if( AddrComp( bp->loc.addr, loc ) == 0 ) {
Error( ERR_NONE, LIT_ENG( ERR_POINT_EXISTS ) );
}
}
cmds = NULL;
condition = NULL;
countdown = 0;
if( ScanQuote( &start, &len ) ) {
if( len != 0 )
cmds = AllocCmdList( start, len );
if( ScanQuote( &start, &len ) ) {
if( len != 0 )
condition = DupStrLen( start, len );
if( !ScanEOC() ) {
countdown = ReqExpr();
}
}
}
ReqEOC();
if( !IS_BP_EXECUTE( th ) ) {
MADTypeInfo( th, &mti );
switch( mti.b.bits / BITS_PER_BYTE ) {
case 1:
case 2:
case 4:
break;
case 8:
if( Is8ByteBreakpointsSupported() )
break;
default:
Error( ERR_NONE, LIT_ENG( ERR_NOT_WATCH_SIZE ) );
break;
}
}
bp = AddPoint( loc, th, unmapped );
if( bp == NULL )
return( NULL );
bp->status.b.unmapped = unmapped;
if( mapaddress ) {
bp->loc.image_name = image_name;
}
if( symaddress ) {
bp->image_name = image_name;
bp->mod_name = mod_name;
bp->sym_name = sym_name;
bp->cue_diff = cue_diff;
bp->addr_diff = addr_diff;
}
bp->cmds = cmds;
if( cmds != NULL )
bp->status.b.use_cmds = true;
bp->condition = condition;
if( condition != NULL )
bp->status.b.use_condition = true;
SetBPCountDown( bp, countdown );
bp->status.b.resume = resume;
bp->status.b.active = active;
if( index != 0 )
bp->index = index;
RecordBreakEvent( bp, B_SET );
return( bp );
}
void scanCB(const sensor_msgs::LaserScanConstPtr& scan_in) {
std::cout << "Scan received" << std::endl;
std::vector<ScanItem> items;
bool startItem = false;
int hitcounter = 0;
ScanItem tmpItem;
int d = 5;
for (int i = 0; i < scan_in->ranges.size() ; i++) {
if (scan_in->ranges[i] > 2) {
if(hitcounter > 0) {
hitcounter--;
}
if(hitcounter == 0 && startItem==true) {
startItem=false;
tmpItem.end_angle = scan_in->angle_min + scan_in->angle_increment * (i-d);
items.push_back(tmpItem);
tmpItem = ScanItem();
}
continue;
}
hitcounter++;
if (hitcounter > d && startItem==false) {
startItem = true;
tmpItem.start_angle = scan_in->angle_min + scan_in->angle_increment * (i-d);
hitcounter = d;
}
if (startItem) {
//std::cout << "distance: " << scan_in->ranges[i] << std::endl;
tmpItem.mean_distance = (tmpItem.mean_distance * tmpItem.counter + scan_in->ranges[i]) / (tmpItem.counter + 1);
tmpItem.counter++;
}
if (hitcounter > d) {
hitcounter = d;
}
//std::cout << "Reading at: " << scan_in->angle_min + scan_in->angle_increment * i
// << " at distance: " << scan_in->ranges[i] <<"m"<<std::endl;
}
std::cout << "Found " << items.size() << " items" << std::endl;
for (int i=0; i<items.size(); i++) {
ScanItem it = items[i];
std::cout << "Item at yawangle: " << it.angle()
<< " at distance: " << it.mean_distance << "m" << std::endl;
}
if (items.size() > 0) {
ScanItem it = items[0];
geometry_msgs::Twist cmd;
if (std::abs(it.angle()) > 0.05) {
cmd.angular.z = it.angle();
cmd_pub.publish(cmd);
} else if (it.mean_distance > 0.2) {
cmd.linear.x = it.mean_distance * 3;
if (cmd.linear.x > 0.2) {
cmd.linear.x = 0.2;
}
cmd_pub.publish(cmd);
}
}
//move towards 1st item
}
