From f7e70475f968debd1dc2017a1caa6fae97fd1f7c Mon Sep 17 00:00:00 2001
From: jj7258 <joelj7258@gmail.com>
Date: Fri, 26 Jul 2024 19:55:26 +0530
Subject: [PATCH] Trying to perform autonomous nav using the octomap data

---
 .../octomap_config/costmap_common_params.yaml | 32 ++++++++++++
 .../octomap_config/global_costmap_params.yaml | 13 +++++
 .../octomap_config/local_costmap_params.yaml  | 14 ++++++
 .../config/octomap_config/move_base.yaml      | 21 ++++++++
 src/navrover_navigation/launch/amcl.launch    | 49 +++++++++++++++++++
 .../launch/octomap_nav.launch                 | 30 ++++++++++++
 src/navrover_simulation/launch/gazebo.launch  |  4 +-
 7 files changed, 161 insertions(+), 2 deletions(-)
 create mode 100644 src/navrover_navigation/config/octomap_config/costmap_common_params.yaml
 create mode 100644 src/navrover_navigation/config/octomap_config/global_costmap_params.yaml
 create mode 100644 src/navrover_navigation/config/octomap_config/local_costmap_params.yaml
 create mode 100644 src/navrover_navigation/config/octomap_config/move_base.yaml
 create mode 100644 src/navrover_navigation/launch/amcl.launch
 create mode 100644 src/navrover_navigation/launch/octomap_nav.launch

