diff --git a/README.md b/README.md index ee63679..dc491c2 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ME5413_Planning_Project NUS ME5413 Autonomous Mobile Robotics Final Project -> Authors: [Dongen](https://github.com/nuslde) and [Shuo](https://github.com/SS47816) +> Authors: [Ziggy](https://github.com/ziggyhuang) and [Shuo](https://github.com/SS47816) ![Ubuntu 20.04](https://img.shields.io/badge/OS-Ubuntu_20.04-informational?style=flat&logo=ubuntu&logoColor=white&color=2bbc8a) ![ROS Noetic](https://img.shields.io/badge/Tools-ROS_Noetic-informational?style=flat&logo=ROS&logoColor=white&color=2bbc8a) @@ -10,7 +10,7 @@ NUS ME5413 Autonomous Mobile Robotics Final Project ![GitHub Repo stars](https://img.shields.io/github/stars/NUS-Advanced-Robotics-Centre/ME5413_Planning_Project?color=FFE333) ![GitHub Repo forks](https://img.shields.io/github/forks/NUS-Advanced-Robotics-Centre/ME5413_Planning_Project?color=FFE333) -![cover_image](src/me5413_world/media/gazebo_world.png) +![cover_image](src/me5413_world/media/rviz_overview.png) ## Dependencies @@ -29,6 +29,7 @@ NUS ME5413 Autonomous Mobile Robotics Final Project * `visualization_msgs` * `tf2` * `tf2_ros` + * `tf2_eigen` * `tf2_geometry_msgs` * `pluginlib` * `map_server` @@ -38,15 +39,14 @@ NUS ME5413 Autonomous Mobile Robotics Final Project * `jackal_navigation` * `velodyne_simulator` * `teleop_twist_keyboard` -* And this [gazebo_model](https://github.com/osrf/gazebo_models) repositiory + * dynamic_reconfigure ## Installation This repo is a ros workspace, containing three rospkgs: -* `interactive_tools` are customized tools to interact with gazebo and your robot +* `me5413_world` the main pkg containing the gazebo world, source code, and the launch files * `jackal_description` contains the modified jackal robot model descriptions -* `me5413_world` the main pkg containing the gazebo world, and the launch files **Note:** If you are working on this project, it is encouraged to fork this repository and work on your own fork! @@ -55,8 +55,8 @@ After forking this repo to your own github: ```bash # Clone your own fork of this repo (assuming home here `~/`) cd -git clone https://github.com//ME5413_Final_Project.git -cd ME5413_Final_Project +git clone https://github.com//ME5413_Planning_Project.git +cd ME5413_Planning_Project # Install all dependencies rosdep install --from-paths src --ignore-src -r -y @@ -67,31 +67,6 @@ catkin_make source devel/setup.bash ``` -To properly load the gazebo world, you will need to have the necessary model files in the `~/.gazebo/models/` directory. - -There are two sources of models needed: - -* [Gazebo official models](https://github.com/osrf/gazebo_models) - - ```bash - # Create the destination directory - cd - mkdir -p .gazebo/models - - # Clone the official gazebo models repo (assuming home here `~/`) - git clone https://github.com/osrf/gazebo_models.git - - # Copy the models into the `~/.gazebo/models` directory - cp -r ~/gazebo_models/* ~/.gazebo/models - ``` - -* [Our customized models](https://github.com/NUS-Advanced-Robotics-Centre/ME5413_Final_Project/tree/main/src/me5413_world/models) - - ```bash - # Copy the customized models into the `~/.gazebo/models` directory - cp -r ~/ME5413_Final_Project/src/me5413_world/models/* ~/.gazebo/models - ``` - ## Usage ### 0. Gazebo World @@ -103,78 +78,46 @@ This command will launch the gazebo with the project world roslaunch me5413_world world.launch ``` -### 1. Manual Control - -If you wish to explore the gazebo world a bit, we provide you a way to manually control the robot around: - -```bash -# Only launch the robot keyboard teleop control -roslaunch me5413_world manual.launch -``` - -**Note:** This robot keyboard teleop control is also included in all other launch files, so you don't need to launch this when you do mapping or navigation. - -![rviz_manual_image](src/me5413_world/media/rviz_manual.png) - -### 2. Mapping - -After launching **Step 0**, in the second terminal: - -```bash -# Launch GMapping -roslaunch me5413_world mapping.launch -``` - -After finishing mapping, run the following command in the thrid terminal to save the map: - -```bash -# Save the map as `my_map` in the `maps/` folder -roscd me5413_world/maps/ -rosrun map_server map_saver -f my_map map:=/map -``` - -![rviz_nmapping_image](src/me5413_world/media/rviz_mapping.png) - -### 3. Navigation +### 1. Path Tracking -Once completed **Step 2** mapping and saved your map, quit the mapping process. - -Then, in the second terminal: +In the second terminal, launch the path publisher node and the path tracker node: ```bash # Load a map and launch AMCL localizer -roslaunch me5413_world navigation.launch +roslaunch me5413_world path_tracking.launch ``` -![rviz_navigation_image](src/me5413_world/media/rviz_navigation.png) +![rviz_tracking_image](src/me5413_world/media/rviz_tracking.png) ## Student Tasks -### 1. Map the environment +### Task 1 & 2: Path Planning -* You may use any SLAM algorithm you like, any type: - * 2D LiDAR - * 3D LiDAR - * Vision - * Multi-sensor -* Verify your SLAM accuracy by comparing your odometry with the published `/gazebo/ground_truth/state` topic (`nav_msgs::Odometry`), which contains the gournd truth odometry of the robot. -* You may want to use tools like [EVO](https://github.com/MichaelGrupp/evo) to quantitatively evaluate the performance of your SLAM algorithm. +* Please refer to the PDF file inside. -### 2. Using your own map, navigate your robot +### Task 3: Path Tracking -* From the starting point, move to the given pose within each area in sequence - * Assembly Line 1, 2 - * Random Box 1, 2, 3, 4 - * Delivery Vehicle 1, 2, 3 -* We have provided you a GUI in RVIZ that allows you to click and publish these given goal poses to the `/move_base_simple/goal` topic: +* Control your robot to follow the given **figure 8** track. + * You may use any algorithms you like. + * Implement your algorithms in the `src/me5413_world/include/me5413_world/path_tracker_node.hpp` and `src/me5413_world/src/path_tracker_node.cpp`, to replace our template code. + * Test your algorithms on the track & Report your tracking accuracy. + +* In the template code, we have provided you: + * A dumb **PID** controller for the throttle. + * A weird **Stanley** controller for the steering. + * However, both are not properly configured or tuned. + +* We have provided you a dynamic reconfigure GUI that allows you to tune some of the parameters: - ![rviz_panel_image](src/me5413_world/media/rviz_panel.png) + ![rqt_reconfig_image](src/me5413_world/media/rqt_reconfig.png) -* We also provides you four topics (and visualized in RVIZ) that computes the real-time pose error between your robot and the selelcted goal pose: - * `/me5413_world/absolute/heading_error` (in degrees, wrt `world` frame, `std_msgs::Float32`) - * `/me5413_world/absolute/position_error` (in meters, wrt `world` frame, `std_msgs::Float32`) - * `/me5413_world/relative/heading_error` (in degrees, wrt `map` frame, `std_msgs::Float32`) - * `/me5413_world/relative/position_error` (in meters wrt `map` frame, `std_msgs::Float32`) +* We also provides you six topics (and visualized in RVIZ) that computes the real-time errors between your robot and the tracking path: + * `/me5413_world/planning/abs_position_error` ([m], `std_msgs::Float32`) + * `/me5413_world/planning/abs_heading_error` ([deg], `std_msgs::Float32`) + * `/me5413_world/planning/abs_speed_error` ([m/s], `std_msgs::Float32`) + * `/me5413_world/planning/rms_position_error` ([m], `std_msgs::Float32`) + * `/me5413_world/planning/rms_heading_error` ([deg], `std_msgs::Float32`) + * `/me5413_world/planning/rms_speed_error` ([m/s], `std_msgs::Float32`) ## Contribution @@ -188,4 +131,4 @@ We are following: ## License -The [ME5413_Final_Project](https://github.com/NUS-Advanced-Robotics-Centre/ME5413_Final_Project) is released under the [MIT License](https://github.com/NUS-Advanced-Robotics-Centre/ME5413_Final_Project/blob/main/LICENSE) \ No newline at end of file +The [ME5413_Planning_Project](https://github.com/NUS-Advanced-Robotics-Centre/ME5413_Planning_Project) is released under the [MIT License](https://github.com/NUS-Advanced-Robotics-Centre/ME5413_Planning_Project/blob/main/LICENSE) \ No newline at end of file diff --git a/src/me5413_world/launch/manual.launch b/src/me5413_world/launch/manual.launch deleted file mode 100644 index 3de746a..0000000 --- a/src/me5413_world/launch/manual.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/src/me5413_world/launch/navigation.launch b/src/me5413_world/launch/path_tracking.launch similarity index 100% rename from src/me5413_world/launch/navigation.launch rename to src/me5413_world/launch/path_tracking.launch diff --git a/src/me5413_world/media/gazebo_world.png b/src/me5413_world/media/gazebo_world.png deleted file mode 100644 index 7070b4d..0000000 Binary files a/src/me5413_world/media/gazebo_world.png and /dev/null differ diff --git a/src/me5413_world/media/rqt_reconfig.png b/src/me5413_world/media/rqt_reconfig.png new file mode 100644 index 0000000..2e16b39 Binary files /dev/null and b/src/me5413_world/media/rqt_reconfig.png differ diff --git a/src/me5413_world/media/rviz_manual.png b/src/me5413_world/media/rviz_manual.png deleted file mode 100644 index 290ba95..0000000 Binary files a/src/me5413_world/media/rviz_manual.png and /dev/null differ diff --git a/src/me5413_world/media/rviz_mapping.png b/src/me5413_world/media/rviz_mapping.png deleted file mode 100644 index 3de4a6f..0000000 Binary files a/src/me5413_world/media/rviz_mapping.png and /dev/null differ diff --git a/src/me5413_world/media/rviz_navigation.png b/src/me5413_world/media/rviz_navigation.png deleted file mode 100644 index ebbbd1a..0000000 Binary files a/src/me5413_world/media/rviz_navigation.png and /dev/null differ diff --git a/src/me5413_world/media/rviz_overview.png b/src/me5413_world/media/rviz_overview.png new file mode 100644 index 0000000..20387e5 Binary files /dev/null and b/src/me5413_world/media/rviz_overview.png differ diff --git a/src/me5413_world/media/rviz_panel.png b/src/me5413_world/media/rviz_panel.png deleted file mode 100644 index 88ba400..0000000 Binary files a/src/me5413_world/media/rviz_panel.png and /dev/null differ diff --git a/src/me5413_world/media/rviz_tracking.png b/src/me5413_world/media/rviz_tracking.png new file mode 100644 index 0000000..30672a5 Binary files /dev/null and b/src/me5413_world/media/rviz_tracking.png differ