void usArc2nsArc( FA *fa )
{
UNIFYARC *uptr;
UNIFYARC *disused_uptr;
ARC *nptr;
ARC *newarc;
uptr = fa->usList;
while( uptr != NULL ){
if( (newarc = calloc( 1, sizeof(ARC) )) == NULL ){
errMes( "Can't alloc forward arc buffer of finite automaton." );
}
connectFA( fa, uptr->inp, uptr->us, uptr->accpt, uptr->start );
uptr = uptr->next;
}
uptr = fa->usList;
while( uptr != NULL ){
if( uptr->reserved ){
uptr->us->accpt |= fa->accpt;
nptr = fa->nsList;
while( nptr != NULL ){
connectFA( uptr->us, nptr->inp, nptr->fa, nptr->accpt, nptr->start );
nptr = nptr->next;
}
}
disused_uptr = uptr;
uptr = uptr->next;
free( disused_uptr );
}
}
void makeTriplet( void )
{
FILE *fp_fa;
FA *fa;
FAprocessed = 0;
if( (fp_fa = fopen( FAfile, "w" )) == NULL ){
errMes( "Can't open dfa File for writting\"%s\"", FAfile );
}
getNewStatNo( FAlist );
if( !SW_Quiet ){
fprintf( stderr, "Now making triplet list" );
NoNewLine = 1;
}
while( 1 ){
if( (fa = processTripletQueue( NULL )) == NULL ) break;
r_makeTriplet( fa, fp_fa );
}
fclose( fp_fa );
if( !SW_Quiet ){
fprintf( stderr, "\rNow making triplet list[%d/%d]\n", FAprocessed, FAtotal );
NoNewLine = 0;
}
if( SW_Verbose ){
verboseMes( "r_makeTriplet: %d/%d(%d%%)",
TFAtravSuccess, TFAtravTotal, 100*TFAtravSuccess/TFAtravTotal);
}
newLineAdjust();
}
BODY *appendTerm( BODY *list, char *name )
{
BODY *newTerm;
if( (newTerm = malloc( sizeof(BODY) )) == NULL ){
errMes( "Can't alloc term list buffer" );
}
strcpy( newTerm->name, name );
newTerm->abort = 0;
newTerm->next = list;
return( newTerm );
}
void setVoca( void )
{
char token1[ SYMBOL_LEN ];
char token2[ SYMBOL_LEN ];
int virgin = 1;
int bodynum = 0;
BODY *bodyList = NULL;
FILE *fp;
char identifier[ SYMBOL_LEN ] = "";
if( (fp = fopen( VocaFile, "r" )) == NULL ){
errMes( "Can't open vocabulary file\"%s\"", VocaFile );
}
if( !SW_Quiet ){
newLineAdjust();
fputs( "Now parsing vocabulary file\n", stderr );
}
while( 1 ){
static char line[ 1000 ];
char *ptr = line;
if( fgets( line, 1000, fp ) == NULL ){
entryTerm( identifier, bodyList, bodynum );
break;
}
if( line[ 0 ] == '\0' ) continue;
if( line[ 0 ] == '#' ){
if( (ptr = gettoken( ptr, token1 )) == NULL ) continue;
if( !virgin ){
entryTerm( identifier, bodyList, bodynum );
bodyList = NULL;
bodynum = 0;
} else {
virgin = 0;
}
strcpy( identifier, token1 + 1 );
continue;
} else {
ptr = gettoken( ptr, token1 );
if( ptr == NULL ) continue;
ptr = gettoken( ptr, token2 );
if( ptr == NULL ){
bodyList = appendTerm( bodyList, token1 );
} else {
bodyList = appendTerm( bodyList, token2 );
}
bodynum++;
}
}
}
FALIST *volatileFA( FALIST *volatileList, FA *fa )
{
FALIST *atom;
if( (atom = malloc( sizeof(FALIST) )) == NULL ){
errMes( "Can't alloc FA list buffer." );
}
fa->volatiled = 1;
atom->fa = fa;
atom->next = volatileList;
return( atom );
}
DirectionSensor::DirectionSensor(::gazebo::physics::ModelPtr model, sdf::ElementPtr sensor,
std::string partId, std::string sensorId):
Sensor(model, sensor, partId, sensorId, 1) // last parameter is the number of input neurons this sensor generates
{
this->sensor_ = boost::dynamic_pointer_cast<::gz::sensors::DirectionSensorDummy>(this->sensor_);
if (!this->sensor_)
{
auto errmsg = "DirectionSensor requires a DirectionSensorDummy as its parent.";
std::cerr << errmsg << std::endl;
throw std::invalid_argument(errmsg);
}
this->sensor_->SetActive(true);
this->output_ = 0.0;
// Create transport node
node_.reset(new gz::transport::Node());
node_->Init();
// subscribe to sound plugin messages
soundPluginSub_ = node_->Subscribe("~/revolve/drive_direction_update", &DirectionSensor::OnDirUpdate, this);
// connect to the update signal
this->updateConnection_ = this->sensor_->ConnectUpdated(
boost::bind(&DirectionSensor::OnUpdate, this, this->sensor_));
// this is the vector that represents the drive direction
this->driveDirection_ = ::gz::math::Vector3(0, 0, 0);
// this is the relative pose of this sensor in the coordinate system of the parent link (never changes)
this->sensorPose_ = this->sensor_->GetPose();
// this is the sensor axis in the link coordinate system (never changes)
this->sensorAxis_ = this->sensorPose_.rot.RotateVector(::gz::math::Vector3(0, 0, 1));
// name of the parent link
std::string parentLinkName = this->sensor_->GetParentName();
// ptr to the parent link
this->linkPtr_ = this->model_->GetLink( parentLinkName );
if (!(this->linkPtr_)) {
std::string errMes("Sound sensor: could not find the link: ");
errMes.append(parentLinkName);
throw std::runtime_error(errMes);
}
}
FALIST *insertFAlist( FALIST *top, FALIST *preAtom, FALIST *nextAtom, FA *fa )
{
FALIST *atom;
if( (atom = malloc( sizeof(FALIST) )) == NULL ){
errMes( "Can't alloc group buffer for unifying FA" );
}
atom->fa = fa;
if( preAtom == NULL ){
atom->next = nextAtom;
return( atom );
} else {
preAtom->next = atom;
atom->next = nextAtom;
return( top );
}
}
FA *processTripletQueue( FA *fa )
{
/* NULL:pop, !NULL:push */
typedef struct _FAQ{
FA *fa;
struct _FAQ *next;
} FAQ;
static FAQ *queueTop = NULL;
static FAQ *queuqTail = NULL;
FAQ *newFAQ;
if( fa != NULL ){
if( (newFAQ = malloc( sizeof(newFAQ) )) == NULL ){
errMes( "Can't malloc queue for breadth-first search of triplet list" );
}
newFAQ->fa = fa;
newFAQ->next = NULL;
if( queueTop == NULL ){
queueTop = queuqTail = newFAQ;
return( NULL );
} else {
queuqTail->next = newFAQ;
queuqTail = newFAQ;
return( NULL );
}
} else {
if( queueTop != NULL ){
FAQ *popedFAQ = queueTop;
FA *popedFA = queueTop->fa;
queueTop = queueTop->next;
free( popedFAQ );
return( popedFA );
} else {
return( NULL );
}
}
}
void connectUnifyFA( FA *fa, int inp, FA *nextFA, FLAG reserved,
CLASSFLAGS accpt, CLASSFLAGS start )
{
/* unifyFAへのアークのリストに入力の辞書順で適切位置に挿入
また同じものがある場合登録しない */
/* nextFA のpsNumをインクリメントしない */
UNIFYARC *newArc;
UNIFYARC *curArc = NULL;
UNIFYARC *nextArc;
UNIFYARC *top = fa->usList;
if( (newArc = calloc( 1, sizeof(UNIFYARC) )) == NULL ){
errMes( "Can't alloc forward arc buffer of finite automaton." );
}
newArc->inp = inp;
newArc->us = nextFA;
newArc->reserved = reserved;
newArc->accpt = accpt;
newArc->start = start;
if( (nextArc = top) != NULL ){
while( 1 ){
if( nextArc->inp > inp ) break;
if( nextArc->inp == inp && nextArc->us == nextFA ) return;
curArc = nextArc;
if( (nextArc = nextArc->next) == NULL ) break;
}
}
if( curArc == NULL ){
newArc->next = top;
fa->usList = newArc;
} else {
newArc->next = nextArc;
curArc->next = newArc;
}
}
