Skip to content

Commit

Permalink
code for the big house
Browse files Browse the repository at this point in the history
  • Loading branch information
@jcobano
jcobano committed Oct 14, 2024
1 parent 894957e commit d9e32d5
Show file tree
Hide file tree
Showing 6 changed files with 62 additions and 17 deletions.
14 changes: 7 additions & 7 deletions include/Grid3D/grid3d.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,7 @@ class Grid3d
if(!lnh.getParam("publish_point_cloud_rate", m_publishPointCloudRate))
m_publishPointCloudRate = 0.2;
if(!lnh.getParam("publish_grid_slice", value))
value = 1.0;
value = 4.0;
if(!lnh.getParam("publish_grid_slice_rate", m_publishGridSliceRate))
m_publishGridSliceRate = 0.2;
m_gridSlice = (float)value;
Expand Down Expand Up @@ -149,12 +149,12 @@ class Grid3d



// Setup point-cloud publisher
if(m_publishPc)
{
m_pcPub = m_nh.advertise<sensor_msgs::PointCloud2>(m_nodeName+"/map_point_cloud", 1, true);
mapTimer = m_nh.createTimer(ros::Duration(1.0/m_publishPointCloudRate), &Grid3d::publishMapPointCloudTimer, this);
}
// Setup point-cloud publisher
if(m_publishPc)
{
m_pcPub = m_nh.advertise<sensor_msgs::PointCloud2>(m_nodeName+"/map_point_cloud", 1, true);
mapTimer = m_nh.createTimer(ros::Duration(1.0/m_publishPointCloudRate), &Grid3d::publishMapPointCloudTimer, this);
}
percent_computed_pub_ = m_nh.advertise<std_msgs::Float32>(m_nodeName+"/percent_computed", 1, false);
}

Expand Down
12 changes: 11 additions & 1 deletion launch/grid3d.launch
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,18 @@
<launch>
<node name="semantic_grid" pkg="heuristic_planners" output="screen" type="grid3d_class.py">
<param name="world_size_x" value="211.8" />
<!-- <param name="world_size_x" value="211.8" />
<param name="world_size_y" value="169.2" />
<param name="world_size_z" value="9.2" />
<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_y" value="107.4" />
<param name="world_size_z" value="71.2" />
<param name="resolution" value="0.2" />
</node>
</launch>
33 changes: 28 additions & 5 deletions launch/simulator_atlas.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,24 +22,47 @@
<!-- <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="Atlas_8_puertas_02"/> -->
<!-- <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="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="lazythetastar_semantic_cost"/>
<arg name="algorithm_name" default="costastar"/>

<!-- 212 -->
<arg name="world_size_x" default="211.8"/>
<!-- <arg name="world_size_x" default="211.8"/> -->
<!-- 170 -->
<arg name="world_size_y" default="169.2"/>
<!-- <arg name="world_size_y" default="169.2"/> -->
<!-- maze.bt with world_size_z=5 -->
<!-- <arg name="world_size_z" default="5"/> -->
<!-- <arg name="world_size_z" default="2"/> manufacturing -->
<!-- 9 -->
<arg name="world_size_z" default="9.2"/> <!-- 6 -->
<!-- <arg name="world_size_z" default="9.2"/>
<arg name="resolution" default="0.2"/> -->

<!-- Atlas -->
<!-- <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"/> -->

<!-- Casa CON muebles -->
<!-- <arg name="world_size_x" default="169.2"/>
<arg name="world_size_y" default="102.2"/>
<arg name="world_size_z" default="71.0"/>
<arg name="resolution" default="0.2"/> -->

<!-- Casa SIN muebles-->
<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="inflate_map" default="true"/>
Expand Down
16 changes: 13 additions & 3 deletions scripts/grid3d_class.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,8 @@ def __init__(self):
# Inicializacion del archivo IFC

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/model.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 All @@ -36,15 +37,24 @@ def __init__(self):
self.lamps = self.model.by_type("IfcFlowTerminal")
self.slabs = self.model.by_type("IfcSlab")
self.windows = self.model.by_type("IfcWindow")
self.glasses = ifcopenshell.util.selector.filter_elements(self.model, "IfcElement, material=Glass")
# self.glasses = ifcopenshell.util.selector.filter_elements(self.model, "IfcElement, material=Glass")

self.semantic_objects = [self.walls, self.doors, self.columns, self.furniture, self.stairs, self.plates, self.lamps, self.glasses]
# self.semantic_objects = [self.walls, self.doors, self.columns, self.furniture, self.stairs, self.plates, self.lamps, self.glasses]
self.semantic_objects = [self.walls, self.doors, self.columns, self.furniture, self.stairs, self.plates, self.lamps]

rospy.loginfo("Termina carga de IFC")
# Model Atlas
# self.world_size_x = rospy.get_param('~world_size_x', 220)
# self.world_size_y = rospy.get_param('~world_size_y', 220)
# self.world_size_z = rospy.get_param('~world_size_z', 20)
# self.resolution = rospy.get_param('~resolution', 0.2)

# Model casoplonv2
self.world_size_x = rospy.get_param('~world_size_x', 220)
self.world_size_y = rospy.get_param('~world_size_y', 220)
self.world_size_z = rospy.get_param('~world_size_z', 20)
self.resolution = rospy.get_param('~resolution', 0.2)

self.x_y_size = self.world_size_x * self.world_size_y

self.semantic_grid_x = []
Expand Down
3 changes: 3 additions & 0 deletions src/ROS/planner_ros_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -353,10 +353,13 @@ class HeuristicPlannerROS
//
if( input_map_ == 1 ){
Planners::utils::configureWorldFromOccupancyWithCosts(occupancy_grid_, *algorithm_);
// std::cout << "input1" << std::endl;
}else if( input_map_ == 2 ){
Planners::utils::configureWorldFromPointCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>(cloud_), *algorithm_, resolution_);
Planners::utils::configureWorldCosts(*m_grid3d_, *algorithm_);
// std::cout << "input2" << std::endl;
}
// std::cout << "input" << std::endl;
//Algorithm specific parameters. Its important to set line of sight after configuring world size(it depends on the resolution)
float sight_dist, cost_weight;
lnh_.param("max_line_of_sight_distance", sight_dist, (float)1000.0); // In meters
Expand Down
1 change: 0 additions & 1 deletion src/utils/ros/ROSInterfaces.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,6 @@ namespace Planners
}
bool configureWorldFromPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &_points, AlgorithmBase &_algorithm, const double &_resolution)
{

for (auto &it : *_points)
_algorithm.addCollision(discretePoint(it, _resolution));

Expand Down

0 comments on commit d9e32d5

Please sign in to comment.