version icuas

robotics-upo · Jun 24, 2024 · 651b06f · 651b06f
1 parent 060d90a
commit 651b06f
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 11 deletions.
diff --git a/include/Grid3D/grid3d.hpp b/include/Grid3D/grid3d.hpp
@@ -710,7 +710,7 @@ bool loadSemanticGrid() {
 						// percent_computed_pub_.publish(percent_msg);
 					}
 					if(m_kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
-					{
+					{  
 						// JAC: Always square distance has been considered!!!!!!
 						// dist = pointNKNSquaredDistance[0];
 						dist = sqrt(pointNKNSquaredDistance[0]);

diff --git a/include/Planners/LazyThetaStarSemantic.hpp b/include/Planners/LazyThetaStarSemantic.hpp
@@ -79,7 +79,7 @@ namespace Planners
 
         // Variable to ensure that the los is true between the parent of the current and one neighbour, so SetVertex function should not be executed
         bool los_neighbour_{false}; /*!< TODO Comment */
-        int coef=10;
+        int coef=3;
 
 
     };

diff --git a/include/Planners/LazyThetaStarSemanticCost.hpp b/include/Planners/LazyThetaStarSemanticCost.hpp
@@ -81,7 +81,7 @@ namespace Planners
 
         // Variable to ensure that the los is true between the parent of the current and one neighbour, so SetVertex function should not be executed
         bool los_neighbour_{false}; /*!< TODO Comment */
-        int coef=10;
+        int coef=3;
 
 
     };

diff --git a/launch/simulator_atlas.launch b/launch/simulator_atlas.launch
@@ -3,12 +3,12 @@
 <launch>
 
   <!-- Start Gazebo with wg world running in (max) realtime -->
-  <include file="$(find hector_gazebo_worlds)/launch/mesh.launch"/>
+  <!-- <include file="$(find hector_gazebo_worlds)/launch/mesh.launch"/> -->
 
   <!-- Spawn simulated quadrotor uav -->
-  <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch" >
+  <!-- <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch" >
     <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/> 
-  </include>
+  </include> -->
 
   <!-- <node pkg="tf" type="static_transform_publisher" name="map_world" args="32 32 0 -3.1416 0 0 map world 100" /> -->
   <!-- <node pkg="tf" type="static_transform_publisher" name="map_world" args="181.5 -10.0 0 0 0 0 map world 100" /> -->
@@ -22,9 +22,11 @@
     <!-- <arg name="map_name"            default="manufacturing"/> -->
     <!-- ATLAS -->
     <!-- <arg name="map_name"            default="Atlas_8_300"/> -->
-    <arg name="map_name"            default="Atlas_8_puertas_02"/>
     <!-- <arg name="map_name"            default="nube_de_puntos2"/> -->
     <!-- <arg name="map_name"            default="nube_de_puntos_con_etiquetas"/> -->
+
+    <!-- <arg name="map_name"            default="Atlas_8_puertas_02"/> -->
+    <arg name="map_name"            default="nube_atlas_recortado_02"/>
     <arg name="map"                 default="$(find heuristic_planners)/resources/3dmaps/$(arg map_name).bt"/>
 
     <arg name="algorithm_name"      default="lazythetastar_semantic_cost"/>

diff --git a/scripts/grid3d_class.py b/scripts/grid3d_class.py
@@ -85,10 +85,7 @@ def __init__(self):
                         for y in range(min_y, max_y + 1):
                             for z in range(min_z, max_z + 1):
                                 # print(f'{x},{y},{z},1')
-                                self.semantic_grid_x.append(int(x))
-                                self.semantic_grid_y.append(int(y))
-                                self.semantic_grid_z.append(int(z))
-                                self.semantic_grid_v.append(int(index + 1))
+