Feature/tidy up slam and nagivation (#90)

* init commit for nagation and slam launch files tidy up * create new world for mini pupper * delete useless files * improve readme and rviz * fix flake8 errors * fine tune control parameters * fix slam parameters and simplify rviz * enable imu usage * minor config update for imu * enable yaw from imu * Update simulation world and map
mangdangroboticsclub · Jun 2, 2024 · 8dad5f0 · 8dad5f0
1 parent 8127302
commit 8dad5f0
Show file tree

Hide file tree

Showing 31 changed files with 1,992 additions and 5,548 deletions.
diff --git a/README.md b/README.md
@@ -127,35 +127,27 @@ ros2 launch mini_pupper_gazebo gazebo.launch.py robot_name:=mini_pupper_2
 ```sh
 # Terminal 2
 . ~/ros2_ws/install/setup.bash
-ros2 launch mini_pupper_navigation slam.launch.py use_sim_time:=true
+ros2 launch mini_pupper_slam slam.launch.py use_sim_time:=true
 ```
 
 - Keyboard control  
 Use the keyboard to remotely control the Mini Pupper to complete the mapping.
 ```sh
 # Terminal 3
+. ~/ros2_ws/install/setup.bash
 ros2 run teleop_twist_keyboard teleop_twist_keyboard
 ```
 
 - Save the map  
-The map will be saved at $HOME.
 ```sh
 # Terminal 4 (on PC)
-ros2 service call /finish_trajectory cartographer_ros_msgs/srv/FinishTrajectory "{trajectory_id: 0}"
-ros2 service call /write_state cartographer_ros_msgs/srv/WriteState "{filename: '${HOME}/cartographer_map.pbstream'}"
-ros2 run nav2_map_server map_saver_cli -f ${HOME}/cartographer_map
+. ~/ros2_ws/install/setup.bash
+ros2 run nav2_map_server map_saver_cli -f ~/map
 ```
+The map will be saved under home directory. Two files will be generated, namely map.pgm and map.yaml.
 
-#### 2.1.4 Test Navigation in Gazebo
 
-- Replace the map files  
-Remember to replace the cartographer_map.pbstream in the maps folder with your new cartographer_map.pbstream first.
-```sh
-# Terminal 2 (on PC)
-cp -f ~/cartographer_map.pgm ~/ros2_ws/src/mini_pupper_ros/mini_pupper_navigation/maps/cartographer_map.pgm
-cp -f ~/cartographer_map.pbstream ~/ros2_ws/src/mini_pupper_ros/mini_pupper_navigation/maps/cartographer_map.pbstream
-cp -f ~/cartographer_map.yaml ~/ros2_ws/src/mini_pupper_ros/mini_pupper_navigation/maps/cartographer_map.yaml
-```
+#### 2.1.4 Test Navigation in Gazebo
 
 - Bring up Gazebo
 ```sh
@@ -164,28 +156,18 @@ cp -f ~/cartographer_map.yaml ~/ros2_ws/src/mini_pupper_ros/mini_pupper_navigati
 ros2 launch mini_pupper_gazebo gazebo.launch.py robot_name:=mini_pupper_2
 ```
 
-- Localization & Navigation
+- Navigation   
 ```sh
-# Terminal 3
+# Terminal 2
 . ~/ros2_ws/install/setup.bash
-ros2 launch mini_pupper_navigation bringup.launch.py use_sim_time:=true
+ros2 launch mini_pupper_navigation navigation.launch.py use_sim_time:=true
 ```
-
-- Localization only  
-(This step is not necessary if you run `bringup.launch.py`.)
+Alternatively, if you wish to use the map you generated in previous step, you can specify the map path with the following command.
 ```sh
-# Terminal 4
-. ~/ros2_ws/install/setup.bash
-ros2 launch mini_pupper_navigation localization.launch.py use_sim_time:=true
+ros2 launch mini_pupper_navigation navigation.launch.py use_sim_time:=true map:=$HOME/map.yaml
 ```
+After executing this command, the rviz window will be displayed. You can utilize the "2D Post Estimate" feature to establish an appropriate initial pose for the robot. Subsequently, you can set a goal for the robot by clicking on "Nav2 Goal", hence Nav2 will plan the path and guide the robot towards reaching the goal.
 
-- Navigation only  
-(This step is not necessary if you run `bringup.launch.py`.)
-```sh
-# Terminal 4
-. ~/ros2_ws/install/setup.bash
-ros2 launch mini_pupper_navigation navigation.launch.py use_sim_time:=true
-```
 
 ## 2.2 Mini Pupper
 
@@ -215,28 +197,29 @@ Note: This step requires both PC and Mini Pupper
 ros2 launch mini_pupper_bringup bringup.launch.py
 ```
 
-- Mapping on PC
+- SLAM on PC
 ```sh
 # Terminal 2 (on PC)
 . ~/ros2_ws/install/setup.bash
-ros2 launch mini_pupper_navigation slam.launch.py
+ros2 launch mini_pupper_slam slam.launch.py
 ```
 
 - Keyboard control  
 Use the keyboard to remotely control the Mini Pupper to complete the mapping.
 ```sh
 # Terminal 3 (on PC)
+. ~/ros2_ws/install/setup.bash
 ros2 run teleop_twist_keyboard teleop_twist_keyboard
 ```
 
 - Save the map  
-The map will be saved at $HOME.
+Run the following command from a new terminal.
 ```sh
 # Terminal 4 (on PC)
-ros2 service call /finish_trajectory cartographer_ros_msgs/srv/FinishTrajectory "{trajectory_id: 0}"
-ros2 service call /write_state cartographer_ros_msgs/srv/WriteState "{filename: '${HOME}/cartographer_map.pbstream'}"
-ros2 run nav2_map_server map_saver_cli -f ${HOME}/cartographer_map
+. ~/ros2_ws/install/setup.bash
+ros2 run nav2_map_server map_saver_cli -f ~/map
 ```
+The map will be saved under home directory. Two files will be generated, namely map.pgm and map.yaml.
 
 #### 2.2.3 Test Navigation
 
@@ -248,20 +231,11 @@ ros2 launch mini_pupper_bringup bringup.launch.py
 
 ```
 
-- Replace the map files  
-Remember to replace the cartographer_map.pbstream in the maps folder with your new cartographer_map.pbstream first.
+- Navigation with previously saved map from step 2.2.2
 ```sh
-# Terminal 2 (on PC)
-cp -f ~/cartographer_map.pgm ~/ros2_ws/src/mini_pupper_ros/mini_pupper_navigation/maps/cartographer_map.pgm
-cp -f ~/cartographer_map.pbstream ~/ros2_ws/src/mini_pupper_ros/mini_pupper_navigation/maps/cartographer_map.pbstream
-cp -f ~/cartographer_map.yaml ~/ros2_ws/src/mini_pupper_ros/mini_pupper_navigation/maps/cartographer_map.yaml
-```
-
-- Navigation
-```sh
-# Terminal 3 (on PC)
+# Terminal 4 (on PC)
 . ~/ros2_ws/install/setup.bash
-ros2 launch mini_pupper_navigation bringup.launch.py
+ros2 launch mini_pupper_navigation navigation.launch.py map:=$HOME/map.yaml
 ```
 
 ### 2.2.1 Test dance

diff --git a/mini_pupper_2_description/config/ros_control/mini_pupper_controller.yaml b/mini_pupper_2_description/config/ros_control/mini_pupper_controller.yaml
@@ -33,15 +33,15 @@ joint_group_effort_controller:
       - velocity
 
     gains:
-      base_lf1 : {p: 4.0, i: 0.0, d: 0.006}
-      lf1_lf2  : {p: 4.0, i: 0.0, d: 0.006}
-      lf2_lf3  : {p: 4.0, i: 0.0, d: 0.006}
-      base_rf1 : {p: 4.0, i: 0.0, d: 0.006}
-      rf1_rf2  : {p: 4.0, i: 0.0, d: 0.006}
-      rf2_rf3  : {p: 4.0, i: 0.0, d: 0.006}
-      base_lb1 : {p: 4.0, i: 0.0, d: 0.006}
-      lb1_lb2  : {p: 4.0, i: 0.0, d: 0.006}
-      lb2_lb3  : {p: 4.0, i: 0.0, d: 0.006}
-      base_rb1 : {p: 4.0, i: 0.0, d: 0.006}
-      rb1_rb2  : {p: 4.0, i: 0.0, d: 0.006}
-      rb2_rb3  : {p: 4.0, i: 0.0, d: 0.006}
+      base_lf1 : {p: 5.0, i: 0.0, d: 0.006}
+      lf1_lf2  : {p: 5.0, i: 0.0, d: 0.006}
+      lf2_lf3  : {p: 5.0, i: 0.0, d: 0.006}
+      base_rf1 : {p: 5.0, i: 0.0, d: 0.006}
+      rf1_rf2  : {p: 5.0, i: 0.0, d: 0.006}
+      rf2_rf3  : {p: 5.0, i: 0.0, d: 0.006}
+      base_lb1 : {p: 5.0, i: 0.0, d: 0.006}
+      lb1_lb2  : {p: 5.0, i: 0.0, d: 0.006}
+      lb2_lb3  : {p: 5.0, i: 0.0, d: 0.006}
+      base_rb1 : {p: 5.0, i: 0.0, d: 0.006}
+      rb1_rb2  : {p: 5.0, i: 0.0, d: 0.006}
+      rb2_rb3  : {p: 5.0, i: 0.0, d: 0.006}