Skip to content

Commit

Permalink
Adding home
Browse files Browse the repository at this point in the history
  • Loading branch information
@jcobano
jcobano committed Oct 28, 2024
1 parent d9e32d5 commit bdeaa3a
Show file tree
Hide file tree
Showing 5 changed files with 120 additions and 22 deletions.
102 changes: 92 additions & 10 deletions include/Grid3D/grid3d.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -389,9 +389,16 @@ bool loadSemanticGrid() {
m_octomap->getMetricMin(minX, minY, minZ);
m_octomap->getMetricMax(maxX, maxY, maxZ);
res = m_octomap->getResolution();

m_maxX = (float)(maxX-minX);
m_maxY = (float)(maxY-minY);
m_maxZ = (float)(maxZ-minZ);

// ATLAS - 1 Floor - Close to origin
// m_maxX = (float)(maxX-minX)/2;
// m_maxY = (float)(maxY-minY)/2;
// m_maxZ = (float)(maxZ-minZ)/2;
// m_maxZ = m_maxZ + 1;
m_resolution = (float)res;
m_oneDivRes = 1.0/m_resolution;
ROS_INFO("Map size:\n");
Expand Down Expand Up @@ -558,6 +565,8 @@ bool loadSemanticGrid() {
m_gridSizeX = (int)(m_maxX*m_oneDivRes);
m_gridSizeY = (int)(m_maxY*m_oneDivRes);
m_gridSizeZ = (int)(m_maxZ*m_oneDivRes);
// ATLAS 1 Floor - Close to origin
// m_gridSizeZ = (int)(m_maxZ*m_oneDivRes)+1;

// CONSIDERING DIMENSIONES OF THE SEMANTIC LAYER
// m_gridSizeX = (int)(212*m_oneDivRes); //1060
Expand Down Expand Up @@ -632,7 +641,7 @@ bool loadSemanticGrid() {
// }
// }

// VERSION OF 2024-MARCH
// VERSION OF 2024-MARCH WITH ATLAR FAR
for(int iz=0; iz<(m_gridSizeZ-2); iz++) //m_gridSizeZ-2?
{
for(int iy=0; iy<(m_gridSizeY-3); iy++) //m_gridSizeY-3?
Expand All @@ -644,33 +653,100 @@ bool loadSemanticGrid() {
}
}
}
// VERSION ATLAS CLOSE TO THE ORIGIN
// for(int iz=0; iz<(m_gridSizeZ); iz++) //m_gridSizeZ-2?
// {
// for(int iy=0; iy<(m_gridSizeY); iy++) //m_gridSizeY-3?
// {
// for(int ix=0; ix<(m_gridSizeX); ix++)
// {
// index = ix + iy*m_gridStepY + iz*m_gridStepZ;
// m_grid[index].semantic = 0;
// }
// }
// }

// int count_semant=0;
for(int i_seman=0; i_seman<index_size_semantic; i_seman++)
{
// std::cout << "semanticGrid_x: " << semanticGrid_x[i_seman] << std::endl;
index_semantic=semanticGrid_x[i_seman];//Correcto
// std::cout << "index_semantic: " << index_semantic << std::endl;
if (semanticGrid_v[i_seman] == 1)
if (semanticGrid_v[i_seman] == 1){
c1=c1+1;
else if (semanticGrid_v[i_seman] == 2)
// std::cout << "semantic: 1" << std::endl;
// usleep(1e4);
// std::cout << "Please a key to go to the next iteration..." << std::endl;
// getchar(); // Comentar para no usar tecla.
}
else if (semanticGrid_v[i_seman] == 2){
c2=c2+1;
else if (semanticGrid_v[i_seman] == 3)
// std::cout << "semantic: 2" << std::endl;
// usleep(1e4);
// std::cout << "Please a key to go to the next iteration..." << std::endl;
// getchar(); // Comentar para no usar tecla.
}
else if (semanticGrid_v[i_seman] == 3){
c3=c3+1;
else if (semanticGrid_v[i_seman] == 4)
// std::cout << "semantic: 3" << std::endl;
// usleep(1e4);
// std::cout << "Please a key to go to the next iteration..." << std::endl;
// getchar(); // Comentar para no usar tecla.
}
else if (semanticGrid_v[i_seman] == 4){
c4=c4+1;
else if (semanticGrid_v[i_seman] == 5)
// std::cout << "semantic: 1" << std::endl;
// usleep(1e4);
// std::cout << "Please a key to go to the next iteration..." << std::endl;
// getchar(); // Comentar para no usar tecla.
}
else if (semanticGrid_v[i_seman] == 5){
c5=c5+1;
else if (semanticGrid_v[i_seman] == 6)
// std::cout << "semantic: 1" << std::endl;
// usleep(1e4);
// std::cout << "Please a key to go to the next iteration..." << std::endl;
// getchar(); // Comentar para no usar tecla.
}
else if (semanticGrid_v[i_seman] == 6){
c6=c6+1;
else if (semanticGrid_v[i_seman] == 7)
// std::cout << "semantic: 1" << std::endl;
// usleep(1e4);
// std::cout << "Please a key to go to the next iteration..." << std::endl;
// getchar(); // Comentar para no usar tecla.
}
else if (semanticGrid_v[i_seman] == 7){
c7=c7+1;
else if (semanticGrid_v[i_seman] == 8)
// std::cout << "semantic: 1" << std::endl;
// usleep(1e4);
// std::cout << "Please a key to go to the next iteration..." << std::endl;
// getchar(); // Comentar para no usar tecla.
}
else if (semanticGrid_v[i_seman] == 8){
c8=c8+1;
else
// std::cout << "semantic: 1" << std::endl;
// usleep(1e4);
// std::cout << "Please a key to go to the next iteration..." << std::endl;
// getchar(); // Comentar para no usar tecla.
}
else{
c0=c0+1;
// std::cout << "semantic: 1" << std::endl;
// usleep(1e4);
// std::cout << "Please a key to go to the next iteration..." << std::endl;
// getchar(); // Comentar para no usar tecla.
}


m_grid[index_semantic].semantic = semanticGrid_v[i_seman];

// if (m_grid[index_semantic].semantic == 3){
// std::cout << "index: " << index_semantic << std::endl;
// std::cout << "semantic: " << m_grid[index_semantic].semantic << std::endl;
// usleep(1e4);
// std::cout << "Please a key to go to the next iteration..." << std::endl;
// getchar(); // Comentar para no usar tecla.
// }


// Check if index_semantic is out world_size
if (index_semantic > m_gridSize)
Expand Down Expand Up @@ -706,6 +782,12 @@ bool loadSemanticGrid() {
}
// std::cout << "count_semant: " << count_semant << std::endl;

std::cout << "semantic: " << semanticGrid_v[100] << std::endl;
std::cout << "indice: " << semanticGrid_x[100] << std::endl;
std::cout << "Y: " << semanticGrid_y[100] << std::endl;
std::cout << "Z: " << semanticGrid_z[100] << std::endl;


for(int iz=0; iz<m_gridSizeZ; iz++)
{
for(int iy=0; iy<m_gridSizeY; iy++)
Expand Down
9 changes: 7 additions & 2 deletions launch/grid3d.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,20 @@
<param name="world_size_y" value="169.2" />
<param name="world_size_z" value="9.2" />
<param name="resolution" value="0.2" /> -->
<!-- ATLAS - 1 FLOOR - CLOSE TO ORIGIN -->
<param name="world_size_x" value="186.0" />
<param name="world_size_y" value="45.2" />
<param name="world_size_z" value="8.4" />
<param name="resolution" value="0.2" />
<!-- Casa CON muebles -->
<!-- <param name="world_size_x" value="169.2" />
<param name="world_size_y" value="102.2" />
<param name="world_size_z" value="71.0" />
<param name="resolution" value="0.2" /> -->
<!-- Casa SIN muebles -->
<param name="world_size_x" value="170.4" />
<!-- <param name="world_size_x" value="170.4" />
<param name="world_size_y" value="107.4" />
<param name="world_size_z" value="71.2" />
<param name="resolution" value="0.2" />
<param name="resolution" value="0.2" /> -->
</node>
</launch>
20 changes: 13 additions & 7 deletions launch/simulator_atlas.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,18 +23,19 @@
<!-- ATLAS -->
<!-- <arg name="map_name" default="Atlas_8_300"/> -->

<!-- <arg name="map_name" default="Atlas_8_puertas_02"/> -->
<arg name="map_name" default="Atlas_8_puertas_02"/>
<!-- <arg name="map_name" default="atlas_revit1000000000"/> -->
<!-- <arg name="map_name" default="casa_reducida5"/> -->
<!-- <arg name="map_name" default="casa_5mill"/> -->
<arg name="map_name" default="casoplonv2_jac_5mill"/>
<!-- <arg name="map_name" default="casoplonv2_jac_5mill"/> -->

<!-- <arg name="map_name" default="Atlas_8_floor_1kk"/> -->
<!-- <arg name="map_name" default="nube_de_puntos2"/> -->
<!-- <arg name="map_name" default="nube_de_puntos_con_etiquetas"/> -->
<arg name="map" default="$(find heuristic_planners)/resources/3dmaps/$(arg map_name).bt"/>

<!-- <arg name="algorithm_name" default="costlazythetastar"/> -->
<arg name="algorithm_name" default="costastar"/>
<arg name="algorithm_name" default="lazythetastar_semantic_cost"/>
<!-- <arg name="algorithm_name" default="costastar"/> -->

<!-- 212 -->
<!-- <arg name="world_size_x" default="211.8"/> -->
Expand All @@ -48,9 +49,14 @@
<arg name="resolution" default="0.2"/> -->

<!-- Atlas -->
<!-- <arg name="world_size_x" default="211.8"/>
<arg name="world_size_x" default="211.8"/>
<arg name="world_size_y" default="169.2"/>
<arg name="world_size_z" default="9.2"/>
<arg name="resolution" default="0.2"/>
<!-- Atlas 1 Floor - Close to origin-->
<!-- <arg name="world_size_x" default="186.0"/>
<arg name="world_size_y" default="45.2"/>
<arg name="world_size_z" default="8.4"/>
<arg name="resolution" default="0.2"/> -->

<!-- Casa CON muebles -->
Expand All @@ -60,10 +66,10 @@
<arg name="resolution" default="0.2"/> -->

<!-- Casa SIN muebles-->
<arg name="world_size_x" default="170.4"/>
<!-- <arg name="world_size_x" default="170.4"/>
<arg name="world_size_y" default="107.4"/>
<arg name="world_size_z" default="71.2"/>
<arg name="resolution" default="0.2"/>
<arg name="resolution" default="0.2"/> -->

<arg name="inflate_map" default="true"/>
<arg name="inflation_size" default="$(arg resolution)"/>
Expand Down
9 changes: 7 additions & 2 deletions scripts/grid3d_class.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,8 @@ def __init__(self):

rospack = rospkg.RosPack()
# model_path = str(rospack.get_path('heuristic_planners') )+ str("/scripts/model.ifc")
model_path = str(rospack.get_path('heuristic_planners') )+ str("/scripts/casoplonv2.ifc")
model_path = str(rospack.get_path('heuristic_planners') )+ str("/scripts/atlas_1F.ifc")
# model_path = str(rospack.get_path('heuristic_planners') )+ str("/scripts/casoplonv2.ifc")
self.model = ifcopenshell.open(model_path)
self.settings = ifcopenshell.geom.settings()
self.settings.set(self.settings.USE_WORLD_COORDS, True)
Expand Down Expand Up @@ -111,6 +112,9 @@ def __init__(self):


# Verifica si el valor mínimo es negativo y suma su valor absoluto a todos los elementos
self.semantic_grid_x -= abs(min_x)
self.semantic_grid_y -= abs(min_y)
self.semantic_grid_z -= abs(min_z)
if min_x < 0:
print(f'Hay offset de X: {min_x}')
self.semantic_grid_x += abs(min_x)
Expand Down Expand Up @@ -142,7 +146,8 @@ def __init__(self):
self.semantic_grid_x[i] = int(index)



print(len(self.semantic_grid_x))
print((self.world_size_x/self.resolution)*(self.world_size_y/self.resolution)*(self.world_size_z/self.resolution))

rospy.loginfo("Termina de rellenar las matrices")

Expand Down
2 changes: 1 addition & 1 deletion src/Planners/LazyThetaStarSemanticCost.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -224,7 +224,7 @@ namespace Planners
// cost2=(c_wall/(1+static_cast<double>(_suc->cost)))*cost2; // + cost2;
cost2=(c_wall/coste)*cost2;

// cost2=(c_wall/static_cast<double>(_suc->cost))*cost2 + cost2;
cost2=(c_wall/static_cast<double>(_suc->cost))*cost2 + cost2;
// std::cout << "cost: " << _suc->cost << std::endl;
// std::cout << "semantic: " << _suc->semantic << std::endl;
// usleep(1e4);
Expand Down

0 comments on commit bdeaa3a

Please sign in to comment.