From: Henrique Scharlau Coelho <henriquescharlaucoelho@gmail.com>
Date: Mon, 31 Jul 2023 00:45:28 +0000 (-0300)
Subject: added pointcloud data to global costmap
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=bb37a028207c4114984b7c5529b3e6b7f7183ac0;p=twil.git

added pointcloud data to global costmap
---

diff --git a/twil_2dnav/config/controller_params.yaml b/twil_2dnav/config/controller_params.yaml
index 808a8f0..b41c28c 100644
--- a/twil_2dnav/config/controller_params.yaml
+++ b/twil_2dnav/config/controller_params.yaml
@@ -80,6 +80,4 @@ local_costmap:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
         inflation_radius: 0.35
-      static_layer:
-        map_subscribe_transient_local: True
       always_send_full_costmap: True
diff --git a/twil_2dnav/config/planner_params.yaml b/twil_2dnav/config/planner_params.yaml
index a9e5c60..bcc6123 100644
--- a/twil_2dnav/config/planner_params.yaml
+++ b/twil_2dnav/config/planner_params.yaml
@@ -18,7 +18,7 @@ global_costmap:
       robot_radius: 0.3
       resolution: 0.05
       track_unknown_space: true
-      plugins: ["static_layer", "inflation_layer"]
+      plugins: ["static_layer", "inflation_layer", "voxel_layer", "obstacle_layer"]
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
@@ -26,4 +26,37 @@ global_costmap:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
         inflation_radius: 0.35
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        footprint_clearing_enabled: true
+        max_obstacle_height: 2.0
+        # publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        unknown_threshold: 15
+        mark_threshold: 0
+        observation_sources: pointcloud
+        combination_method: 1
+        pointcloud:  # no frame set, uses frame from message
+          topic: /camera/depth/points
+          max_obstacle_height: 2.0
+          min_obstacle_height: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+          raytrace_max_range: 2.5
+          raytrace_min_range: 0.0
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        combination_method: 0 # o padrão é 1, que parece ser o melhor na maioria dos casos
+        observation_sources: pointcloud
+        pointcloud:  
+          topic: /camera/depth/points
+          data_type: "PointCloud2"
       always_send_full_costmap: True
diff --git a/twil_2dnav/launch/nav2_navigator.launch.xml b/twil_2dnav/launch/nav2_navigator.launch.xml
index b82f8ce..669a664 100644
--- a/twil_2dnav/launch/nav2_navigator.launch.xml
+++ b/twil_2dnav/launch/nav2_navigator.launch.xml
@@ -20,52 +20,53 @@
 *******************************************************************************-->
 
 <launch>
-	<arg name="use_sim_time" default="false"/>
-	<arg name="map" default="$(find-pkg-share ufrgs_maps)/map/centenario1.yaml"/>
-	<arg name="autostart" default="true"/>
-	<arg name="controller" default=""/>
-	
-	<node name="map_server" pkg="nav2_map_server" exec="map_server">
-		<param name="use_sim_time" value="$(var use_sim_time)"/>
-		<param name="yaml_filename" value="$(var map)"/>
-	</node>
+    <arg name="use_sim_time" default="false"/>
+    <arg name="map" default="$(find-pkg-share ufrgs_maps)/map/centenario1.yaml"/>
+    <arg name="autostart" default="true"/>
+    <arg name="controller" default=""/>
 
-	<node pkg="tf2_ros" exec="static_transform_publisher" name="odom_frame_publisher" args="--frame-id map --child-frame-id odom"/>
+    <node pkg="nav2_map_server" name="map_server" exec="map_server">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param name="yaml_filename" value="$(var map)"/>
+    </node>
 
-	<node pkg="nav2_controller" exec="controller_server" name="controller_server">
-		<param name="use_sim_time" value="$(var use_sim_time)"/>
-		<param from="$(find-pkg-share twil_2dnav)/config/controller_params.yaml"/>
-		<remap from="odom" to="$(var controller)/odom"/>
-		<remap from="cmd_vel" to="$(var controller)/command"/>
-	</node>
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="odom_frame_publisher" args="--frame-id map --child-frame-id odom"/>
 
-	<node pkg="nav2_planner" exec="planner_server" name="planner_server">
-		<param name="use_sim_time" value="$(var use_sim_time)"/>
-		<param from="$(find-pkg-share twil_2dnav)/config/planner_params.yaml"/>
-	</node>
+    <node pkg="nav2_controller" exec="controller_server" name="controller_server">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param from="$(find-pkg-share twil_2dnav)/config/controller_params.yaml"/>
+        <remap from="odom" to="$(var controller)/odom"/>
+        <remap from="cmd_vel" to="$(var controller)/command"/>
+    </node>
 
-	<node pkg="nav2_behaviors" exec="behavior_server" name="behavior_server">
-		<param name="use_sim_time" value="$(var use_sim_time)"/>
-		<param from="$(find-pkg-share twil_2dnav)/config/behavior_params.yaml"/>
-		<param name="robot_base_frame" value="twil_origin"/>
-		<remap from="cmd_vel" to="$(var controller)/command"/>
-	</node>
+    <node pkg="nav2_planner" exec="planner_server" name="planner_server">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param from="$(find-pkg-share twil_2dnav)/config/planner_params.yaml"/>
+    </node>
 
-	<node pkg="nav2_bt_navigator" exec="bt_navigator" name="bt_navigator">
-		<param name="use_sim_time" value="$(var use_sim_time)"/>
-		<param from="$(find-pkg-share twil_2dnav)/config/bt_navigator_params.yaml"/>
-		<param name="robot_base_frame" value="twil_origin"/>
-		<remap from="odom" to="$(var controller)/odom"/>
-	</node>
+    <node pkg="nav2_behaviors" exec="behavior_server" name="behavior_server">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param from="$(find-pkg-share twil_2dnav)/config/behavior_params.yaml"/>
+        <param name="robot_base_frame" value="twil_origin"/>
+        <remap from="cmd_vel" to="$(var controller)/command"/>
+    </node>
 
-	<node pkg="nav2_lifecycle_manager" exec="lifecycle_manager" name="lifecycle_manager_nav2">
-		<param name="use_sim_time" value="$(var use_sim_time)"/>
-		<param name="autostart" value="$(var autostart)"/>
-		<param name="node_names" value="['map_server',
+    <node pkg="nav2_bt_navigator" exec="bt_navigator" name="bt_navigator">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param from="$(find-pkg-share twil_2dnav)/config/bt_navigator_params.yaml"/>
+        <param name="robot_base_frame" value="twil_origin"/>
+        <remap from="odom" to="$(var controller)/odom"/>
+    </node>
+
+    <node pkg="nav2_lifecycle_manager" exec="lifecycle_manager" name="lifecycle_manager_nav2">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param name="autostart" value="$(var autostart)"/>
+        <param name="node_names" value="[
+            'map_server',
 			'controller_server',
 			'planner_server',
 			'behavior_server',
 			'bt_navigator']"/>
-	</node>
+    </node>
 
 </launch>