Update README.md

Author: haider8645

README.md

@@ -119,9 +119,9 @@ make install
 The test executables are in the folder: `build/src/test/`.
 
 ---
-# ROS 2 Humble Test Environment with Gazebo Fortress
+# ROS 2 Humble Test Environment with Turtlebot3 and Husky
 
-This provides instructions for setting up a test environment using **Gazebo Fortress** and **ROS 2 Humble**. The setup includes configurations for using the Husky robot and ensures that the necessary resources are in place for smooth operation.
+This provides instructions for setting up a test environment using **Gazebo Fortress** for Husky and **Gazebo Classic** for Turtlebot3 with **ROS 2 Humble**. The setup includes configurations for using the Husky robot and ensures that the necessary resources are in place for smooth operation.
 
 ## Prerequisites
 
@@ -143,7 +143,7 @@ sudo apt-get install -y ros-humble-libg2o
 ### 3. Get ugv_nav4d_ros2 and a test environment for robot husky in gazebo
 
 ```
-mkdir -p your_ros2_workspace/src
+mkdir -p ~/your_ros2_workspace/src
 cd ~/your_ros2_workspace/src
 git clone https://github.com/dfki-ric/ugv_nav4d_ros2.git
 ```
@@ -154,6 +154,7 @@ git clone https://github.com/dfki-ric/ros2_humble_gazebo_sim.git
 cd ros2_humble_gazebo_sim
 bash install_dependencies.bash
 ```
+
 ### 4. Husky Configuration
 To ensure that Gazebo can find the robot model, you need to export the following environment variable. Replace /path/to/ with the actual **complete** path where you clone the repository `ros2_humble_gazebo_sim`. Add this command to your terminal:
 ```
@@ -169,7 +170,55 @@ source path/to/ugv_nav4d/build/install/env.sh
 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 
-### 6. Start the Test Environment
+### 6. Test Environment with Turlebot3 and Nav2 Integration
+Follow the steps in this section to play around with a Turtlebot3 and Nav2. Ugv_nav4d expects a pointcloud map. The map can be provided by SLAM or static pointclouds as `PLY`. A dummp flat plane `PLY` file is used in these steps.
+
+Install turtlebot3-gazebo package and launch simulation
+```
+sudo apt-get install ros-humble-turtlebot3-gazebo
+ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py headless:=false x_pose:=2.0 y_pose:=2.0
+```
+Clone a repo with config files for nav2 and ugv_nav4d
+```
+cd ~/your_ros2_workspace
+git clone [email protected]:haider8645/turtlebot3_nav2_ugv_nav4d_config.git
+```
+Start nav2 controller_server
+
+Note: Please provide the fullpath for `your_ros2_workspace` in the launch file arguments.
+```
+cd turtlebot3_nav2_ugv_nav4d_config
+ros2 launch turtle_nav2.launch.py nav2_param_path:=/path/to/your_ros2_workspace/turtlebot3_nav2_ugv_nav4d_config/turtle_nav2.yaml rviz_config_path:=/path/to/your_ros2_workspace/turtlebot3_nav2_ugv_nav4d_config/turtle.rviz
+```
+In a new terminal, configure and activate the nav2 controller_server
+```
+ros2 lifecycle set /controller_server configure
+ros2 lifecycle set /controller_server activate
+```
+In a new terminal, start ugv_nav4d
+
+Note: Please provide the fullpath for `your_ros2_workspace` in the launch file arguments and accordingly edit the parameter `mls_file_path` in `turtle_ugv_nav4d.yaml`.
+```
+ros2 launch ugv_nav4d_ros2 ugv_nav4d.launch.py goal_topic:=/goal_pose main_param_file:=/path/to/your_ros2_workspace/turtlebot3_nav2_ugv_nav4d_config/turtle_ugv_nav4d.yaml
+```
+In new terminals, start scripts to send FollowPath action calls to nav2 and for Path visualization
+```
+cd ~/your_ros2_workspace/src/ugv_nav4d_ros2/scripts
+python3 follow_path_client.py
+```
+and 
+```
+cd ~/your_ros2_workspace/src/ugv_nav4d_ros2/scripts
+python3 visualize_path.py
+```
+Visualize the MLS Map using
+```
+ros2 service call /ugv_nav4d_ros2/map_publish std_srvs/srv/Trigger
+```
+
+You can now send goals to the planner using `2D Goal Pose` in rviz and visualize the results.
+
+### 7. Test Environment with Husky
 Launch the Gazebo simulation by executing the following command in your terminal:
 ```
 source ~/your_ros2_workspace/install/setup.bash
@@ -230,7 +279,7 @@ In a new terminal, start a python script to visualize the labeled path in rviz2.
 cd ~/your_ros2_workspace/src/ugv_nav4d_ros2/scripts
 python3 visualize_path.py
 ```
-### 7. Plan
+### 8. Plan
 
 In a new terminal, start Rviz2.
 ```