diff --git a/src/navrover_navigation/config/octomap_config/costmap_common_params.yaml b/src/navrover_navigation/config/octomap_config/costmap_common_params.yaml
new file mode 100644
index 0000000..e733bea
--- /dev/null
+++ b/src/navrover_navigation/config/octomap_config/costmap_common_params.yaml
@@ -0,0 +1,32 @@
+footprint: [[-0.25, -0.20], [-0.25, 0.20], [0.25, 0.20], [0.25, -0.20]]
+footprint_padding: 0.01
+
+robot_radius: 0.3
+transform_tolerance: 0.2
+
+obstacle_layer:
+  enabled: true
+  obstacle_range: 5.5
+  raytrace_range: 6.0
+  inflation_radius: 0.2
+  track_unknown_space: true
+  combination_method: 1
+
+  observation_sources: point_cloud_sensor
+  point_cloud_sensor:
+    sensor_frame: map
+    data_type: PointCloud2
+    topic: /kinect/depth/points
+    marking: true
+    clearing: true
+    min_obstacle_height: 0.05
+    max_obstacle_height: 2.0
+
+inflation_layer:
+  enabled: true
+  cost_scaling_factor: 10.0
+  inflation_radius: 0.5
+
+static_layer:
+  enabled: true
+  map_topic: "/map"
\ No newline at end of file
diff --git a/src/navrover_navigation/config/octomap_config/global_costmap_params.yaml b/src/navrover_navigation/config/octomap_config/global_costmap_params.yaml
new file mode 100644
index 0000000..0325e22
--- /dev/null
+++ b/src/navrover_navigation/config/octomap_config/global_costmap_params.yaml
@@ -0,0 +1,13 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_footprint
+  update_frequency: 1.0
+  publish_frequency: 0.5
+  static_map: false
+  rolling_window: false
+  resolution: 0.05
+  transform_tolerance: 0.5
+  plugins:
+    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
+    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
+    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}
\ No newline at end of file
diff --git a/src/navrover_navigation/config/octomap_config/local_costmap_params.yaml b/src/navrover_navigation/config/octomap_config/local_costmap_params.yaml
new file mode 100644
index 0000000..b30739c
--- /dev/null
+++ b/src/navrover_navigation/config/octomap_config/local_costmap_params.yaml
@@ -0,0 +1,14 @@
+local_costmap:
+  global_frame: odom
+  robot_base_frame: base_footprint
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 4.0
+  height: 4.0
+  resolution: 0.05
+  transform_tolerance: 0.5
+  plugins:
+    - {name: obstacle_layer,      type: "costmap_2d::VoxelLayer"}
+    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}
\ No newline at end of file
diff --git a/src/navrover_navigation/config/octomap_config/move_base.yaml b/src/navrover_navigation/config/octomap_config/move_base.yaml
new file mode 100644
index 0000000..8a6f1c7
--- /dev/null
+++ b/src/navrover_navigation/config/octomap_config/move_base.yaml
@@ -0,0 +1,21 @@
+shutdown_costmaps: false
+controller_frequency: 5.0
+planner_patience: 5.0
+controller_patience: 15.0
+conservative_reset_dist: 3.0
+planner_frequency: 1.0
+oscillation_timeout: 10.0
+oscillation_distance: 0.2
+
+global_costmap:
+  static_map: true
+  
+local_costmap:
+  static_map: false
+  rolling_window: true
+  width: 4.0
+  height: 4.0
+  resolution: 0.05
+
+base_global_planner: "navfn/NavfnROS"
+base_local_planner: "base_local_planner/TrajectoryPlannerROS"
\ No newline at end of file
diff --git a/src/navrover_navigation/launch/amcl.launch b/src/navrover_navigation/launch/amcl.launch
new file mode 100644
index 0000000..4f3fd2f
--- /dev/null
+++ b/src/navrover_navigation/launch/amcl.launch
@@ -0,0 +1,49 @@
+<launch>
+
+  <arg name="use_map_topic" default="true"/>
+  <arg name="scan_topic" default="scan"/>
+​
+  <node pkg="amcl" type="amcl" name="amcl">
+    <param name="use_map_topic" value="$(arg use_map_topic)"/>
+    <!-- Publish scans from best pose at a max of 10 Hz -->
+    <param name="odom_model_type" value="diff"/>
+    <param name="odom_alpha1" value="0.005"/>
+    <param name="odom_alpha2" value="0.0.005"/>
+    <!-- translation variance, m -->
+    <param name="odom_alpha3" value="0.01"/>
+    <param name="odom_alpha4" value="0.005"/>
+    <param name="odom_alpha5" value="0.003"/>
+    <param name="gui_publish_rate" value="10.0"/>
+    <param name="laser_max_beams" value="60"/>
+    <param name="laser_max_range" value="12.0"/>
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="2000"/>
+    <param name="kld_err" value="0.05"/>
+    <param name="kld_z" value="0.99"/>
+    <!-- Laser Parameters -->
+    <param name="laser_z_hit" value="0.5"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.5"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <!-- <param name="laser_model_type" value="beam"/> -->
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.25"/>
+    <param name="update_min_a" value="0.2"/>
+    <param name="odom_frame_id" value="odom"/>
+    <param name="base_frame_id" value="base_footprint"/>
+    <param name="resample_interval" value="2"/>
+    <!-- Increase tolerance because the computer can get quite busy -->
+    <param name="transform_tolerance" value="1.0"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+    <param name="initial_pose_x" value="0.0"/> 
+    <param name="initial_pose_y" value="0.0"/>
+    <param name="initial_pose_a" value="0.0"/>
+    <remap from="scan" to="$(arg scan_topic)"/>    
+  </node>
+
+
+</launch>
\ No newline at end of file
diff --git a/src/navrover_navigation/launch/octomap_nav.launch b/src/navrover_navigation/launch/octomap_nav.launch
new file mode 100644
index 0000000..a99a9f7
--- /dev/null
+++ b/src/navrover_navigation/launch/octomap_nav.launch
@@ -0,0 +1,30 @@
+<launch>
+  <!-- Load the OctoMap -->
+  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
+    <param name="frame_id" type="string" value="map" />
+    <param name="resolution" value="0.05" />
+    <param name="filename" value="$(find navrover_navigation)/maps/octomaps/teleop.bt" />
+    <param name="pointcloud_max_z" value="2.0" />
+    <param name="pointcloud_min_z" value="0.05" />
+    <param name="occupancy_min_z" value="0.05" />
+    <param name="occupancy_max_z" value="2.0" />
+ </node>
+
+    <!-- Run the map server -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find navrover_navigation)/maps/teleop.yaml" />
+  
+  <!-- AMCL -->
+  <include file="$(find navrover_navigation)/launch/amcl.launch" />
+
+  <!-- Move base -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <rosparam file="$(find navrover_navigation)/config/octomap_config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find navrover_navigation)/config/octomap_config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find navrover_navigation)/config/octomap_config/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find navrover_navigation)/config/octomap_config/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find navrover_navigation)/config/octomap_config/move_base.yaml" command="load" />
+  </node>
+
+  <!-- rviz -->
+  <!-- <node type="rviz" name="rviz" pkg="rviz" args="-d $(find navrover_navigation)/rviz/navigation.rviz" /> -->
+</launch>
\ No newline at end of file
diff --git a/src/navrover_simulation/launch/gazebo.launch b/src/navrover_simulation/launch/gazebo.launch
index 3885b2c..9c96126 100644
--- a/src/navrover_simulation/launch/gazebo.launch
+++ b/src/navrover_simulation/launch/gazebo.launch
@@ -23,8 +23,8 @@
 
   <!-- Run Gazebo -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <!-- <arg name="world_name" value="$(find navrover_simulation)/worlds/teleop.world"/> -->
-    <arg name="world_name" value="$(find navrover_simulation)/worlds/nights_and_weekends_s5.world"/>
+    <arg name="world_name" value="$(find navrover_simulation)/worlds/teleop.world"/>
+    <!-- <arg name="world_name" value="$(find navrover_simulation)/worlds/nights_and_weekends_s5.world"/> -->
     <arg name="paused" value="false"/>
     <arg name="use_sim_time" value="true"/>
     <arg name="gui" value="true"/>