void Action::parse( Filereader & f, TokenStruct< std::string > & ts, Domain & d ) {
f.next();
f.assert( ":PARAMETERS" );
f.assert( "(" );
TokenStruct< std::string > astruct = f.parseTypedList( true, d.types );
params = d.convertTypes( astruct.types );
parseConditions( f, astruct, d );
}
void TypeGround::parse( Filereader & f, TokenStruct< std::string > & ts, Domain & d ) {
f.next();
params.resize( lifted->params.size() );
for ( unsigned i = 0; i < lifted->params.size(); ++i, f.next() ) {
std::string s = f.getToken();
std::pair< bool, unsigned > p = d.types[lifted->params[i]]->parseObject( s );
if ( p.first ) params[i] = p.second;
else {
std::pair< bool, int > q = d.types[lifted->params[i]]->parseConstant( s );
if ( q.first ) params[i] = q.second;
else f.tokenExit( s );
}
}
f.assert_token( ")" );
}
void HTNMethod::parseSHOPConditions( Filereader & f, TokenStruct< std::string > & ts, Domain & d ) {
f.next();
f.assert( "(" );
if ( f.getChar() != ')' ) {
if(f.getChar() == '('){
pre = new And;
}
else
pre = createSHOPCondition( f, d );
pre->SHOPparse( f, ts, d );
}
else ++f.c;
}
void HTNMethod::SHOPparse( Filereader & f, TokenStruct< std::string > & ts, Domain & d ) {
TokenStruct< std::string > astruct = f.parseTypedList( false );
params = d.convertTypes( astruct.types );
parseSHOPConditions( f, astruct, d );
parseTaskList( f, astruct, d );
}
void HTNMethod::parseTaskList( Filereader & f, TokenStruct< std::string > & ts, Domain & d ) {
f.next();
f.assert( "(" );
for ( f.next(); f.getChar() != ')'; f.next() ) {
f.assert( "(" );
std::string s = f.getToken();
Task * task = new Task(s);
task->SHOPparse( f, ts, d );
tasks.push_back( task );
d.tasks.insert( task );
}
++f.c;
f.next();
f.assert( ")" );
}
void Derived::parse( Filereader & f, TokenStruct< std::string > & ts, Domain & d ) {
f.next();
f.assert( "(" );
name = f.getToken( d.preds );
TokenStruct< std::string > dstruct = f.parseTypedList( true, d.types );
params = d.convertTypes( dstruct.types );
f.next();
f.assert( "(" );
cond = createCondition( f, d );
cond->parse( f, dstruct, d );
f.next();
f.assert( ")" );
}
void Forall::parse( Filereader & f, TokenStruct< std::string > & ts, Domain & d ) {
f.next();
f.assert( "(" );
TokenStruct< std::string > fs = f.parseTypedList( true, d.types );
params = d.convertTypes( fs.types );
TokenStruct< std::string > fstruct( ts );
fstruct.append( fs );
f.next();
f.assert( "(" );
if ( f.getChar() != ')' ) {
cond = createCondition( f, d );
cond->parse( f, fstruct, d );
}
else ++f.c;
f.next();
f.assert( ")" );
}
int main(int argc, char** argv)
{
marker.line_list(linelist);
ros::init (argc, argv, "uncovered_regions");
ros::NodeHandle nh;
//publishers
pub_waypoint = nh.advertise<pcl::PointCloud<pcl::PointXYZ>> ("waypoints", 1);
pub_waypoint_marker = nh.advertise<visualization_msgs::Marker> ("all_nodes", 10);
pub_mesh_marker= nh.advertise<visualization_msgs::Marker> ("mesh_marker", 10);
pub_linelist = nh.advertise<visualization_msgs::Marker> ("line_list",10);
pub_covered_regions=nh.advertise<visualization_msgs::Marker>("covered_regions",10);
//subscribers
ros::Subscriber odometry_sub = nh.subscribe<nav_msgs::Odometry>("quad_sim_sysid_node/pose", 1, GetOdom);
//reading stl file
Filereader file;
string path=ros::package::getPath("koptplanner")+"/src/mesh.txt";
file.read(path,mesh_coor);
marker.triangle_list(mesh_marker);
marker.triangle_list(covered_regions);
std_msgs::ColorRGBA mesh_color;
geometry_msgs::Point mesh_point;
//define mesh color
mesh_color.r = 0 ;
mesh_color.g = 0.5 ;
mesh_color.b = 0.5 ;
mesh_color.a = 0.5;
//assigning each triangle of mesh to triangle list marker
for(int i=0;i<mesh_coor.size();i++)
{
mesh_point.x = mesh_coor[i][0];
mesh_point.y = mesh_coor[i][1];
mesh_point.z = mesh_coor[i][2];
mesh_marker.points.push_back(mesh_point);
// marker.add_point(mesh_marker,mesh_point,mesh_color);
}
mesh_marker.color=mesh_color;
path=ros::package::getPath("boiler_gazebo")+"/src/latestPath.csv";
file.read(path,csv_values);
// display results
cout.precision(2);
cout.setf(ios::fixed,ios::floatfield);
path=ros::package::getPath("koptplanner")+"/src/tour.txt";
file.read(path,tour);
// display results
cout.precision(2);
cout.setf(ios::fixed,ios::floatfield);
//waypoint marker msg basically points
marker.points(waypoint_marker_msg);
//scaling down the size
waypoint_marker_msg.scale.x = 0.1;
waypoint_marker_msg.scale.y = 0.1;
waypoint_marker_msg.scale.z = 0.1;
std_msgs::ColorRGBA color_msg_local;
waypoint_marker_msg.points.resize(csv_values.size());
waypoint_marker_msg.colors.resize(csv_values.size());
waypoint_msg.header.frame_id = "world"; //point cloud of positions
waypoint_msg.height = 1; waypoint_msg.width =csv_values.size();
for (int i=0;i<csv_values.size();i++)
{
for(int j=0;j<csv_values[i].size();j++)
{
cout<<csv_values[i][j]<<" ";
}
cout<<endl;
waypoint_msg.points.push_back(pcl::PointXYZ(csv_values[i][0],csv_values[i][1],csv_values[i][2]));
}
geometry_msgs::Point point_local;
mesh_color.r=0.5;
for(int i=0;i<csv_values.size();i++)
{
int l=csv_values[i][6]-2;
if(l>=0)
{
point_local.x=csv_values[i][0];point_local.y=csv_values[i][1];point_local.z=csv_values[i][2];
marker.add_point(linelist,point_local,mesh_color);
point_local.x=(mesh_marker.points.at(l*3).x+mesh_marker.points.at(l*3+1).x+mesh_marker.points.at(l*3+2).x)/3;
point_local.y=(mesh_marker.points.at(l*3).y+mesh_marker.points.at(l*3+1).y+mesh_marker.points.at(l*3+2).y)/3;
point_local.z=(mesh_marker.points.at(l*3).z+mesh_marker.points.at(l*3+1).z+mesh_marker.points.at(l*3+2).z)/3;
marker.add_point(linelist,point_local,mesh_color);
}
}
//dummy_msg because by erasing the waypoint we will not get correct id of covered triangle
dummy_msg.header.frame_id = "world"; //point cloud of positions
dummy_msg.height = 1; dummy_msg.width =csv_values.size();
for (int i=0;i<csv_values.size();i++)
{
dummy_msg.points.push_back(pcl::PointXYZ(csv_values[i][0],csv_values[i][1],csv_values[i][2]));
}
ros::Rate loop_rate(10.0);
while(ros::ok())
{
pub_linelist.publish(linelist);
ros::spinOnce();
loop_rate.sleep();
}
}
Map::Map(const char *map, Style* style){
this->style = style;
Filereader* reader = new Filereader(map);
Chunk *animation_data;
Chunk *dmap_data;
Chunk *chk;
while((chk = reader->getNextChunk())){
if(strncmp(chk->header->type,"ANIM",4) == 0) {
animation_data = chk;
}
if(strncmp(chk->header->type,"DMAP",4) == 0) {
dmap_data = chk;
}
}
// Read animation data
char* starting = animation_data->data;
char* offset = animation_data->data;
while((int)offset - (int)starting < animation_data->header->size )
{
TileAnimation anim;
anim.base = *reinterpret_cast<short*>(offset);
offset += sizeof(short);
anim.frame_rate = *reinterpret_cast<char*>(offset);
offset += sizeof(char);
anim.repeat = *reinterpret_cast<unsigned char*>(offset);
offset += sizeof(unsigned char);
anim.anim_length = *reinterpret_cast<char*>(offset);
offset += sizeof(char);
// unused char in file structure
offset += sizeof(char);
anim.tiles = reinterpret_cast<short*>(offset);
offset += anim.anim_length * sizeof(short);
std::vector<int> animationTiles;
for(int i=0; i<anim.anim_length;i++)
{
animationTiles.push_back(anim.tiles[i]);
}
animatedGeom[anim.base].curTile = 0;
animatedGeom[anim.base].tick = 0;
animatedGeom[anim.base].anim = anim;
animatedGeom[anim.base].animationTiles = animationTiles;
animatedGeom[anim.base+1000].curTile = 0;
animatedGeom[anim.base+1000].tick = 0;
animatedGeom[anim.base+1000].anim = anim;
animatedGeom[anim.base+1000].animationTiles = animationTiles;
tileAnimations.push_back(anim);
std::cout << anim.base << " " << (int)anim.frame_rate << " " << (int)anim.repeat << " " << (int)anim.anim_length << " " << anim.tiles[1] << " " << animationTiles.size() << std::endl;
}
// Read dmap data
CompressedMap c_map;
offset = dmap_data->data;
c_map.base = reinterpret_cast<int*>(offset);
offset += 256 * 256 * sizeof(int);
c_map.column_words = *reinterpret_cast<int*>(offset);
offset += sizeof(int);
c_map.columns = reinterpret_cast<int*>(offset);
offset += c_map.column_words * sizeof(int);
c_map.num_blocks = *reinterpret_cast<int*>(offset);
offset += sizeof(int);
c_map.blocks = reinterpret_cast<BlockInfo*>(offset);
offset += c_map.num_blocks * sizeof(BlockInfo);
for(int x=0;x<255;x++) for(int y=0;y<255;y++)
{
int base = c_map.base[y*256+x];
ColInfo* column = reinterpret_cast<ColInfo*>(c_map.columns + base);
int i = 0;
for(i=0;i<(column->height-column->offset);i++){
this->addBlock(c_map.blocks[column->blockd[i]], Vector3(x, -y, i+column->offset));
}
}
delete reader;
GLenum FBOstatus;
glGenTextures(1, &depthTextureId);
glBindTexture(GL_TEXTURE_2D, depthTextureId);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP );
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP );
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_COMPARE_R_TO_TEXTURE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
glTexParameteri(GL_TEXTURE_2D, GL_DEPTH_TEXTURE_MODE, GL_INTENSITY);
glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, shadowMapWidth, shadowMapHeight, 0, GL_DEPTH_COMPONENT, GL_UNSIGNED_BYTE, 0);
glBindTexture(GL_TEXTURE_2D, 0);
glGenFramebuffers(1, &fboId);
glBindFramebuffer(GL_FRAMEBUFFER, fboId);
glDrawBuffer(GL_NONE);
glReadBuffer(GL_NONE);
glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depthTextureId, 0);
FBOstatus = glCheckFramebufferStatus(GL_FRAMEBUFFER);
if(FBOstatus != GL_FRAMEBUFFER_COMPLETE)
{
std::cout << "FBO error: " << FBOstatus << std::endl;
}
glBindFramebufferEXT(GL_FRAMEBUFFER, 0);
}
void Action::parseConditions( Filereader & f, TokenStruct< std::string > & ts, Domain & d ) {
f.next();
f.assert( ":" );
std::string s = f.getToken();
if ( s == "PRECONDITION" ) {
f.next();
f.assert( "(" );
if ( f.getChar() != ')' ) {
pre = createCondition( f, d );
pre->parse( f, ts, d );
}
else ++f.c;
f.next();
f.assert( ":" );
s = f.getToken();
}
if ( s != "EFFECT" ) f.tokenExit( s );
f.next();
f.assert( "(" );
if ( f.getChar() != ')' ) {
eff = createCondition( f, d );
eff->parse( f, ts, d );
}
else ++f.c;
f.next();
f.assert( ")" );
}
void Forall::SHOPparse( Filereader & f, TokenStruct< std::string > & ts, Domain & d ) {
f.next();
f.assert( "(" );
TokenStruct< std::string > fs = f.parseTypedList( false );
params = d.convertTypes( fs.types );
TokenStruct< std::string > fstruct( ts );
fstruct.append( fs );
f.next();
f.assert( "(" );
if ( f.getChar() != ')' ) {
cond = new And;
cond->SHOPparse( f, fstruct, d );
}
else ++f.c;
f.next();
f.assert( "(" );
if ( f.getChar() != ')' ) {
cond1 = new And;
cond1->SHOPparse( f, fstruct, d );
}
else ++f.c;
f.next();
f.assert( ")" );
}
