├── CMakeLists.txt ├── README.md ├── jackal_description ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── launch │ └── description.launch ├── meshes │ ├── accessory_fender.stl │ ├── ark_enclosure.stl │ ├── bridge-plate.stl │ ├── camera-beam.stl │ ├── camera-bracket.stl │ ├── hokuyo_ust10.stl │ ├── hokuyo_utm30.stl │ ├── jackal-base.stl │ ├── jackal-fender.stl │ ├── jackal-fenders.stl │ ├── jackal-wheel.stl │ ├── kinect_mount.stl │ ├── novatel-smart6.stl │ ├── novatel-smart7.stl │ ├── sick-lms1xx-inverted-bracket.stl │ ├── sick-lms1xx-upright-bracket.stl │ ├── stereo-camera-beam.stl │ ├── stereo-camera-bracket.stl │ ├── velodyne_tower.stl │ └── wibotic_bumper.stl ├── package.xml ├── scripts │ ├── env_run │ └── env_run.bat └── urdf │ ├── accessories.urdf.xacro │ ├── accessories │ ├── bridge_plate.urdf.xacro │ ├── camera_mount.urdf.xacro │ ├── hdl32_mount.urdf.xacro │ ├── hokuyo_ust10.urdf.xacro │ ├── hokuyo_utm30.urdf.xacro │ ├── novatel_smart6.urdf.xacro │ ├── novatel_smart7.urdf.xacro │ ├── sick_lms1xx_inverted_mount.urdf.xacro │ ├── sick_lms1xx_upright_mount.urdf.xacro │ ├── standoffs.urdf.xacro │ ├── stereo_camera_mount.urdf.xacro │ └── vlp16_mount.urdf.xacro │ ├── configs │ ├── base │ ├── base.bat │ ├── front_bumblebee2 │ ├── front_bumblebee2.bat │ ├── front_flea3 │ ├── front_flea3.bat │ ├── front_laser │ └── front_laser.bat │ ├── custom_example.urdf │ ├── empty.urdf │ ├── jackal.gazebo │ └── jackal.urdf.xacro └── me5413_world ├── CMakeLists.txt ├── cfg ├── path_publisher.cfg └── path_tracker.cfg ├── include └── me5413_world │ ├── math_utils.hpp │ ├── path_publisher_node.hpp │ ├── path_tracker_MPC_node.hpp │ ├── path_tracker_node.hpp │ └── pid.hpp ├── launch ├── include │ └── spawn_jackal.launch ├── path_tracking.launch ├── path_tracking_MPC.launch └── world.launch ├── media ├── rqt_reconfig.png ├── rviz_overview.png └── rviz_tracking.png ├── package.xml ├── rviz ├── gmapping.rviz ├── manual.rviz └── navigation.rviz ├── src ├── path_publisher_node.cpp ├── path_tracker_MPC_node.cpp └── path_tracker_node.cpp └── worlds └── me5413_planning.world /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Path Tracking in ROS using Pure Pursuit & MPC Algorithm 2 | 3 | ![rviz_overview](https://github.com/KAN201197/Path_Tracking/assets/128454220/a1c0f9b5-7f51-4a2d-8e41-f76909842186) 4 | 5 | This repository implements two different algorithms for path tracking in ROS: Model Predictive Control (MPC) and Pure Pursuit. Both algorithms are designed to guide a robot along a predefined path while maintaining stability and accuracy. 6 | 7 | ## Introduction 8 | 9 | Path tracking is an essential capability for autonomous mobile robots, enabling them to follow a desired trajectory accurately. This repository provides a ROS-based implementation of two popular path tracking algorithms: 10 | - **Model Predictive Control (MPC)** 11 | - **Pure Pursuit** 12 | 13 | ## Features 14 | 15 | - **Model Predictive Control (MPC)**: Uses a mathematical model to predict the future states of the robot (based on horizontal horizon) and optimize control inputs accordingly. 16 | - **Pure Pursuit**: A geometric algorithm that computes the necessary steering angle to reach a lookahead point on the path. 17 | - **PID Controller**: Used to control linear velocity of the robot. 18 | - **ROS Integration**: Fully integrated with ROS for real-time operation. 19 | - **Dynamic Reconfigure**: Allows runtime parameter tuning. 20 | 21 | ## Prerequisites 22 | * System Requirements: 23 | * Ubuntu 20.04 24 | * ROS Noetic 25 | * C++11 and above 26 | * CMake: 3.0.2 and above 27 | * ACADO (can be installed through this link: https://github.com/acado/acado) 28 | * This repo depends on the following standard ROS pkgs: 29 | * `roscpp` 30 | * `rospy` 31 | * `rviz` 32 | * `std_msgs` 33 | * `nav_msgs` 34 | * `geometry_msgs` 35 | * `visualization_msgs` 36 | * `tf2` 37 | * `tf2_ros` 38 | * `tf2_eigen` 39 | * `tf2_geometry_msgs` 40 | * `gazebo_ros` 41 | * `jsk_rviz_plugins` 42 | * `jackal_gazebo` 43 | * `jackal_navigation` 44 | * `velodyne_simulator` 45 | * `dynamic_reconfigure` 46 | 47 | ## Building the Package (Installation) 48 | 49 | This repo is a ros workspace, containing two rospkgs: 50 | * `me5413_world` the main pkg containing the gazebo world, source code, and the launch files 51 | * `jackal_description` contains the modified jackal robot model descriptions 52 | 53 | 1. Build ROS workspace and Clone the repository: 54 | 55 | ```bash 56 | mkdir ~catkin_ws/src -p 57 | 58 | cd catkin_ws/src 59 | 60 | git clone https://github.com/KAN201197/Path_Tracking.git 61 | 62 | cd .. 63 | 64 | rosdep update 65 | 66 | rosdep install --from-paths src --ignore-src -r -y 67 | ``` 68 | 69 | 2. Build the package using `catkin_make`: 70 | 71 | ```bash 72 | cd ~/catkin_ws 73 | 74 | catkin_make 75 | 76 | source devel/setup.bash 77 | ``` 78 | 79 | ## Usage 80 | 81 | ### Running the Nodes 82 | 83 | 1. Launch the ROS master: 84 | 85 | ```bash 86 | roscore 87 | ``` 88 | 89 | 2. In a new terminal, launch the gazebo world: 90 | 91 | ```bash 92 | roslaunch me5413_world world.launch 93 | ``` 94 | ![image](https://github.com/KAN201197/Path_Tracking/assets/128454220/bfe37ae9-bece-4b41-ba05-9e9761253460) 95 | 96 | 4. In a new terminal, launch the MPC path tracker: 97 | 98 | ```bash 99 | roslaunch me5413_world path_tracking_MPC.launch 100 | ``` 101 | 102 | 5. If you want to run other path tracking algorithm, launch the Pure Pursuit path tracker: 103 | 104 | ```bash 105 | roslaunch me5413_world path_tracking.launch 106 | ``` 107 | ![2024-05-1420-16-16-ezgif com-video-to-gif-converter](https://github.com/KAN201197/Path_Tracking/assets/128454220/76e790c6-0883-49c9-8a6a-5b261d840b5e) 108 | 109 | ## Configuration 110 | 111 | ![image](https://github.com/KAN201197/Path_Tracking/assets/128454220/4e8e2954-4bd2-46b9-92f5-ccc425527b8f) 112 | 113 | The dynamic reconfigure server allows you to adjust parameters at runtime. You can use rqt_reconfigure to tune the following parameters: 114 | 115 | - speed_target: Desired speed of the robot. 116 | - robot_length: Length of the robot for kinematic modeling. 117 | - PID_Kp, PID_Ki, PID_Kd: PID controller gains. 118 | - lookahead_distance: Distance to the lookahead point. 119 | -------------------------------------------------------------------------------- /jackal_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package jackal_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.8.6 (2022-11-16) 6 | ------------------ 7 | * Set the GPS plugin's reference heading to 90 so it's ENU 8 | * Use xacro properties defined from environment variables for Microstrain URDF (`#123 `_) 9 | * Add GAZEBO_WORLD\_{LAT|LON} envars to change the reference coordinate of the robot's integral GPS 10 | * Contributors: Chris Iverach-Brereton, Joey Yang 11 | 12 | 0.8.5 (2022-05-17) 13 | ------------------ 14 | * Added Blackfly entry to URDF 15 | * Added Blackfly description to package.xml 16 | * Contributors: Luis Camero 17 | 18 | 0.8.4 (2022-05-09) 19 | ------------------ 20 | 21 | 0.8.3 (2022-03-08) 22 | ------------------ 23 | * Added the option to remove tower from VLP16 mount 24 | * Added SICK TIM551 to URDF and package.xml 25 | * Added UTM30 (`#106 `_) 26 | * Updated Navsat and LMS1xx mounts (`#103 `_) 27 | * Updated hokuyo_ust10_mount to include min and max angle 28 | * Removed extra spaces 29 | * Updated SICK LMS1XX mount and NAVSAT mount 30 | * Maintained backward compatibility with LMS1xx standard upright poisition by adding mount types 31 | * Updated hokuyo_ust10_mount to include min and max angle (`#102 `_) 32 | * Updated hokuyo_ust10_mount to include min and max angle 33 | * Removed extra spaces 34 | * Contributors: Luis Camero, luis-camero 35 | 36 | 0.8.2 (2022-02-15) 37 | ------------------ 38 | * Moved microstrain link to accessories.urdf and updated envvars 39 | * Added velodyne tower mesh 40 | * Added Microstrain GX5 to description 41 | * Removed unnecessary URDF 42 | * Added Wibotic mesh and STL 43 | * Contributors: Luis Camero 44 | 45 | 0.8.1 (2022-01-18) 46 | ------------------ 47 | * Updated to match melodic-devel 48 | * Contributors: Luis Camero 49 | 50 | 0.8.0 (2021-04-23) 51 | ------------------ 52 | 53 | 0.7.5 (2021-03-24) 54 | ------------------ 55 | * Add the origin block to the fender UST-10 macros; otherwise enabling them crashes 56 | * Contributors: Chris I-B 57 | 58 | 0.7.4 (2021-03-16) 59 | ------------------ 60 | * Bumped CMake version to avoid author warning. 61 | * Contributors: Tony Baltovski 62 | 63 | 0.7.3 (2021-03-08) 64 | ------------------ 65 | * Add VLP16 support, refactor main/secondary laser envar support (#79) 66 | * Contributors: Chris I-B 67 | 68 | 0.7.2 (2020-09-29) 69 | ------------------ 70 | 71 | 0.7.1 (2020-08-24) 72 | ------------------ 73 | 74 | 0.7.0 (2020-04-20) 75 | ------------------ 76 | * [jackal_description] Re-added pointgrey_camera_description as run depend. 77 | * Contributors: Tony Baltovski 78 | 79 | 0.6.4 (2020-03-04) 80 | ------------------ 81 | * Modify the hokuyo accessory so that it works properly in gazebo/rviz. Add an additional environment var JACKAL_LASER_HOKUYO which overrides the default lms1xx sensor with the ust10. 82 | * use env_run.bat on Windows (`#3 `_) 83 | * add setlocal 84 | * Fix jackal_description install location & fold xacro includes (`#2 `_) 85 | * Fix install location. 86 | * Fold xacro includes 87 | * add env-hook batch scripts (`#1 `_) 88 | * Contributors: Chris I-B, James Xu, Sean Yen, Tony Baltovski 89 | 90 | 0.6.3 (2019-07-18) 91 | ------------------ 92 | * Added all extra fender changes 93 | * Contributors: Dave Niewinski 94 | 95 | * Made minor changes to syntax for kinetic warnings 96 | * Contributors: Dave Niewinski 97 | 98 | * Added stereo camera accessory. 99 | * Removed unused variable jackal_description_dir 100 | * Make urdf refer explicitly to jackal_description, rather than relying on current working directory being correct, for easier external includes 101 | * Contributors: Arnold Kalmbach, Tony Baltovski, akalmbach 102 | 103 | 0.5.1 (2015-02-02) 104 | ------------------ 105 | * Modified the accessories.urdf.xacro to include both the GPS and mount plate, including standoffs. 106 | * Eliminate rosrun from the xacro wrapper. 107 | * Contributors: BryceVoort, Mike Purvis 108 | 109 | 0.5.0 (2015-01-20) 110 | ------------------ 111 | * Add hook for custom URDF insertion to jackal.urdf.xacro. 112 | * Add xacro wrapper script to provide some pre-cooked "configs", especially for simulated Jackal. 113 | * Switch to parameterizing URDF with optenv. 114 | * Add laser bracket STL. 115 | * Contributors: Mike Purvis 116 | 117 | 0.4.2 (2015-01-14) 118 | ------------------ 119 | 120 | 0.4.1 (2015-01-07) 121 | ------------------ 122 | 123 | 0.4.0 (2014-12-12) 124 | ------------------ 125 | * add pointgrey camera 126 | * Removed inertial and geometry of the base_link. 127 | * hector gazebo plugin for gps is added. 128 | * hector gazebo plugin for imu sensor is added 129 | * Contributors: Mike Purvis, spourmehr 130 | 131 | 0.3.0 (2014-09-10) 132 | ------------------ 133 | * Add comment about accessory args. 134 | * Add front laser accessory to description. 135 | * Contributors: Mike Purvis 136 | 137 | 0.2.1 (2014-09-10) 138 | ------------------ 139 | 140 | 0.2.0 (2014-09-09) 141 | ------------------ 142 | * Changed physical and collision properties. 143 | * Fixed inertia parameters. Added imu plugin--not working 144 | * Install launch directory. 145 | * Contributors: Mike Purvis, Shokoofeh 146 | 147 | 0.1.1 (2014-09-06) 148 | ------------------ 149 | * Remove unnecessary find packages. 150 | * Contributors: Mike Purvis 151 | 152 | 0.1.0 (2014-09-05) 153 | ------------------ 154 | * Updated description with v0.9 hardware changes. 155 | * Contributors: Mike Purvis 156 | -------------------------------------------------------------------------------- /jackal_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(jackal_description) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roslaunch) 5 | 6 | catkin_package() 7 | 8 | roslaunch_add_file_check(launch/description.launch) 9 | 10 | install(DIRECTORY meshes launch urdf 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 12 | ) 13 | 14 | if(WIN32) 15 | install(PROGRAMS scripts/env_run.bat 16 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 17 | ) 18 | else() 19 | install(PROGRAMS scripts/env_run 20 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 21 | ) 22 | endif() 23 | -------------------------------------------------------------------------------- /jackal_description/README.md: -------------------------------------------------------------------------------- 1 | # Jackal Description 2 | 3 | This packages contains the meshes and URDF of the Jackal robot, its supported sensors, and their supported mounts. 4 | 5 | # Jackal Payloads 6 | 7 | ## Microstrain IMU 8 | ```bash 9 | export JACKAL_IMU_MICROSTRAIN=1 10 | ``` 11 | ###### Launch 12 | ```bash 13 | export JACKAL_IMU_MICROSTRAIN_NAME="microstrain" 14 | ``` 15 | ###### Description 16 | ```bash 17 | export JACKAL_IMU_MICROSTRAIN_LINK="microstrain_link" 18 | export JACKAL_IMU_MICROSTRAIN_PARENT="base_link" 19 | export JACKAL_IMU_MICROSTRAIN_OFFSET="-0.139 0.096 0.100" 20 | export JACKAL_IMU_MICROSTRAIN_RPY="3.14159 0 -1.5707" 21 | ``` 22 | 23 | ## 2D Laser 24 | #### Primary 25 | ```bash 26 | export JACKAL_LASER=1 27 | export JACKAL_LASER_MODEL=lms1xx # or tim551 or ust10 or utm30 28 | ``` 29 | ###### Launch 30 | ```bash 31 | export JACKAL_LASER_TOPIC="front/scan" 32 | export JACKAL_LASER_HOST="192.168.131.20" 33 | ``` 34 | ###### Description 35 | ```bash 36 | export JACKAL_LASER_TOWER=1 37 | export JACKAL_LASER_MOUNT="front" 38 | export JACKAL_LASER_PREFIX="front" 39 | export JACKAL_LASER_PARENT="front_mount" 40 | export JACKAL_LASER_MOUNT_TYPE="upright" # or "inverted" 41 | export JACKAL_LASER_OFFSET="0 0 0" 42 | export JACKAL_LASER_RPY="0 0 0" 43 | ``` 44 | #### Secondary 45 | ```bash 46 | export JACKAL_LASER_SECONDARY=1 47 | export JACKAL_LASER_SECONDARY_MODEL="lms1xx" # or "tim551" or "ust10" or "utm30" 48 | ``` 49 | ###### Launch 50 | ```bash 51 | export JACKAL_LASER_SECONDARY_HOST="192.168.131.21" 52 | export JACKAL_LASER_SECONDARY_TOPIC="rear/scan" 53 | ``` 54 | ###### Description 55 | ```bash 56 | export JACKAL_LASER_SECONDARY_TOWER=1 57 | export JACKAL_LASER_SECONDARY_MOUNT="rear" 58 | export JACKAL_LASER_SECONDARY_PREFIX="rear" 59 | export JACKAL_LASER_SECONDARY_PARENT="rear_mount" 60 | export JACKAL_LASER_SECONDARY_OFFSET="0 0 0" 61 | export JACKAL_LASER_SECONDARY_RPY="0 0 3.13159" 62 | ``` 63 | 64 | ## 3D Laser 65 | ```bash 66 | export JACKAL_LASER_3D=1 67 | ``` 68 | ###### Launch 69 | ```bash 70 | export JACKAL_LASER_3D_HOST="192.168.131.20" 71 | export JACKAL_LASER_3D_TOPIC="mid/points" 72 | ``` 73 | ###### Description 74 | ```bash 75 | export JACKAL_LASER_3D_TOWER=1 76 | export JACKAL_LASER_3D_MOUNT="mid" 77 | export JACKAL_LASER_3D_PREFIX="mid" 78 | export JACKAL_LASER_3D_PARENT="mid_mount" 79 | export JACKAL_LASER_3D_MODEL="vlp16" # or "hdl32e" 80 | export JACKAL_LASER_3D_OFFSET="0 0 0" 81 | export JACKAL_LASER_3D_RPY="0 0 0" 82 | ``` 83 | 84 | ## NAVSAT 85 | ```bash 86 | export JACKAL_NAVSAT=1 87 | ``` 88 | ###### Launch 89 | ```bash 90 | export JACKAL_NAVSAT_PORT="/dev/clearpath/gps" 91 | export JACKAL_NAVSAT_BAUD=57600 92 | export JACKAL_NAVSAT_RTK=0 93 | export JACKAL_NAVSAT_RTK_DEVICE=wlan0 94 | export JACKAL_NAVSAT_RTK_BAUD=57600 95 | ``` 96 | ###### Description 97 | ```bash 98 | export JACKAL_NAVSAT_TOWER=1 99 | export JACKAL_NAVSAT_HEIGHT=0.1 # in meters 100 | export JACKAL_NAVSAT_MOUNT="rear" 101 | export JACKAL_NAVSAT_PREFIX="rear" 102 | export JACKAL_NAVSAT_PARENT="rear_mount" 103 | export JACKAL_NAVSAT_MODEL="smart6" # or "smart7" 104 | export JACKAL_NAVSAT_OFFSET="0 0 0" 105 | export JACKAL_NAVSAT_RPY="0 0 0" 106 | ``` 107 | 108 | ## Pointgrey Flea3 109 | ```bash 110 | export JACKAL_FLEA3=1 111 | ``` 112 | ###### Launch 113 | ```bash 114 | export JACKAL_FLEA3_SERIAL=0 115 | export JACKAL_FLEA3_FRAME_RATE=30 116 | export JACKAL_FLEA3_CALIBRATION=0 117 | ``` 118 | ###### Description 119 | ```bash 120 | export JACKAL_FLEA3_TOWER=1 121 | export JACKAL_FLEA3_TILT=0.5236 122 | export JACKAL_FLEA3_NAME="front" 123 | export JACKAL_FLEA3_MOUNT="front" 124 | export JACKAL_FLEA3_PREFIX="front" 125 | export JACKAL_FLEA3_PARENT="front_mount" 126 | export JACKAL_FLEA3_OFFSET="0 0 0" 127 | export JACKAL_FLEA3_RPY="0 0 0" 128 | ``` 129 | 130 | ## Stereo Pointgrey Flea3 131 | ```bash 132 | export JACKAL_STEREO_FLEA3=1 133 | ``` 134 | ###### Launch 135 | ```bash 136 | export JACKAL_FLEA3_FRAME_RATE=30 137 | export JACKAL_FLEA3_LEFT_SERIAL=0 138 | export JACKAL_FLEA3_LEFT_CALIBRATION=0 139 | export JACKAL_FLEA3_RIGHT_SERIAL=0 140 | export JACKAL_FLEA3_RIGHT_CALIBRATION=0 141 | ``` 142 | ###### Description 143 | ```bash 144 | export JACKAL_STEREO_SEPERATION=0.16 145 | export JACKAL_FLEA3_TILT="0.5236" 146 | export JACKAL_FLEA3_MOUNT="front" 147 | export JACKAL_FLEA3_PREFIX="front" 148 | export JACKAL_FLEA3_PARENT="front_mount" 149 | export JACKAL_FLEA3_LEFT_NAME="front/left" 150 | export JACKAL_FLEA3_RIGHT_NAME="front/right" 151 | export JACKAL_FLEA3_OFFSET="0 0 0" 152 | export JACKAL_FLEA3_RPY="0 0 0" 153 | ``` 154 | 155 | ## Bumblebee2 156 | ```bash 157 | export JACKAL_BB2=1 158 | ``` 159 | ###### Launch 160 | ```bash 161 | export JACKAL_BB2_SERIAL=0 162 | export JACKAL_BB2_CALIBRATION=0 163 | ``` 164 | ###### Description 165 | ```bash 166 | export JACKAL_BB2_TILT=0 167 | export JACKAL_BB2_TOWER=1 168 | export JACKAL_BB2_NAME="front" 169 | export JACKAL_BB2_MOUNT="front" 170 | export JACKAL_BB2_PREFIX="front" 171 | export JACKAL_BB2_PARENT="front_mount" 172 | export JACKAL_BB2_OFFSET="0 0 0" 173 | export JACKAL_BB2_RPY="0 0 0" 174 | ``` 175 | 176 | ## Flir Blackfly 177 | ```bash 178 | export JACKAL_BLACKFLY=1 179 | ``` 180 | ###### Launch 181 | ```bash 182 | export JACKAL_BLACKFLY_SERIAL=0 183 | export JACKAL_BLACKFLY_DEVICE="USB" # or "GigE" 184 | export JACKAL_BLACKFLY_ENCODING="BayerRGB" 185 | export JACKAL_BLACKFLY_FRAMERATE=30 186 | ``` 187 | ###### Description 188 | ```bash 189 | export JACKAL_BLACKFLY_PREFIX="front_camera" 190 | export JACKAL_BLACKFLY_PARENT="front_mount" 191 | export JACKAL_BLACKFLY_OFFSET="0 0 0" 192 | export JACKAL_BLACKFLY_RPY="0 0 0" 193 | ``` 194 | 195 | ## Front and Rear Accessory Fender 196 | ```bash 197 | export JACKAL_FRONT_ACCESSORY_FENDER=1 198 | export JACKAL_REAR_ACCESSORY_FENDER=1 199 | export JACKAL_FRONT_FENDER_UST10=1 200 | export JACKAL_REAR_FENDER_UST10=1 201 | ``` 202 | ###### Launch 203 | ```bash 204 | export JACKAL_FRONT_LASER_TOPIC=front/scan 205 | export JACKAL_FRONT_LASER_HOST="192.168.131.20" 206 | export JACKAL_REAR_LASER_TOPIC=rear/scan 207 | export JACKAL_REAR_LASER_HOST="192.168.131.21" 208 | ``` 209 | 210 | ## Decorations 211 | Description only accessories (i.e. no driver). 212 | ###### Wibotic Q-Charging Bumper: 213 | ```bash 214 | export JACKAL_WIBOTIC_BUMPER=1 215 | ``` 216 | ###### Backpack Computer Enclosure: 217 | ```bash 218 | export JACKAL_ARK_ENCLOSURE=1 219 | ``` -------------------------------------------------------------------------------- /jackal_description/launch/description.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /jackal_description/meshes/accessory_fender.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/accessory_fender.stl -------------------------------------------------------------------------------- /jackal_description/meshes/ark_enclosure.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/ark_enclosure.stl -------------------------------------------------------------------------------- /jackal_description/meshes/bridge-plate.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/bridge-plate.stl -------------------------------------------------------------------------------- /jackal_description/meshes/camera-beam.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/camera-beam.stl -------------------------------------------------------------------------------- /jackal_description/meshes/camera-bracket.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/camera-bracket.stl -------------------------------------------------------------------------------- /jackal_description/meshes/hokuyo_ust10.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/hokuyo_ust10.stl -------------------------------------------------------------------------------- /jackal_description/meshes/hokuyo_utm30.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/hokuyo_utm30.stl -------------------------------------------------------------------------------- /jackal_description/meshes/jackal-base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/jackal-base.stl -------------------------------------------------------------------------------- /jackal_description/meshes/jackal-fender.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/jackal-fender.stl -------------------------------------------------------------------------------- /jackal_description/meshes/jackal-fenders.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/jackal-fenders.stl -------------------------------------------------------------------------------- /jackal_description/meshes/jackal-wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/jackal-wheel.stl -------------------------------------------------------------------------------- /jackal_description/meshes/kinect_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/kinect_mount.stl -------------------------------------------------------------------------------- /jackal_description/meshes/novatel-smart6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/novatel-smart6.stl -------------------------------------------------------------------------------- /jackal_description/meshes/novatel-smart7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/novatel-smart7.stl -------------------------------------------------------------------------------- /jackal_description/meshes/sick-lms1xx-inverted-bracket.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/sick-lms1xx-inverted-bracket.stl -------------------------------------------------------------------------------- /jackal_description/meshes/sick-lms1xx-upright-bracket.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/sick-lms1xx-upright-bracket.stl -------------------------------------------------------------------------------- /jackal_description/meshes/stereo-camera-beam.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/stereo-camera-beam.stl -------------------------------------------------------------------------------- /jackal_description/meshes/stereo-camera-bracket.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/stereo-camera-bracket.stl -------------------------------------------------------------------------------- /jackal_description/meshes/velodyne_tower.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/velodyne_tower.stl -------------------------------------------------------------------------------- /jackal_description/meshes/wibotic_bumper.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/jackal_description/meshes/wibotic_bumper.stl -------------------------------------------------------------------------------- /jackal_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | jackal_description 4 | 0.8.6 5 | URDF robot description for Jackal 6 | 7 | Mike Purvis 8 | 9 | BSD 10 | 11 | Mike Purvis 12 | 13 | catkin 14 | roslaunch 15 | flir_camera_description 16 | robot_state_publisher 17 | urdf 18 | xacro 19 | lms1xx 20 | pointgrey_camera_description 21 | sick_tim 22 | velodyne_description 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /jackal_description/scripts/env_run: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # This simple wrapper allowing us to pass a set of 3 | # environment variables to be sourced prior to running 4 | # another command. Used in the launch file for setting 5 | # robot configurations prior to xacro. 6 | 7 | ENVVARS_FILE=$1 8 | shift 1 9 | 10 | set -a 11 | source $ENVVARS_FILE 12 | $@ 13 | -------------------------------------------------------------------------------- /jackal_description/scripts/env_run.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | setlocal 3 | REM This simple wrapper allowing us to pass a set of 4 | REM environment variables to be sourced prior to running 5 | REM another command. Used in the launch file for setting 6 | REM robot configurations prior to xacro. 7 | 8 | set ENVVARS_FILE=%1 9 | 10 | REM get arguments starting from %2 11 | shift 12 | set RUNNER_COMMAND= 13 | :getrunnerloop 14 | if "%1"=="" goto after_loop 15 | if "%RUNNER_COMMAND%" == "" ( 16 | set RUNNER_COMMAND=%1 17 | ) else ( 18 | set RUNNER_COMMAND=%RUNNER_COMMAND% %1 19 | ) 20 | shift 21 | goto getrunnerloop 22 | 23 | :after_loop 24 | call %ENVVARS_FILE% 25 | call %RUNNER_COMMAND% 26 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 314 | 315 | 316 | 317 | 318 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 342 | 343 | 344 | 345 | 346 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | Gazebo/DarkGrey 496 | false 497 | 498 | 499 | 500 | 501 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/bridge_plate.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/camera_mount.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | Gazebo/DarkGrey 33 | 34 | 35 | 36 | Gazebo/DarkGrey 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/hdl32_mount.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/hokuyo_ust10.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | true 19 | 20 | 0 0 0 0 0 0 21 | false 22 | ${update_rate} 23 | 24 | 25 | 26 | ${sample_size} 27 | 1 28 | ${min_angle} 29 | ${max_angle} 30 | 31 | 32 | 33 | ${min_range} 34 | ${max_range} 35 | 0.01 36 | 37 | 38 | gaussian 39 | 0.0 40 | 0.001 41 | 42 | 43 | 44 | ${topic} 45 | ${frame} 46 | ${robot_namespace} 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | Gazebo/DarkGrey 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/hokuyo_utm30.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | true 22 | 23 | 0 0 0 0 0 0 24 | false 25 | ${update_rate} 26 | 27 | 28 | 29 | ${sample_size} 30 | 1 31 | ${min_angle} 32 | ${max_angle} 33 | 34 | 35 | 36 | ${min_range} 37 | ${max_range} 38 | 0.01 39 | 40 | 41 | gaussian 42 | 0.0 43 | 0.001 44 | 45 | 46 | 47 | ${topic} 48 | ${frame} 49 | ${robot_namespace} 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | Gazebo/DarkGrey 83 | 84 | 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/novatel_smart6.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/novatel_smart7.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/sick_lms1xx_inverted_mount.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | Gazebo/DarkGrey 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/sick_lms1xx_upright_mount.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | Gazebo/DarkGrey 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/standoffs.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/stereo_camera_mount.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | Gazebo/DarkGrey 41 | 42 | 43 | 44 | Gazebo/DarkGrey 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /jackal_description/urdf/accessories/vlp16_mount.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /jackal_description/urdf/configs/base: -------------------------------------------------------------------------------- 1 | # The base Jackal configuration has no accessories at all, 2 | # so nothing need be specified here; the defaults as given 3 | # in the URDF suffice to define this config. 4 | -------------------------------------------------------------------------------- /jackal_description/urdf/configs/base.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | 3 | REM The base Jackal configuration has no accessories at all, 4 | REM so nothing need be specified here; the defaults as given 5 | REM in the URDF suffice to define this config. 6 | -------------------------------------------------------------------------------- /jackal_description/urdf/configs/front_bumblebee2: -------------------------------------------------------------------------------- 1 | 2 | JACKAL_BB2=1 3 | -------------------------------------------------------------------------------- /jackal_description/urdf/configs/front_bumblebee2.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | 3 | set JACKAL_BB2=1 4 | -------------------------------------------------------------------------------- /jackal_description/urdf/configs/front_flea3: -------------------------------------------------------------------------------- 1 | 2 | JACKAL_FLEA3=1 3 | -------------------------------------------------------------------------------- /jackal_description/urdf/configs/front_flea3.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | 3 | set JACKAL_FLEA3=1 4 | -------------------------------------------------------------------------------- /jackal_description/urdf/configs/front_laser: -------------------------------------------------------------------------------- 1 | # The front_laser configuration of Jackal is sufficient for 2 | # basic gmapping and navigation. It is mostly the default 3 | # config, but with a SICK LMS100 series LIDAR on the front, 4 | # pointing forward. 5 | 6 | JACKAL_LASER=1 7 | -------------------------------------------------------------------------------- /jackal_description/urdf/configs/front_laser.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | 3 | REM The front_laser configuration of Jackal is sufficient for 4 | REM basic gmapping and navigation. It is mostly the default 5 | REM config, but with a SICK LMS100 series LIDAR on the front, 6 | REM pointing forward. 7 | 8 | set JACKAL_LASER=1 9 | -------------------------------------------------------------------------------- /jackal_description/urdf/custom_example.urdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | > 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /jackal_description/urdf/empty.urdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | -------------------------------------------------------------------------------- /jackal_description/urdf/jackal.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | true 6 | 50.0 7 | base_link 8 | /gazebo/ground_truth/state 9 | 0.0 10 | world 11 | 0 0 0 12 | 0 0 0 13 | 14 | 15 | 16 | 17 | 18 | / 19 | 20 | 21 | 22 | 23 | 24 | / 25 | 50.0 26 | imu_link 27 | imu/data 28 | 0.005 0.005 0.005 29 | 0.005 0.005 0.005 30 | 0.005 0.005 0.005 31 | 0.005 0.005 0.005 32 | 0.005 33 | 0.005 34 | 35 | 36 | 37 | 38 | 39 | 40 40 | / 41 | navsat_link 42 | base_link 43 | /navsat/fix 44 | /navsat/vel 45 | $(optenv GAZEBO_WORLD_LAT 49.9) 46 | $(optenv GAZEBO_WORLD_LON 8.9) 47 | 90 48 | 0 49 | 0.0001 0.0001 0.0001 50 | 51 | 52 | 53 | 54 | false 55 | 56 | 57 | 58 | Gazebo/DarkGrey 59 | false 60 | 61 | 62 | false 63 | 64 | 65 | 66 | Gazebo/DarkGrey 67 | false 68 | 69 | 70 | Gazebo/Yellow 71 | false 72 | 73 | 74 | Gazebo/Yellow 75 | false 76 | 77 | 78 | -------------------------------------------------------------------------------- /jackal_description/urdf/jackal.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 49 | 50 | 51 | 52 | 53 | Gazebo/DarkGrey 54 | false 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 71 | 72 | transmission_interface/SimpleTransmission 73 | 74 | hardware_interface/VelocityJointInterface 75 | 76 | 77 | 1 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | -------------------------------------------------------------------------------- /me5413_world/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(me5413_world) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | find_package(catkin REQUIRED COMPONENTS 9 | rospy 10 | roscpp 11 | rviz 12 | std_msgs 13 | nav_msgs 14 | geometry_msgs 15 | tf2 16 | tf2_ros 17 | tf2_eigen 18 | tf2_geometry_msgs 19 | gazebo_ros 20 | jsk_rviz_plugins 21 | jackal_gazebo 22 | jackal_navigation 23 | dynamic_reconfigure 24 | ) 25 | 26 | ## Find ACADO 27 | find_package(ACADO REQUIRED) 28 | 29 | if(ACADO_FOUND) 30 | message(STATUS "Found ACADO version: ${ACADO_VERSION}") 31 | else() 32 | message(FATAL_ERROR "ACADO not found. Please install ACADO.") 33 | endif() 34 | 35 | ## Generate dynamic reconfigure options 36 | generate_dynamic_reconfigure_options( 37 | cfg/path_publisher.cfg 38 | cfg/path_tracker.cfg 39 | ) 40 | 41 | ## Declare the catkin package 42 | catkin_package( 43 | INCLUDE_DIRS include 44 | LIBRARIES me5413_world 45 | CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs nav_msgs dynamic_reconfigure 46 | DEPENDS system_lib 47 | ) 48 | 49 | ## Include directories 50 | include_directories( 51 | include 52 | ${catkin_INCLUDE_DIRS} 53 | ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake 54 | ${ACADO_INCLUDE_DIRS} 55 | ) 56 | 57 | ## Link directories 58 | link_directories(${ACADO_LIBRARY_DIRS}) 59 | 60 | ## Add Nodes 61 | add_executable(path_publisher_node src/path_publisher_node.cpp) 62 | target_link_libraries(path_publisher_node ${catkin_LIBRARIES}) 63 | add_dependencies(path_publisher_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 64 | 65 | add_executable(path_tracker_node src/path_tracker_node.cpp) 66 | target_link_libraries(path_tracker_node ${catkin_LIBRARIES}) 67 | add_dependencies(path_tracker_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 68 | 69 | add_executable(path_tracker_MPC_node src/path_tracker_MPC_node.cpp) 70 | target_link_libraries(path_tracker_MPC_node ${catkin_LIBRARIES} ${ACADO_LIBRARIES}) 71 | add_dependencies(path_tracker_MPC_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 72 | -------------------------------------------------------------------------------- /me5413_world/cfg/path_publisher.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "me5413_world" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("speed_target", double_t, 1, "Default: 0.5[m/s]", 0.5, 0.1, 1.0) 9 | 10 | gen.add("track_A_axis", double_t, 1, "Default: 8.0", 8.0, 1.0, 15.0) 11 | gen.add("track_B_axis", double_t, 1, "Default: 8.0", 8.0, 1.0, 15.0) 12 | gen.add("track_wp_num", int_t, 1, "Default: 500", 500, 100, 2000) 13 | gen.add("local_prev_wp_num", int_t, 1, "Default: 10", 10, 1, 20) 14 | gen.add("local_next_wp_num", int_t, 1, "Default: 50", 50, 5, 200) 15 | 16 | exit(gen.generate(PACKAGE, "path_publisher_node", "path_publisher")) -------------------------------------------------------------------------------- /me5413_world/cfg/path_tracker.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "me5413_world" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("speed_target", double_t, 1, "Default: 0.5[m/s]", 0.5, 0.1, 1.0) 9 | gen.add("PID_Kp", double_t, 1, "Default: 0.15", 0.5, 0, 10.0) 10 | gen.add("PID_Ki", double_t, 1, "Default: 0.01", 0.2, 0, 10.0) 11 | gen.add("PID_Kd", double_t, 1, "Default: 0.0", 0.2, 0, 10.0) 12 | gen.add("robot_length", double_t, 1, "Length of the robot (for pure pursuit algorithm). Default: 0.5", 0.5, 0.1, 10.0) 13 | gen.add("lookahead_distance", double_t, 1, "Default lookahead distance. Default: 0.5", 0.5, 0.1, 10.0) 14 | 15 | exit(gen.generate(PACKAGE, "path_tracker_MPC_node", "path_tracker")) 16 | -------------------------------------------------------------------------------- /me5413_world/include/me5413_world/math_utils.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace me5413_world 7 | { 8 | 9 | // Return PI 10 | constexpr double pi() { return M_PI; } 11 | 12 | // Convert degrees to radians 13 | inline double deg2rad(const double x) { return x * pi() / 180; } 14 | 15 | // Convert radians to degrees 16 | inline double rad2deg(const double x) { return x * 180 / pi(); } 17 | 18 | // Convert metre per second to kilometers per hour 19 | inline double mps2kph(const double x) { return x * 3.6; } 20 | 21 | // Convert kilometers per hour to meter per second 22 | inline double kph2mps(const double x) { return x / 3.6; } 23 | 24 | // Convert angle into range [-pi, +pi] 25 | inline double unifyAngleRange(const double angle) 26 | { 27 | auto new_angle = angle; 28 | while (new_angle > pi()) 29 | { 30 | new_angle -= 2 * pi(); 31 | } 32 | while (new_angle < -pi()) 33 | { 34 | new_angle += 2 * pi(); 35 | } 36 | return new_angle; 37 | } 38 | 39 | // Limit the value within [lower_bound, upper_bound] 40 | inline double limitWithinRange(const double value, const double lower_bound, const double upper_bound) 41 | { 42 | auto new_value = std::max(value, lower_bound); 43 | new_value = std::min(new_value, upper_bound); 44 | return new_value; 45 | } 46 | 47 | // Check if a value is legal (not nan or inf) 48 | inline bool isLegal(const double x) 49 | { 50 | return (std::isnan(x) || std::isinf(x))? false : true; 51 | } 52 | 53 | } // end of namespace me5413_world 54 | -------------------------------------------------------------------------------- /me5413_world/include/me5413_world/path_publisher_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PATH_PUBLISHER_NODE_H_ 2 | #define PATH_PUBLISHER_NODE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace me5413_world 30 | { 31 | 32 | class PathPublisherNode 33 | { 34 | public: 35 | PathPublisherNode(); 36 | virtual ~PathPublisherNode(){}; 37 | 38 | private: 39 | void timerCallback(const ros::TimerEvent &); 40 | void robotOdomCallback(const nav_msgs::Odometry::ConstPtr &odom); 41 | void publishGlobalPath(); 42 | void publishLocalPath(const geometry_msgs::Pose &robot_pose, const int n_wp_prev, const int n_wp_post); 43 | 44 | std::vector createGlobalPath(const double A, const double B, const double t_res); 45 | int closestWaypoint(const geometry_msgs::Pose &robot_pose, const nav_msgs::Path &path, const int id_start); 46 | int nextWaypoint(const geometry_msgs::Pose &robot_pose, const nav_msgs::Path &path, const int id_start); 47 | double getYawFromOrientation(const geometry_msgs::Quaternion &orientation); 48 | tf2::Transform convertPoseToTransform(const geometry_msgs::Pose &pose); 49 | std::pair calculatePoseError(const geometry_msgs::Pose &pose_robot, const geometry_msgs::Pose &pose_goal); 50 | 51 | // ROS declaration 52 | ros::NodeHandle nh_; 53 | ros::Timer timer_; 54 | tf2_ros::Buffer tf2_buffer_; 55 | tf2_ros::TransformListener tf2_listener_; 56 | tf2_ros::TransformBroadcaster tf2_bcaster_; 57 | dynamic_reconfigure::Server server; 58 | dynamic_reconfigure::Server::CallbackType f; 59 | 60 | ros::Subscriber sub_robot_odom_; 61 | 62 | ros::Publisher pub_global_path_; 63 | ros::Publisher pub_local_path_; 64 | ros::Publisher pub_abs_position_error_; 65 | ros::Publisher pub_abs_heading_error_; 66 | ros::Publisher pub_abs_speed_error_; 67 | ros::Publisher pub_rms_position_error_; 68 | ros::Publisher pub_rms_heading_error_; 69 | ros::Publisher pub_rms_speed_error_; 70 | 71 | // Robot pose 72 | std::string world_frame_; 73 | std::string robot_frame_; 74 | 75 | geometry_msgs::Pose pose_world_goal_; 76 | nav_msgs::Odometry odom_world_robot_; 77 | 78 | nav_msgs::Path global_path_msg_; 79 | nav_msgs::Path local_path_msg_; 80 | 81 | std_msgs::Float32 abs_position_error_; 82 | std_msgs::Float32 abs_heading_error_; 83 | std_msgs::Float32 abs_speed_error_; 84 | std_msgs::Float32 rms_position_error_; 85 | std_msgs::Float32 rms_heading_error_; 86 | std_msgs::Float32 rms_speed_error_; 87 | 88 | int current_id_; 89 | long long num_time_steps_; 90 | double sum_sqr_position_error_; 91 | double sum_sqr_heading_error_; 92 | double sum_sqr_speed_error_; 93 | }; 94 | 95 | } // namespace me5413_world 96 | 97 | #endif // PATH_PUBLISHER_NODE_H_ -------------------------------------------------------------------------------- /me5413_world/include/me5413_world/path_tracker_MPC_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PATH_TRACKER_NODE_H_ 2 | #define PATH_TRACKER_NODE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include "angles/angles.h" 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include "me5413_world/pid.hpp" 33 | #include 34 | #include 35 | 36 | namespace me5413_world 37 | { 38 | 39 | class PathTrackerNode 40 | { 41 | public: 42 | PathTrackerNode(); 43 | virtual ~PathTrackerNode() {}; 44 | 45 | private: 46 | void robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom); 47 | void localPathCallback(const nav_msgs::Path::ConstPtr& path); 48 | tf2::Transform convertPoseToTransform(const geometry_msgs::Pose& pose); 49 | geometry_msgs::Twist computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal); 50 | 51 | // ROS declaration 52 | ros::NodeHandle nh_; 53 | ros::Subscriber sub_robot_odom_; 54 | ros::Subscriber sub_local_path_; 55 | ros::Publisher pub_cmd_vel_; 56 | 57 | tf2_ros::Buffer tf2_buffer_; 58 | tf2_ros::TransformListener tf2_listener_; 59 | tf2_ros::TransformBroadcaster tf2_bcaster_; 60 | dynamic_reconfigure::Server server; 61 | dynamic_reconfigure::Server::CallbackType f; 62 | 63 | // Robot pose 64 | std::string world_frame_; 65 | std::string robot_frame_; 66 | nav_msgs::Odometry odom_world_robot_; 67 | geometry_msgs::Pose pose_world_goal_; 68 | 69 | // Controllers 70 | control::PID pid_; 71 | 72 | ACADO::VariablesGrid x_init; 73 | ACADO::VariablesGrid u_init; 74 | 75 | void setupMPC(ACADO::VariablesGrid& x_init, ACADO::VariablesGrid& u_init, const geometry_msgs::Pose& pose_goal); 76 | ACADO::VariablesGrid runMPC(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal); 77 | }; 78 | 79 | } // namespace me5413_world 80 | 81 | #endif // PATH_TRACKER_NODE_H_ -------------------------------------------------------------------------------- /me5413_world/include/me5413_world/path_tracker_node.hpp: -------------------------------------------------------------------------------- 1 | /** path_tracker_node.hpp 2 | * 3 | * Copyright (C) 2024 Shuo SUN & Advanced Robotics Center, National University of Singapore 4 | * 5 | * MIT License 6 | * 7 | * Declarations for PathTrackerNode class 8 | */ 9 | 10 | #ifndef PATH_TRACKER_NODE_H_ 11 | #define PATH_TRACKER_NODE_H_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include "angles/angles.h" 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include "me5413_world/pid.hpp" 41 | 42 | namespace me5413_world 43 | { 44 | 45 | class PathTrackerNode 46 | { 47 | public: 48 | PathTrackerNode(); 49 | virtual ~PathTrackerNode() {}; 50 | 51 | private: 52 | void robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom); 53 | void localPathCallback(const nav_msgs::Path::ConstPtr& path); 54 | 55 | tf2::Transform convertPoseToTransform(const geometry_msgs::Pose& pose); 56 | geometry_msgs::Twist computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal); 57 | double computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal); 58 | double computeLookaheadDistance(const nav_msgs::Odometry& odom_robot); 59 | //tf2::Vector3 findClosestPointOnPath(const tf2::Vector3& point_robot, const std::vector& path_points, double lookahead_distance); 60 | 61 | // ROS declaration 62 | ros::NodeHandle nh_; 63 | ros::Subscriber sub_robot_odom_; 64 | ros::Subscriber sub_local_path_; 65 | ros::Publisher pub_cmd_vel_; 66 | 67 | tf2_ros::Buffer tf2_buffer_; 68 | tf2_ros::TransformListener tf2_listener_; 69 | tf2_ros::TransformBroadcaster tf2_bcaster_; 70 | dynamic_reconfigure::Server server; 71 | dynamic_reconfigure::Server::CallbackType f; 72 | 73 | // Robot pose 74 | std::string world_frame_; 75 | std::string robot_frame_; 76 | nav_msgs::Odometry odom_world_robot_; 77 | geometry_msgs::Pose pose_world_goal_; 78 | 79 | // Controllers 80 | control::PID pid_; 81 | 82 | // std::vector path_points_; 83 | }; 84 | 85 | } // namespace me5413_world 86 | 87 | #endif // PATH_TRACKER_NODE_H_ -------------------------------------------------------------------------------- /me5413_world/include/me5413_world/pid.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace control 7 | { 8 | class PID 9 | { 10 | public: 11 | PID() {}; 12 | PID(double dt, double max, double min, double Kp, double Kd, double Ki); 13 | ~PID() {}; 14 | 15 | void updateSettings(const double Kp, const double Kd, const double Ki); 16 | // Returns the manipulated variable given a setpoint and current process value 17 | double calculate(const double setpoint, const double pv); 18 | 19 | private: 20 | double dt_; 21 | double max_; 22 | double min_; 23 | double Kp_; 24 | double Kd_; 25 | double Ki_; 26 | double pre_error_; 27 | double integral_; 28 | }; 29 | 30 | PID::PID(double dt, double max, double min, double Kp, double Kd, double Ki) : 31 | dt_(dt), 32 | max_(max), 33 | min_(min), 34 | Kp_(Kp), 35 | Kd_(Kd), 36 | Ki_(Ki), 37 | pre_error_(0), 38 | integral_(0) 39 | {}; 40 | 41 | void PID::updateSettings(const double Kp, const double Kd, const double Ki) 42 | { 43 | this->Kp_ = Kp; 44 | this->Kd_ = Kd; 45 | this->Ki_ = Ki; 46 | }; 47 | 48 | double PID::calculate(const double setpoint, const double pv) 49 | { 50 | // Calculate error 51 | double error = setpoint - pv; 52 | 53 | // Proportional term 54 | const double P_term = Kp_ * error; 55 | 56 | // Integral term 57 | integral_ += error * dt_; 58 | const double I_term = Ki_ * integral_; 59 | 60 | // Derivative term 61 | const double derivative = (error - pre_error_) / dt_; 62 | const double D_term = Kd_ * derivative; 63 | 64 | // Calculate total output 65 | double output = P_term + I_term + D_term; 66 | 67 | // Restrict to max/min 68 | output = std::min(output, max_); 69 | output = std::max(output, min_); 70 | 71 | // Save error to previous error 72 | pre_error_ = error; 73 | 74 | return output; 75 | }; 76 | 77 | } // namespace control -------------------------------------------------------------------------------- /me5413_world/launch/include/spawn_jackal.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /me5413_world/launch/path_tracking.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /me5413_world/launch/path_tracking_MPC.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /me5413_world/launch/world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /me5413_world/media/rqt_reconfig.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/me5413_world/media/rqt_reconfig.png -------------------------------------------------------------------------------- /me5413_world/media/rviz_overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/me5413_world/media/rviz_overview.png -------------------------------------------------------------------------------- /me5413_world/media/rviz_tracking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KAN201197/Path_Tracking_with_ROS/f326feee9ec880060d28bc87d15ea6eea9cfc02f/me5413_world/media/rviz_tracking.png -------------------------------------------------------------------------------- /me5413_world/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | me5413_world 4 | 0.0.0 5 | The me5413_world package 6 | 7 | 8 | 9 | 10 | shuo 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | rospy 54 | roscpp 55 | rviz 56 | std_msgs 57 | nav_msgs 58 | geometry_msgs 59 | tf2 60 | tf2_ros 61 | tf2_eigen 62 | tf2_geometry_msgs 63 | gazebo_ros 64 | jsk_rviz_plugins 65 | jackal_gazebo 66 | jackal_navigation 67 | velodyne_simulator 68 | dynamic_reconfigure 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /me5413_world/rviz/gmapping.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Sensing1 10 | - /Localization1 11 | Splitter Ratio: 0.5117647051811218 12 | Tree Height: 420 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.6096654534339905 27 | - Class: rviz/Time 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: PointCloud2 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Class: rviz/RobotModel 58 | Collision Enabled: false 59 | Enabled: true 60 | Links: 61 | All Links Enabled: true 62 | Expand Joint Details: false 63 | Expand Link Details: false 64 | Expand Tree: false 65 | Link Tree Style: Links in Alphabetic Order 66 | base_link: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | chassis_link: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | Value: true 75 | front_camera: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | Value: true 80 | front_camera_beam: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | front_camera_mount: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | front_camera_optical: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | front_fender_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | front_left_wheel_link: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | front_mount: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | front_right_wheel_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | imu_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | mid_mount: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | mid_vlp16_leg1: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | Value: true 126 | mid_vlp16_leg2: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | Value: true 131 | mid_vlp16_leg3: 132 | Alpha: 1 133 | Show Axes: false 134 | Show Trail: false 135 | Value: true 136 | mid_vlp16_leg4: 137 | Alpha: 1 138 | Show Axes: false 139 | Show Trail: false 140 | Value: true 141 | mid_vlp16_mount: 142 | Alpha: 1 143 | Show Axes: false 144 | Show Trail: false 145 | mid_vlp16_plate: 146 | Alpha: 1 147 | Show Axes: false 148 | Show Trail: false 149 | Value: true 150 | navsat_link: 151 | Alpha: 1 152 | Show Axes: false 153 | Show Trail: false 154 | Value: true 155 | rear_fender_link: 156 | Alpha: 1 157 | Show Axes: false 158 | Show Trail: false 159 | Value: true 160 | rear_left_wheel_link: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | Value: true 165 | rear_mount: 166 | Alpha: 1 167 | Show Axes: false 168 | Show Trail: false 169 | rear_right_wheel_link: 170 | Alpha: 1 171 | Show Axes: false 172 | Show Trail: false 173 | Value: true 174 | tim551: 175 | Alpha: 1 176 | Show Axes: false 177 | Show Trail: false 178 | Value: true 179 | tim551_mount_link: 180 | Alpha: 1 181 | Show Axes: false 182 | Show Trail: false 183 | velodyne: 184 | Alpha: 1 185 | Show Axes: false 186 | Show Trail: false 187 | Value: true 188 | velodyne_base_link: 189 | Alpha: 1 190 | Show Axes: false 191 | Show Trail: false 192 | Value: true 193 | Name: RobotModel 194 | Robot Description: robot_description 195 | TF Prefix: "" 196 | Update Interval: 0 197 | Value: true 198 | Visual Enabled: true 199 | - Class: rviz/InteractiveMarkers 200 | Enable Transparency: true 201 | Enabled: true 202 | Name: InteractiveMarkers 203 | Show Axes: false 204 | Show Descriptions: true 205 | Show Visual Aids: false 206 | Update Topic: /twist_marker_server/update 207 | Value: true 208 | - Class: rviz/Group 209 | Displays: 210 | - Acceleration properties: 211 | Acc. vector alpha: 1 212 | Acc. vector color: 255; 0; 0 213 | Acc. vector scale: 1 214 | Derotate acceleration: true 215 | Enable acceleration: false 216 | Axes properties: 217 | Axes scale: 1 218 | Enable axes: true 219 | Box properties: 220 | Box alpha: 1 221 | Box color: 255; 0; 0 222 | Enable box: false 223 | x_scale: 1 224 | y_scale: 1 225 | z_scale: 1 226 | Class: rviz_imu_plugin/Imu 227 | Enabled: true 228 | Name: Imu 229 | Queue Size: 10 230 | Topic: /imu/data 231 | Unreliable: false 232 | Value: true 233 | fixed_frame_orientation: true 234 | - Class: rviz/Image 235 | Enabled: true 236 | Image Topic: /front/image_raw 237 | Max Value: 1 238 | Median window: 5 239 | Min Value: 0 240 | Name: Image 241 | Normalize Range: true 242 | Queue Size: 2 243 | Transport Hint: raw 244 | Unreliable: false 245 | Value: true 246 | - Alpha: 1 247 | Autocompute Intensity Bounds: true 248 | Autocompute Value Bounds: 249 | Max Value: 9.14406681060791 250 | Min Value: -0.8228924870491028 251 | Value: true 252 | Axis: X 253 | Channel Name: intensity 254 | Class: rviz/LaserScan 255 | Color: 255; 255; 255 256 | Color Transformer: AxisColor 257 | Decay Time: 0 258 | Enabled: true 259 | Invert Rainbow: false 260 | Max Color: 255; 255; 255 261 | Min Color: 0; 0; 0 262 | Name: LaserScan 263 | Position Transformer: XYZ 264 | Queue Size: 10 265 | Selectable: true 266 | Size (Pixels): 3 267 | Size (m): 0.05000000074505806 268 | Style: Spheres 269 | Topic: /front/scan 270 | Unreliable: false 271 | Use Fixed Frame: true 272 | Use rainbow: true 273 | Value: true 274 | - Alpha: 1 275 | Autocompute Intensity Bounds: true 276 | Autocompute Value Bounds: 277 | Max Value: 4.77785587310791 278 | Min Value: -0.07101771235466003 279 | Value: true 280 | Axis: Z 281 | Channel Name: intensity 282 | Class: rviz/PointCloud2 283 | Color: 255; 255; 255 284 | Color Transformer: AxisColor 285 | Decay Time: 0 286 | Enabled: true 287 | Invert Rainbow: false 288 | Max Color: 255; 255; 255 289 | Min Color: 0; 0; 0 290 | Name: PointCloud2 291 | Position Transformer: XYZ 292 | Queue Size: 10 293 | Selectable: true 294 | Size (Pixels): 3 295 | Size (m): 0.019999999552965164 296 | Style: Points 297 | Topic: /mid/points 298 | Unreliable: false 299 | Use Fixed Frame: true 300 | Use rainbow: true 301 | Value: true 302 | Enabled: true 303 | Name: Sensing 304 | - Class: rviz/Group 305 | Displays: 306 | - Alpha: 0.699999988079071 307 | Class: rviz/Map 308 | Color Scheme: map 309 | Draw Behind: true 310 | Enabled: true 311 | Name: Map 312 | Topic: /map 313 | Unreliable: false 314 | Use Timestamp: false 315 | Value: true 316 | - Alpha: 1 317 | Arrow Length: 0.30000001192092896 318 | Axes Length: 0.30000001192092896 319 | Axes Radius: 0.009999999776482582 320 | Class: rviz/PoseArray 321 | Color: 255; 25; 0 322 | Enabled: true 323 | Head Length: 0.07000000029802322 324 | Head Radius: 0.029999999329447746 325 | Name: PoseArray 326 | Queue Size: 10 327 | Shaft Length: 0.23000000417232513 328 | Shaft Radius: 0.009999999776482582 329 | Shape: Arrow (Flat) 330 | Topic: /particlecloud 331 | Unreliable: false 332 | Value: true 333 | Enabled: true 334 | Name: Localization 335 | Enabled: true 336 | Global Options: 337 | Background Color: 48; 48; 48 338 | Default Light: true 339 | Fixed Frame: base_link 340 | Frame Rate: 30 341 | Name: root 342 | Tools: 343 | - Class: rviz/Interact 344 | Hide Inactive Objects: true 345 | - Class: rviz/MoveCamera 346 | - Class: rviz/Select 347 | - Class: rviz/FocusCamera 348 | - Class: rviz/Measure 349 | - Class: rviz/SetInitialPose 350 | Theta std deviation: 0.2617993950843811 351 | Topic: /initialpose 352 | X std deviation: 0.5 353 | Y std deviation: 0.5 354 | - Class: rviz/SetGoal 355 | Topic: /move_base_simple/goal 356 | - Class: rviz/PublishPoint 357 | Single click: true 358 | Topic: /clicked_point 359 | Value: true 360 | Views: 361 | Current: 362 | Class: rviz/ThirdPersonFollower 363 | Distance: 4 364 | Enable Stereo Rendering: 365 | Stereo Eye Separation: 0.05999999865889549 366 | Stereo Focal Distance: 1 367 | Swap Stereo Eyes: false 368 | Value: false 369 | Field of View: 0.8999999761581421 370 | Focal Point: 371 | X: 0 372 | Y: 0 373 | Z: 0 374 | Focal Shape Fixed Size: true 375 | Focal Shape Size: 0.05000000074505806 376 | Invert Z Axis: false 377 | Name: Current View 378 | Near Clip Distance: 0.009999999776482582 379 | Pitch: 0.6000000238418579 380 | Target Frame: base_link 381 | Yaw: 3.1500000953674316 382 | Saved: 383 | - Class: rviz/ThirdPersonFollower 384 | Distance: 4 385 | Enable Stereo Rendering: 386 | Stereo Eye Separation: 0.05999999865889549 387 | Stereo Focal Distance: 1 388 | Swap Stereo Eyes: false 389 | Value: false 390 | Field of View: 0.8999999761581421 391 | Focal Point: 392 | X: 0 393 | Y: 0 394 | Z: 0 395 | Focal Shape Fixed Size: true 396 | Focal Shape Size: 0.05000000074505806 397 | Invert Z Axis: false 398 | Name: ThirdPersonFollower 399 | Near Clip Distance: 0.009999999776482582 400 | Pitch: 0.6000000238418579 401 | Target Frame: base_link 402 | Yaw: 3.1500000953674316 403 | - Angle: -1.5700000524520874 404 | Class: rviz/TopDownOrtho 405 | Enable Stereo Rendering: 406 | Stereo Eye Separation: 0.05999999865889549 407 | Stereo Focal Distance: 1 408 | Swap Stereo Eyes: false 409 | Value: false 410 | Invert Z Axis: false 411 | Name: TopDownOrtho 412 | Near Clip Distance: 0.009999999776482582 413 | Scale: 100 414 | Target Frame: base_link 415 | X: 0 416 | Y: 0 417 | Window Geometry: 418 | Displays: 419 | collapsed: false 420 | Height: 1016 421 | Hide Left Dock: false 422 | Hide Right Dock: false 423 | Image: 424 | collapsed: false 425 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000022f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002720000012a0000001600ffffff00000001000001990000035ffc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035f000000a400fffffffb0000000c00540065006c0065006f00700000000287000001150000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000039fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000043d0000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 426 | Selection: 427 | collapsed: false 428 | Time: 429 | collapsed: false 430 | Tool Properties: 431 | collapsed: false 432 | Views: 433 | collapsed: false 434 | Width: 1848 435 | X: 72 436 | Y: 27 437 | -------------------------------------------------------------------------------- /me5413_world/rviz/manual.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5117647051811218 10 | Tree Height: 420 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.6096654534339905 25 | - Class: rviz/Time 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: PointCloud2 29 | - Class: rviz_panel::simplePanel 30 | Name: simplePanel 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/TF 57 | Enabled: false 58 | Frame Timeout: 15 59 | Frames: 60 | All Enabled: true 61 | Marker Alpha: 1 62 | Marker Scale: 1 63 | Name: TF 64 | Show Arrows: true 65 | Show Axes: true 66 | Show Names: true 67 | Tree: 68 | {} 69 | Update Interval: 0 70 | Value: false 71 | - Alpha: 1 72 | Class: rviz/RobotModel 73 | Collision Enabled: false 74 | Enabled: true 75 | Links: 76 | All Links Enabled: true 77 | Expand Joint Details: false 78 | Expand Link Details: false 79 | Expand Tree: false 80 | Link Tree Style: Links in Alphabetic Order 81 | base_link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | chassis_link: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | front_camera: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | front_camera_beam: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | front_camera_mount: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | front_camera_optical: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | front_fender_link: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | Value: true 114 | front_left_wheel_link: 115 | Alpha: 1 116 | Show Axes: false 117 | Show Trail: false 118 | Value: true 119 | front_mount: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | front_right_wheel_link: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | imu_link: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | mid_mount: 133 | Alpha: 1 134 | Show Axes: false 135 | Show Trail: false 136 | mid_vlp16_leg1: 137 | Alpha: 1 138 | Show Axes: false 139 | Show Trail: false 140 | Value: true 141 | mid_vlp16_leg2: 142 | Alpha: 1 143 | Show Axes: false 144 | Show Trail: false 145 | Value: true 146 | mid_vlp16_leg3: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | Value: true 151 | mid_vlp16_leg4: 152 | Alpha: 1 153 | Show Axes: false 154 | Show Trail: false 155 | Value: true 156 | mid_vlp16_mount: 157 | Alpha: 1 158 | Show Axes: false 159 | Show Trail: false 160 | mid_vlp16_plate: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | Value: true 165 | navsat_link: 166 | Alpha: 1 167 | Show Axes: false 168 | Show Trail: false 169 | Value: true 170 | rear_fender_link: 171 | Alpha: 1 172 | Show Axes: false 173 | Show Trail: false 174 | Value: true 175 | rear_left_wheel_link: 176 | Alpha: 1 177 | Show Axes: false 178 | Show Trail: false 179 | Value: true 180 | rear_mount: 181 | Alpha: 1 182 | Show Axes: false 183 | Show Trail: false 184 | rear_right_wheel_link: 185 | Alpha: 1 186 | Show Axes: false 187 | Show Trail: false 188 | Value: true 189 | tim551: 190 | Alpha: 1 191 | Show Axes: false 192 | Show Trail: false 193 | Value: true 194 | tim551_mount_link: 195 | Alpha: 1 196 | Show Axes: false 197 | Show Trail: false 198 | velodyne: 199 | Alpha: 1 200 | Show Axes: false 201 | Show Trail: false 202 | Value: true 203 | velodyne_base_link: 204 | Alpha: 1 205 | Show Axes: false 206 | Show Trail: false 207 | Value: true 208 | Name: RobotModel 209 | Robot Description: robot_description 210 | TF Prefix: "" 211 | Update Interval: 0 212 | Value: true 213 | Visual Enabled: true 214 | - Class: rviz/InteractiveMarkers 215 | Enable Transparency: true 216 | Enabled: true 217 | Name: InteractiveMarkers 218 | Show Axes: false 219 | Show Descriptions: true 220 | Show Visual Aids: false 221 | Update Topic: /twist_marker_server/update 222 | Value: true 223 | - Class: rviz/Group 224 | Displays: 225 | - Acceleration properties: 226 | Acc. vector alpha: 1 227 | Acc. vector color: 255; 0; 0 228 | Acc. vector scale: 1 229 | Derotate acceleration: true 230 | Enable acceleration: false 231 | Axes properties: 232 | Axes scale: 1 233 | Enable axes: true 234 | Box properties: 235 | Box alpha: 1 236 | Box color: 255; 0; 0 237 | Enable box: false 238 | x_scale: 1 239 | y_scale: 1 240 | z_scale: 1 241 | Class: rviz_imu_plugin/Imu 242 | Enabled: true 243 | Name: Imu 244 | Queue Size: 10 245 | Topic: /imu/data 246 | Unreliable: false 247 | Value: true 248 | fixed_frame_orientation: true 249 | - Class: rviz/Image 250 | Enabled: true 251 | Image Topic: /front/image_raw 252 | Max Value: 1 253 | Median window: 5 254 | Min Value: 0 255 | Name: Image 256 | Normalize Range: true 257 | Queue Size: 2 258 | Transport Hint: raw 259 | Unreliable: false 260 | Value: true 261 | - Alpha: 1 262 | Autocompute Intensity Bounds: true 263 | Autocompute Value Bounds: 264 | Max Value: 6.684496879577637 265 | Min Value: -0.751370906829834 266 | Value: true 267 | Axis: X 268 | Channel Name: intensity 269 | Class: rviz/LaserScan 270 | Color: 255; 255; 255 271 | Color Transformer: AxisColor 272 | Decay Time: 0 273 | Enabled: true 274 | Invert Rainbow: false 275 | Max Color: 255; 255; 255 276 | Min Color: 0; 0; 0 277 | Name: LaserScan 278 | Position Transformer: XYZ 279 | Queue Size: 10 280 | Selectable: true 281 | Size (Pixels): 3 282 | Size (m): 0.05000000074505806 283 | Style: Spheres 284 | Topic: /front/scan 285 | Unreliable: false 286 | Use Fixed Frame: true 287 | Use rainbow: true 288 | Value: true 289 | - Alpha: 1 290 | Autocompute Intensity Bounds: true 291 | Autocompute Value Bounds: 292 | Max Value: 2.438411235809326 293 | Min Value: -0.0709778368473053 294 | Value: true 295 | Axis: Z 296 | Channel Name: intensity 297 | Class: rviz/PointCloud2 298 | Color: 255; 255; 255 299 | Color Transformer: AxisColor 300 | Decay Time: 0 301 | Enabled: true 302 | Invert Rainbow: false 303 | Max Color: 255; 255; 255 304 | Min Color: 0; 0; 0 305 | Name: PointCloud2 306 | Position Transformer: XYZ 307 | Queue Size: 10 308 | Selectable: true 309 | Size (Pixels): 3 310 | Size (m): 0.019999999552965164 311 | Style: Points 312 | Topic: /mid/points 313 | Unreliable: false 314 | Use Fixed Frame: true 315 | Use rainbow: true 316 | Value: true 317 | Enabled: true 318 | Name: Sensing 319 | Enabled: true 320 | Global Options: 321 | Background Color: 48; 48; 48 322 | Default Light: true 323 | Fixed Frame: base_link 324 | Frame Rate: 30 325 | Name: root 326 | Tools: 327 | - Class: rviz/Interact 328 | Hide Inactive Objects: true 329 | - Class: rviz/MoveCamera 330 | - Class: rviz/Select 331 | - Class: rviz/FocusCamera 332 | - Class: rviz/Measure 333 | - Class: rviz/SetInitialPose 334 | Theta std deviation: 0.2617993950843811 335 | Topic: /initialpose 336 | X std deviation: 0.5 337 | Y std deviation: 0.5 338 | - Class: rviz/SetGoal 339 | Topic: /move_base_simple/goal 340 | - Class: rviz/PublishPoint 341 | Single click: true 342 | Topic: /clicked_point 343 | Value: true 344 | Views: 345 | Current: 346 | Class: rviz/ThirdPersonFollower 347 | Distance: 4 348 | Enable Stereo Rendering: 349 | Stereo Eye Separation: 0.05999999865889549 350 | Stereo Focal Distance: 1 351 | Swap Stereo Eyes: false 352 | Value: false 353 | Field of View: 0.8999999761581421 354 | Focal Point: 355 | X: 0 356 | Y: 0 357 | Z: 0 358 | Focal Shape Fixed Size: true 359 | Focal Shape Size: 0.05000000074505806 360 | Invert Z Axis: false 361 | Name: Current View 362 | Near Clip Distance: 0.009999999776482582 363 | Pitch: 0.6000000238418579 364 | Target Frame: base_link 365 | Yaw: 3.1500000953674316 366 | Saved: 367 | - Class: rviz/ThirdPersonFollower 368 | Distance: 4 369 | Enable Stereo Rendering: 370 | Stereo Eye Separation: 0.05999999865889549 371 | Stereo Focal Distance: 1 372 | Swap Stereo Eyes: false 373 | Value: false 374 | Field of View: 0.8999999761581421 375 | Focal Point: 376 | X: 0 377 | Y: 0 378 | Z: 0 379 | Focal Shape Fixed Size: true 380 | Focal Shape Size: 0.05000000074505806 381 | Invert Z Axis: false 382 | Name: ThirdPersonFollower 383 | Near Clip Distance: 0.009999999776482582 384 | Pitch: 0.6000000238418579 385 | Target Frame: base_link 386 | Yaw: 3.1500000953674316 387 | - Angle: -1.5700000524520874 388 | Class: rviz/TopDownOrtho 389 | Enable Stereo Rendering: 390 | Stereo Eye Separation: 0.05999999865889549 391 | Stereo Focal Distance: 1 392 | Swap Stereo Eyes: false 393 | Value: false 394 | Invert Z Axis: false 395 | Name: TopDownOrtho 396 | Near Clip Distance: 0.009999999776482582 397 | Scale: 100 398 | Target Frame: base_link 399 | X: 0 400 | Y: 0 401 | Window Geometry: 402 | Displays: 403 | collapsed: false 404 | Height: 1016 405 | Hide Left Dock: false 406 | Hide Right Dock: false 407 | Image: 408 | collapsed: false 409 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000022f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002720000012a0000001600ffffff00000001000001990000035ffc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035f000000a400fffffffb0000001600730069006d0070006c006500500061006e0065006c00000002700000012c0000001600fffffffb0000000c00540065006c0065006f00700000000287000001150000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000039fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000043d0000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 410 | Selection: 411 | collapsed: false 412 | Time: 413 | collapsed: false 414 | Tool Properties: 415 | collapsed: false 416 | Views: 417 | collapsed: false 418 | Width: 1848 419 | X: 72 420 | Y: 27 421 | simplePanel: 422 | collapsed: false 423 | -------------------------------------------------------------------------------- /me5413_world/rviz/navigation.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Planning1 10 | - /Evaluation1 11 | - /Evaluation1/Absolute Error1 12 | - /Evaluation1/RMSE1 13 | Splitter Ratio: 0.6628242135047913 14 | Tree Height: 473 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.6096654534339905 29 | - Class: rviz/Time 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: Image 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 1 59 | Class: rviz/RobotModel 60 | Collision Enabled: false 61 | Enabled: true 62 | Links: 63 | All Links Enabled: true 64 | Expand Joint Details: false 65 | Expand Link Details: false 66 | Expand Tree: false 67 | Link Tree Style: Links in Alphabetic Order 68 | base_link: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | chassis_link: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | front_camera: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | front_camera_beam: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | front_camera_mount: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | front_camera_optical: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | front_fender_link: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | front_left_wheel_link: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Value: true 106 | front_mount: 107 | Alpha: 1 108 | Show Axes: false 109 | Show Trail: false 110 | front_right_wheel_link: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | imu_link: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | mid_mount: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | mid_vlp16_leg1: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | mid_vlp16_leg2: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | Value: true 133 | mid_vlp16_leg3: 134 | Alpha: 1 135 | Show Axes: false 136 | Show Trail: false 137 | Value: true 138 | mid_vlp16_leg4: 139 | Alpha: 1 140 | Show Axes: false 141 | Show Trail: false 142 | Value: true 143 | mid_vlp16_mount: 144 | Alpha: 1 145 | Show Axes: false 146 | Show Trail: false 147 | mid_vlp16_plate: 148 | Alpha: 1 149 | Show Axes: false 150 | Show Trail: false 151 | Value: true 152 | navsat_link: 153 | Alpha: 1 154 | Show Axes: false 155 | Show Trail: false 156 | Value: true 157 | rear_fender_link: 158 | Alpha: 1 159 | Show Axes: false 160 | Show Trail: false 161 | Value: true 162 | rear_left_wheel_link: 163 | Alpha: 1 164 | Show Axes: false 165 | Show Trail: false 166 | Value: true 167 | rear_mount: 168 | Alpha: 1 169 | Show Axes: false 170 | Show Trail: false 171 | rear_right_wheel_link: 172 | Alpha: 1 173 | Show Axes: false 174 | Show Trail: false 175 | Value: true 176 | tim551: 177 | Alpha: 1 178 | Show Axes: false 179 | Show Trail: false 180 | Value: true 181 | tim551_mount_link: 182 | Alpha: 1 183 | Show Axes: false 184 | Show Trail: false 185 | velodyne: 186 | Alpha: 1 187 | Show Axes: false 188 | Show Trail: false 189 | Value: true 190 | velodyne_base_link: 191 | Alpha: 1 192 | Show Axes: false 193 | Show Trail: false 194 | Value: true 195 | Name: RobotModel 196 | Robot Description: robot_description 197 | TF Prefix: "" 198 | Update Interval: 0 199 | Value: true 200 | Visual Enabled: true 201 | - Class: rviz/InteractiveMarkers 202 | Enable Transparency: true 203 | Enabled: true 204 | Name: InteractiveMarkers 205 | Show Axes: false 206 | Show Descriptions: false 207 | Show Visual Aids: false 208 | Update Topic: /twist_marker_server/update 209 | Value: true 210 | - Class: rviz/Group 211 | Displays: 212 | - Alpha: 1 213 | Autocompute Intensity Bounds: true 214 | Autocompute Value Bounds: 215 | Max Value: -9999 216 | Min Value: 9999 217 | Value: true 218 | Axis: X 219 | Channel Name: intensity 220 | Class: rviz/LaserScan 221 | Color: 255; 255; 255 222 | Color Transformer: AxisColor 223 | Decay Time: 0 224 | Enabled: false 225 | Invert Rainbow: false 226 | Max Color: 255; 255; 255 227 | Min Color: 0; 0; 0 228 | Name: LaserScan 229 | Position Transformer: XYZ 230 | Queue Size: 10 231 | Selectable: true 232 | Size (Pixels): 3 233 | Size (m): 0.05000000074505806 234 | Style: Spheres 235 | Topic: /front/scan 236 | Unreliable: false 237 | Use Fixed Frame: true 238 | Use rainbow: true 239 | Value: false 240 | - Alpha: 1 241 | Autocompute Intensity Bounds: true 242 | Autocompute Value Bounds: 243 | Max Value: -0.00716748833656311 244 | Min Value: -0.06999534368515015 245 | Value: true 246 | Axis: Z 247 | Channel Name: intensity 248 | Class: rviz/PointCloud2 249 | Color: 255; 255; 255 250 | Color Transformer: AxisColor 251 | Decay Time: 0 252 | Enabled: false 253 | Invert Rainbow: false 254 | Max Color: 255; 255; 255 255 | Min Color: 0; 0; 0 256 | Name: PointCloud2 257 | Position Transformer: XYZ 258 | Queue Size: 10 259 | Selectable: true 260 | Size (Pixels): 3 261 | Size (m): 0.019999999552965164 262 | Style: Points 263 | Topic: /mid/points 264 | Unreliable: false 265 | Use Fixed Frame: true 266 | Use rainbow: true 267 | Value: false 268 | - Class: rviz/Camera 269 | Enabled: false 270 | Image Rendering: background and overlay 271 | Image Topic: /front/image_raw 272 | Name: Camera 273 | Overlay Alpha: 0.5 274 | Queue Size: 1 275 | Transport Hint: compressed 276 | Unreliable: false 277 | Value: false 278 | Visibility: 279 | Evaluation: 280 | Absolute Error: 281 | ADE (m): true 282 | AHE (deg): true 283 | ASE (m/s): true 284 | Value: true 285 | RMSE: 286 | RMSE (heading): true 287 | RMSE (position): true 288 | RMSE (speed): true 289 | Value: true 290 | Value: true 291 | Grid: true 292 | InteractiveMarkers: true 293 | Planning: 294 | Full Path: true 295 | Local Path: true 296 | TFTrajectory: true 297 | Value: true 298 | RobotModel: true 299 | Sensing: 300 | Image: true 301 | LaserScan: true 302 | PointCloud2: true 303 | Value: true 304 | Value: true 305 | Zoom Factor: 1 306 | - Class: rviz/Image 307 | Enabled: true 308 | Image Topic: /front/image_raw 309 | Max Value: 1 310 | Median window: 5 311 | Min Value: 0 312 | Name: Image 313 | Normalize Range: true 314 | Queue Size: 2 315 | Transport Hint: compressed 316 | Unreliable: false 317 | Value: true 318 | Enabled: true 319 | Name: Sensing 320 | - Class: rviz/Group 321 | Displays: 322 | - Alpha: 1 323 | Buffer Length: 1 324 | Class: rviz/Path 325 | Color: 25; 255; 0 326 | Enabled: true 327 | Head Diameter: 0.30000001192092896 328 | Head Length: 0.20000000298023224 329 | Length: 0.30000001192092896 330 | Line Style: Billboards 331 | Line Width: 0.029999999329447746 332 | Name: Full Path 333 | Offset: 334 | X: 0 335 | Y: 0 336 | Z: 0 337 | Pose Color: 255; 85; 255 338 | Pose Style: None 339 | Queue Size: 1 340 | Radius: 0.029999999329447746 341 | Shaft Diameter: 0.10000000149011612 342 | Shaft Length: 0.10000000149011612 343 | Topic: /me5413_world/planning/global_path 344 | Unreliable: true 345 | Value: true 346 | - Alpha: 1 347 | Buffer Length: 1 348 | Class: rviz/Path 349 | Color: 114; 159; 207 350 | Enabled: true 351 | Head Diameter: 0.25 352 | Head Length: 0.20000000298023224 353 | Length: 0.30000001192092896 354 | Line Style: Lines 355 | Line Width: 0.029999999329447746 356 | Name: Local Path 357 | Offset: 358 | X: 0 359 | Y: 0 360 | Z: 0 361 | Pose Color: 114; 159; 207 362 | Pose Style: Arrows 363 | Queue Size: 1 364 | Radius: 0.029999999329447746 365 | Shaft Diameter: 0.10000000149011612 366 | Shaft Length: 0.20000000298023224 367 | Topic: /me5413_world/planning/local_path 368 | Unreliable: true 369 | Value: true 370 | - Class: jsk_rviz_plugin/TFTrajectory 371 | Enabled: true 372 | Name: TFTrajectory 373 | Value: true 374 | color: 173; 127; 168 375 | duration: 10 376 | frame: base_link 377 | line_width: 0.05000000074505806 378 | Enabled: true 379 | Name: Planning 380 | - Class: rviz/Group 381 | Displays: 382 | - Class: rviz/Group 383 | Displays: 384 | - Buffer length: 100 385 | Class: jsk_rviz_plugin/Plotter2D 386 | Enabled: true 387 | Name: ADE (m) 388 | Show Value: true 389 | Topic: /me5413_world/planning/abs_position_error 390 | Value: true 391 | auto color change: true 392 | auto scale: true 393 | background color: 0; 0; 0 394 | backround alpha: 0 395 | border: true 396 | foreground alpha: 0.699999988079071 397 | foreground color: 25; 255; 240 398 | height: 128 399 | left: 20 400 | linewidth: 2 401 | max color: 255; 0; 0 402 | max value: 10 403 | min value: 0 404 | show caption: true 405 | text size: 14 406 | top: 20 407 | update interval: 0.03999999910593033 408 | width: 128 409 | - Buffer length: 100 410 | Class: jsk_rviz_plugin/Plotter2D 411 | Enabled: true 412 | Name: AHE (deg) 413 | Show Value: true 414 | Topic: /me5413_world/planning/abs_heading_error 415 | Value: true 416 | auto color change: true 417 | auto scale: true 418 | background color: 0; 0; 0 419 | backround alpha: 0 420 | border: true 421 | foreground alpha: 0.699999988079071 422 | foreground color: 25; 255; 240 423 | height: 128 424 | left: 160 425 | linewidth: 2 426 | max color: 255; 0; 0 427 | max value: 10 428 | min value: 0 429 | show caption: true 430 | text size: 14 431 | top: 20 432 | update interval: 0.03999999910593033 433 | width: 128 434 | - Buffer length: 100 435 | Class: jsk_rviz_plugin/Plotter2D 436 | Enabled: true 437 | Name: ASE (m/s) 438 | Show Value: true 439 | Topic: /me5413_world/planning/abs_speed_error 440 | Value: true 441 | auto color change: true 442 | auto scale: true 443 | background color: 0; 0; 0 444 | backround alpha: 0 445 | border: true 446 | foreground alpha: 0.699999988079071 447 | foreground color: 25; 255; 240 448 | height: 128 449 | left: 300 450 | linewidth: 2 451 | max color: 255; 0; 0 452 | max value: 10 453 | min value: 0 454 | show caption: true 455 | text size: 14 456 | top: 20 457 | update interval: 0.03999999910593033 458 | width: 128 459 | Enabled: true 460 | Name: Absolute Error 461 | - Class: rviz/Group 462 | Displays: 463 | - Buffer length: 100 464 | Class: jsk_rviz_plugin/Plotter2D 465 | Enabled: true 466 | Name: RMSE (xy) 467 | Show Value: true 468 | Topic: /me5413_world/planning/rms_position_error 469 | Value: true 470 | auto color change: true 471 | auto scale: true 472 | background color: 0; 0; 0 473 | backround alpha: 0 474 | border: true 475 | foreground alpha: 0.699999988079071 476 | foreground color: 25; 255; 240 477 | height: 128 478 | left: 20 479 | linewidth: 2 480 | max color: 255; 0; 0 481 | max value: 10 482 | min value: 0 483 | show caption: true 484 | text size: 14 485 | top: 180 486 | update interval: 0.03999999910593033 487 | width: 128 488 | - Buffer length: 100 489 | Class: jsk_rviz_plugin/Plotter2D 490 | Enabled: true 491 | Name: RMSE (yaw) 492 | Show Value: true 493 | Topic: /me5413_world/planning/rms_heading_error 494 | Value: true 495 | auto color change: true 496 | auto scale: true 497 | background color: 0; 0; 0 498 | backround alpha: 0 499 | border: true 500 | foreground alpha: 0.699999988079071 501 | foreground color: 25; 255; 240 502 | height: 128 503 | left: 160 504 | linewidth: 2 505 | max color: 255; 0; 0 506 | max value: 10 507 | min value: 0 508 | show caption: true 509 | text size: 14 510 | top: 180 511 | update interval: 0.03999999910593033 512 | width: 128 513 | - Buffer length: 100 514 | Class: jsk_rviz_plugin/Plotter2D 515 | Enabled: true 516 | Name: RMSE (speed) 517 | Show Value: true 518 | Topic: /me5413_world/planning/rms_speed_error 519 | Value: true 520 | auto color change: true 521 | auto scale: true 522 | background color: 0; 0; 0 523 | backround alpha: 0 524 | border: true 525 | foreground alpha: 0.699999988079071 526 | foreground color: 25; 255; 240 527 | height: 128 528 | left: 300 529 | linewidth: 2 530 | max color: 255; 0; 0 531 | max value: 10 532 | min value: 0 533 | show caption: true 534 | text size: 14 535 | top: 180 536 | update interval: 0.03999999910593033 537 | width: 128 538 | Enabled: true 539 | Name: RMSE 540 | Enabled: true 541 | Name: Evaluation 542 | Enabled: true 543 | Global Options: 544 | Background Color: 48; 48; 48 545 | Default Light: true 546 | Fixed Frame: world 547 | Frame Rate: 30 548 | Name: root 549 | Tools: 550 | - Class: rviz/Interact 551 | Hide Inactive Objects: true 552 | - Class: rviz/MoveCamera 553 | - Class: rviz/Select 554 | - Class: rviz/FocusCamera 555 | - Class: rviz/Measure 556 | - Class: rviz/SetInitialPose 557 | Theta std deviation: 0.2617993950843811 558 | Topic: /initialpose 559 | X std deviation: 0.5 560 | Y std deviation: 0.5 561 | - Class: rviz/SetGoal 562 | Topic: /move_base_simple/goal 563 | - Class: rviz/PublishPoint 564 | Single click: true 565 | Topic: /clicked_point 566 | Value: true 567 | Views: 568 | Current: 569 | Class: rviz/ThirdPersonFollower 570 | Distance: 12.296568870544434 571 | Enable Stereo Rendering: 572 | Stereo Eye Separation: 0.05999999865889549 573 | Stereo Focal Distance: 1 574 | Swap Stereo Eyes: false 575 | Value: false 576 | Field of View: 0.7853981852531433 577 | Focal Point: 578 | X: 0 579 | Y: 0 580 | Z: 0 581 | Focal Shape Fixed Size: false 582 | Focal Shape Size: 0.05000000074505806 583 | Invert Z Axis: false 584 | Name: Current View 585 | Near Clip Distance: 0.009999999776482582 586 | Pitch: 0.6847973465919495 587 | Target Frame: base_link 588 | Yaw: 3.105402708053589 589 | Saved: 590 | - Class: rviz/ThirdPersonFollower 591 | Distance: 4 592 | Enable Stereo Rendering: 593 | Stereo Eye Separation: 0.05999999865889549 594 | Stereo Focal Distance: 1 595 | Swap Stereo Eyes: false 596 | Value: false 597 | Field of View: 0.8999999761581421 598 | Focal Point: 599 | X: 0 600 | Y: 0 601 | Z: 0 602 | Focal Shape Fixed Size: true 603 | Focal Shape Size: 0.05000000074505806 604 | Invert Z Axis: false 605 | Name: ThirdPersonFollower 606 | Near Clip Distance: 0.009999999776482582 607 | Pitch: 0.6000000238418579 608 | Target Frame: base_link 609 | Yaw: 3.1500000953674316 610 | - Angle: -1.5700000524520874 611 | Class: rviz/TopDownOrtho 612 | Enable Stereo Rendering: 613 | Stereo Eye Separation: 0.05999999865889549 614 | Stereo Focal Distance: 1 615 | Swap Stereo Eyes: false 616 | Value: false 617 | Invert Z Axis: false 618 | Name: TopDownOrtho 619 | Near Clip Distance: 0.009999999776482582 620 | Scale: 100 621 | Target Frame: base_link 622 | X: 0 623 | Y: 0 624 | Window Geometry: 625 | Camera: 626 | collapsed: false 627 | Displays: 628 | collapsed: false 629 | Height: 1043 630 | Hide Left Dock: false 631 | Hide Right Dock: true 632 | Image: 633 | collapsed: false 634 | QMainWindow State: 000000ff00000000fd00000004000000000000019d0000037afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000216000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002590000015e0000001600fffffffb0000000c00430061006d0065007200610000000316000000a10000001600ffffff000000010000014f0000037afc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000037a000000a400fffffffb0000001600730069006d0070006c006500500061006e0065006c00000002d2000000e50000000000000000fb0000000c00540065006c0065006f00700000000287000001150000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000039fc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005dd0000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 635 | Selection: 636 | collapsed: false 637 | Time: 638 | collapsed: false 639 | Tool Properties: 640 | collapsed: false 641 | Views: 642 | collapsed: true 643 | Width: 1920 644 | X: 0 645 | Y: 0 646 | -------------------------------------------------------------------------------- /me5413_world/src/path_publisher_node.cpp: -------------------------------------------------------------------------------- 1 | #include "me5413_world/path_publisher_node.hpp" 2 | 3 | namespace me5413_world 4 | { 5 | 6 | // Dynamic Parameters 7 | double SPEED_TARGET; 8 | double TRACK_A_AXIS; 9 | double TRACK_B_AXIS; 10 | double TRACK_WP_NUM; 11 | double LOCAL_PREV_WP_NUM; 12 | double LOCAL_NEXT_WP_NUM; 13 | bool PARAMS_UPDATED = false; 14 | 15 | void dynamicParamCallback(me5413_world::path_publisherConfig& config, uint32_t level) 16 | { 17 | // Common Params 18 | SPEED_TARGET = config.speed_target; 19 | // Global Path Settings 20 | TRACK_A_AXIS = config.track_A_axis; 21 | TRACK_B_AXIS = config.track_B_axis; 22 | TRACK_WP_NUM = config.track_wp_num; 23 | LOCAL_PREV_WP_NUM = config.local_prev_wp_num; 24 | LOCAL_NEXT_WP_NUM = config.local_next_wp_num; 25 | PARAMS_UPDATED = true; 26 | }; 27 | 28 | PathPublisherNode::PathPublisherNode() : tf2_listener_(tf2_buffer_) 29 | { 30 | f = boost::bind(&dynamicParamCallback, _1, _2); 31 | server.setCallback(f); 32 | 33 | this->timer_ = nh_.createTimer(ros::Duration(0.1), &PathPublisherNode::timerCallback, this); 34 | this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &PathPublisherNode::robotOdomCallback, this); 35 | this->pub_global_path_ = nh_.advertise("/me5413_world/planning/global_path", 1); 36 | this->pub_local_path_ = nh_.advertise("/me5413_world/planning/local_path", 1); 37 | this->pub_abs_position_error_ = nh_.advertise("/me5413_world/planning/abs_position_error", 1); 38 | this->pub_abs_heading_error_ = nh_.advertise("/me5413_world/planning/abs_heading_error", 1); 39 | this->pub_abs_speed_error_ = nh_.advertise("/me5413_world/planning/abs_speed_error", 1); 40 | this->pub_rms_position_error_ = nh_.advertise("/me5413_world/planning/rms_position_error", 1); 41 | this->pub_rms_heading_error_ = nh_.advertise("/me5413_world/planning/rms_heading_error", 1); 42 | this->pub_rms_speed_error_ = nh_.advertise("/me5413_world/planning/rms_speed_error", 1); 43 | 44 | // Initialization 45 | this->robot_frame_ = "base_link"; 46 | this->world_frame_ = "world"; 47 | 48 | this->global_path_msg_.header.frame_id = this->world_frame_; 49 | this->local_path_msg_.header.frame_id = this->world_frame_; 50 | this->global_path_msg_.poses = createGlobalPath(TRACK_A_AXIS, TRACK_B_AXIS, 1.0/TRACK_WP_NUM); 51 | 52 | this->abs_position_error_.data = 0.0; 53 | this->abs_heading_error_.data = 0.0; 54 | this->rms_position_error_.data = 0.0; 55 | this->rms_heading_error_.data = 0.0; 56 | 57 | this->current_id_ = 0; 58 | this->num_time_steps_ = 1; 59 | this->sum_sqr_position_error_ = 0.0; 60 | this->sum_sqr_heading_error_ = 0.0; 61 | }; 62 | 63 | void PathPublisherNode::timerCallback(const ros::TimerEvent &) 64 | { 65 | // Create and Publish Paths 66 | if (PARAMS_UPDATED) 67 | { 68 | this->global_path_msg_.poses = createGlobalPath(TRACK_A_AXIS, TRACK_B_AXIS, 1.0/TRACK_WP_NUM); 69 | this->current_id_ = 0; 70 | PARAMS_UPDATED = false; 71 | } 72 | publishGlobalPath(); 73 | publishLocalPath(this->odom_world_robot_.pose.pose, LOCAL_PREV_WP_NUM, LOCAL_NEXT_WP_NUM); 74 | 75 | // Calculate absolute errors (wrt to world frame) 76 | const std::pair abs_errors = calculatePoseError(this->odom_world_robot_.pose.pose, this->pose_world_goal_); 77 | this->abs_position_error_.data = abs_errors.first; 78 | this->abs_heading_error_.data = abs_errors.second; 79 | tf2::Vector3 velocity; 80 | tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, velocity); 81 | this->abs_speed_error_.data = velocity.length() - SPEED_TARGET; 82 | 83 | // Calculate average errors 84 | this->sum_sqr_position_error_ += std::pow(abs_errors.first, 2); 85 | this->sum_sqr_heading_error_ += std::pow(abs_errors.second, 2); 86 | this->sum_sqr_speed_error_ += std::pow(this->abs_speed_error_.data, 2); 87 | this->rms_position_error_.data = std::sqrt(sum_sqr_position_error_/num_time_steps_); 88 | this->rms_heading_error_.data = std::sqrt(sum_sqr_heading_error_/num_time_steps_); 89 | this->rms_speed_error_.data = std::sqrt(sum_sqr_speed_error_/num_time_steps_); 90 | 91 | // Publish errors 92 | this->pub_abs_position_error_.publish(this->abs_position_error_); 93 | this->pub_abs_heading_error_.publish(this->abs_heading_error_); 94 | this->pub_abs_speed_error_.publish(this->abs_speed_error_); 95 | this->pub_rms_position_error_.publish(this->rms_position_error_); 96 | this->pub_rms_heading_error_.publish(this->rms_heading_error_); 97 | this->pub_rms_speed_error_.publish(this->rms_speed_error_); 98 | 99 | // Count 100 | this->num_time_steps_++; 101 | 102 | return; 103 | }; 104 | 105 | void PathPublisherNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr &odom) 106 | { 107 | this->world_frame_ = odom->header.frame_id; 108 | this->robot_frame_ = odom->child_frame_id; 109 | this->odom_world_robot_ = *odom.get(); 110 | 111 | const tf2::Transform T_world_robot = convertPoseToTransform(this->odom_world_robot_.pose.pose); 112 | const tf2::Transform T_robot_world = T_world_robot.inverse(); 113 | 114 | geometry_msgs::TransformStamped transformStamped; 115 | transformStamped.header.stamp = ros::Time::now(); 116 | transformStamped.header.frame_id = this->robot_frame_; 117 | transformStamped.child_frame_id = this->world_frame_; 118 | transformStamped.transform.translation = tf2::toMsg(T_robot_world.getOrigin()); 119 | // transformStamped.transform.translation.z = 0.0; 120 | transformStamped.transform.rotation = tf2::toMsg(T_robot_world.getRotation()); 121 | this->tf2_bcaster_.sendTransform(transformStamped); 122 | 123 | return; 124 | }; 125 | 126 | std::vector PathPublisherNode::createGlobalPath(const double A, const double B, const double t_res) 127 | { 128 | std::vector poses; 129 | const double t_increament = t_res * 2 * M_PI; 130 | 131 | // Calculate the positions 132 | for (double t = 0.0; t <= 2 * M_PI; t += t_increament) 133 | { 134 | geometry_msgs::PoseStamped pose; 135 | pose.pose.position.x = A * std::sin(t); 136 | pose.pose.position.y = B * std::sin(t) * std::cos(t); 137 | poses.push_back(pose); 138 | } 139 | 140 | // Calcuate the orientations 141 | tf2::Quaternion q; 142 | for (int i = 0; i < poses.size(); i++) 143 | { 144 | const double x_d = poses[i + 1].pose.position.x - poses[i].pose.position.x; 145 | const double y_d = poses[i + 1].pose.position.y - poses[i].pose.position.y; 146 | const double yaw = std::atan2(y_d, x_d); 147 | 148 | q.setRPY(0.0, 0.0, yaw); 149 | q.normalize(); 150 | poses[i].pose.orientation = tf2::toMsg(q); 151 | } 152 | poses.back().pose.orientation = tf2::toMsg(q); 153 | 154 | return poses; 155 | }; 156 | 157 | void PathPublisherNode::publishGlobalPath() 158 | { 159 | // Update the message 160 | this->global_path_msg_.header.stamp = ros::Time::now(); 161 | this->pub_global_path_.publish(this->global_path_msg_); 162 | }; 163 | 164 | void PathPublisherNode::publishLocalPath(const geometry_msgs::Pose &robot_pose, const int n_wp_prev, const int n_wp_post) 165 | { 166 | int id_next = nextWaypoint(robot_pose, this->global_path_msg_, this->current_id_); 167 | if (this->global_path_msg_.poses.empty()) 168 | { 169 | ROS_WARN("Global Path not published yet, waiting"); 170 | } 171 | else if (id_next >= this->global_path_msg_.poses.size() - 1) 172 | { 173 | ROS_WARN("Robot has reached the end of the track, please restart"); 174 | } 175 | else 176 | { 177 | this->current_id_ = std::max(this->current_id_, id_next - 1); 178 | int id_start = std::max(id_next - n_wp_prev, 0); 179 | int id_end = std::min(id_next + n_wp_post, int(this->global_path_msg_.poses.size() - 1)); 180 | 181 | std::vector::const_iterator start = this->global_path_msg_.poses.begin() + id_start; 182 | std::vector::const_iterator end = this->global_path_msg_.poses.begin() + id_end; 183 | 184 | // Update the message 185 | this->local_path_msg_.header.stamp = ros::Time::now(); 186 | this->local_path_msg_.poses = std::vector(start, end); 187 | this->pub_local_path_.publish(this->local_path_msg_); 188 | this->pose_world_goal_ = this->local_path_msg_.poses[n_wp_prev].pose; 189 | } 190 | }; 191 | 192 | int PathPublisherNode::closestWaypoint(const geometry_msgs::Pose &robot_pose, const nav_msgs::Path &path, const int id_start = 0) 193 | { 194 | const double yaw_robot = getYawFromOrientation(robot_pose.orientation); 195 | 196 | double min_dist = DBL_MAX; 197 | int id_closest = id_start; 198 | for (int i = id_start; i < path.poses.size(); i++) 199 | { 200 | const double dist = std::hypot(robot_pose.position.x - path.poses[i].pose.position.x, robot_pose.position.y - path.poses[i].pose.position.y); 201 | 202 | if (dist <= min_dist) 203 | { 204 | min_dist = dist; 205 | id_closest = i; 206 | } 207 | else 208 | { 209 | break; 210 | } 211 | } 212 | 213 | return id_closest; 214 | }; 215 | 216 | int PathPublisherNode::nextWaypoint(const geometry_msgs::Pose &robot_pose, const nav_msgs::Path &path, const int id_start = 0) 217 | { 218 | int id_closest = closestWaypoint(robot_pose, path, id_start); 219 | double yaw_T_robot_wp = atan2((path.poses[id_closest].pose.position.y - robot_pose.position.y), 220 | (path.poses[id_closest].pose.position.x - robot_pose.position.x)); 221 | 222 | const double yaw_robot = getYawFromOrientation(robot_pose.orientation); 223 | const double angle = std::fabs(yaw_robot - yaw_T_robot_wp); 224 | const double angle_norm = std::min(2 * M_PI - angle, angle); // TODO: check if this is correct 225 | 226 | if (angle_norm > M_PI / 2) 227 | { 228 | id_closest++; 229 | } 230 | 231 | return id_closest; 232 | }; 233 | 234 | double PathPublisherNode::getYawFromOrientation(const geometry_msgs::Quaternion &orientation) 235 | { 236 | tf2::Quaternion q; 237 | tf2::fromMsg(orientation, q); 238 | const tf2::Matrix3x3 m = tf2::Matrix3x3(q); 239 | double roll, pitch, yaw; 240 | m.getRPY(roll, pitch, yaw); 241 | 242 | return yaw; 243 | }; 244 | 245 | tf2::Transform PathPublisherNode::convertPoseToTransform(const geometry_msgs::Pose &pose) 246 | { 247 | tf2::Transform T; 248 | T.setOrigin(tf2::Vector3(pose.position.x, pose.position.y, 0)); 249 | tf2::Quaternion q; 250 | q.setValue(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); 251 | T.setRotation(q); 252 | 253 | return T; 254 | }; 255 | 256 | std::pair PathPublisherNode::calculatePoseError(const geometry_msgs::Pose &pose_robot, const geometry_msgs::Pose &pose_goal) 257 | { 258 | // Positional Error 259 | const double position_error = std::hypot( 260 | pose_robot.position.x - pose_goal.position.x, 261 | pose_robot.position.y - pose_goal.position.y 262 | ); 263 | 264 | // Heading Error 265 | tf2::Quaternion q_robot, q_wp; 266 | tf2::fromMsg(pose_robot.orientation, q_robot); 267 | tf2::fromMsg(pose_goal.orientation, q_wp); 268 | const tf2::Matrix3x3 m_robot = tf2::Matrix3x3(q_robot); 269 | const tf2::Matrix3x3 m_goal = tf2::Matrix3x3(q_wp); 270 | 271 | double roll, pitch, yaw_robot, yaw_wp; 272 | m_robot.getRPY(roll, pitch, yaw_robot); 273 | m_goal.getRPY(roll, pitch, yaw_wp); 274 | 275 | const double heading_error = (yaw_robot - yaw_wp) / M_PI * 180.0; 276 | 277 | return std::pair( 278 | position_error, 279 | isnan(heading_error)? 0.0 : heading_error 280 | ); 281 | }; 282 | 283 | } // namespace me5413_world 284 | 285 | int main(int argc, char **argv) 286 | { 287 | ros::init(argc, argv, "path_publisher_node"); 288 | me5413_world::PathPublisherNode path_publisher_node; 289 | ros::spin(); // spin the ros node. 290 | return 0; 291 | }; -------------------------------------------------------------------------------- /me5413_world/src/path_tracker_MPC_node.cpp: -------------------------------------------------------------------------------- 1 | #include "me5413_world/path_tracker_MPC_node.hpp" 2 | #include "me5413_world/math_utils.hpp" 3 | 4 | namespace me5413_world 5 | { 6 | 7 | // Dynamic Parameters 8 | double SPEED_TARGET; 9 | double PID_Kp, PID_Ki, PID_Kd; 10 | double ROBOT_LENGTH; 11 | bool DEFAULT_LOOKAHEAD_DISTANCE; 12 | bool PARAMS_UPDATED; 13 | 14 | void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level) 15 | { 16 | SPEED_TARGET = config.speed_target; 17 | PID_Kp = config.PID_Kp; 18 | PID_Ki = config.PID_Ki; 19 | PID_Kd = config.PID_Kd; 20 | ROBOT_LENGTH = config.robot_length; 21 | DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; 22 | 23 | PARAMS_UPDATED = true; 24 | }; 25 | 26 | PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_) 27 | { 28 | f = boost::bind(&dynamicParamCallback, _1, _2); 29 | server.setCallback(f); 30 | 31 | this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &PathTrackerNode::robotOdomCallback, this); 32 | this->sub_local_path_ = nh_.subscribe("/me5413_world/planning/local_path", 1, &PathTrackerNode::localPathCallback, this); 33 | this->pub_cmd_vel_ = nh_.advertise("/jackal_velocity_controller/cmd_vel", 1); 34 | 35 | // Initialization 36 | this->robot_frame_ = "base_link"; 37 | this->world_frame_ = "world"; 38 | 39 | this->pid_ = control::PID(0.1, 1.0, -1.0, PID_Kp, PID_Ki, PID_Kd); 40 | }; 41 | 42 | void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) 43 | { 44 | // Calculate absolute errors (wrt to world frame) 45 | this->pose_world_goal_ = path->poses[11].pose; 46 | this->pub_cmd_vel_.publish(computeControlOutputs(this->odom_world_robot_, this->pose_world_goal_)); 47 | 48 | return; 49 | }; 50 | 51 | void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom) 52 | { 53 | this->world_frame_ = odom->header.frame_id; 54 | this->robot_frame_ = odom->child_frame_id; 55 | this->odom_world_robot_ = *odom.get(); 56 | 57 | return; 58 | }; 59 | 60 | geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) 61 | { 62 | // Velocity 63 | tf2::Vector3 robot_vel; 64 | tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); 65 | const double velocity = robot_vel.length(); 66 | 67 | // Update PID controller parameters if they are updated dynamically 68 | if (PARAMS_UPDATED) 69 | { 70 | this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); 71 | PARAMS_UPDATED = false; 72 | } 73 | 74 | // Compute linear speed using PID controller 75 | double target_speed = SPEED_TARGET; 76 | double linear_speed = this->pid_.calculate(target_speed, velocity); 77 | 78 | // Run MPC to get the desireable steering angle 79 | ACADO::VariablesGrid control_signals = runMPC(odom_robot, pose_goal); 80 | 81 | geometry_msgs::Twist cmd_vel; 82 | cmd_vel.linear.x = linear_speed; 83 | cmd_vel.angular.z = control_signals(0, 1); 84 | 85 | // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; 86 | // std::cout << "lateral error is " << lat_error << " heading_error is " << heading_error << " steering is " << cmd_vel.angular.z << std::endl; 87 | 88 | return cmd_vel; 89 | } 90 | 91 | void PathTrackerNode::setupMPC(ACADO::VariablesGrid& x_init, ACADO::VariablesGrid& u_init, const geometry_msgs::Pose& pose_goal){ 92 | using namespace ACADO; 93 | using namespace Eigen; 94 | 95 | DifferentialState x, y, theta, v; 96 | Control a, delta; 97 | 98 | DifferentialEquation f; 99 | 100 | const double L = ROBOT_LENGTH; 101 | 102 | f << dot(x) == v * cos(theta); 103 | f << dot(y) == v * sin(theta); 104 | f << dot(theta) == v * tan(delta) / L; 105 | f << dot(v) == a; 106 | 107 | OCP ocp(0.0, 2.0, 20); // Prediction horizon 108 | ocp.subjectTo(f); 109 | 110 | ocp.subjectTo(-1.0 <= delta <= 1.0); 111 | ocp.subjectTo(-1.0 <= a <= 1.0); 112 | 113 | double goal_x = pose_goal.position.x; 114 | double goal_y = pose_goal.position.y; 115 | double goal_theta = tf2::getYaw(pose_goal.orientation); 116 | 117 | Function h; 118 | h << x << y << theta << v; 119 | 120 | DVector ref(4); 121 | ref(0) = goal_x; 122 | ref(1) = goal_y; 123 | ref(2) = goal_theta; 124 | ref(3) = SPEED_TARGET; 125 | 126 | MatrixXd Q(4,4); 127 | Q.setIdentity(); 128 | Q(0,0) = 1.0; 129 | Q(1,1) = 1.0; 130 | Q(2,2) = 1.0; 131 | Q(3,3) = 1.0; 132 | 133 | ocp.minimizeLSQ(Q, h, ref); 134 | 135 | OptimizationAlgorithm algorithm(ocp); 136 | algorithm.initializeDifferentialStates(x_init); 137 | algorithm.initializeControls(u_init); 138 | 139 | algorithm.solve(); 140 | } 141 | 142 | ACADO::VariablesGrid PathTrackerNode::runMPC(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) { 143 | using namespace ACADO; 144 | 145 | VariablesGrid x_init(4, 0.0, 2.0, 20); 146 | VariablesGrid u_init(2, 0.0, 2.0, 20); 147 | 148 | setupMPC(x_init, u_init, pose_goal); 149 | 150 | x_init.print(); 151 | 152 | VariablesGrid x0(4, 0.0, 0.0, 1); 153 | tf2::Vector3 point_robot; 154 | tf2::fromMsg(odom_robot.pose.pose.position, point_robot); 155 | double yaw_robot = tf2::getYaw(odom_robot.pose.pose.orientation); 156 | 157 | x0(0, 0) = point_robot.getX(); 158 | x0(0, 1) = point_robot.getY(); 159 | x0(0, 2) = yaw_robot; 160 | x0(0, 3) = odom_robot.twist.twist.linear.x; 161 | 162 | setupMPC(x0, u_init, pose_goal); 163 | 164 | return u_init; 165 | } 166 | 167 | } // namespace me5413_world 168 | 169 | int main(int argc, char** argv) 170 | { 171 | ros::init(argc, argv, "path_tracker_node"); 172 | me5413_world::PathTrackerNode path_tracker_node; 173 | ros::spin(); // spin the ros node. 174 | return 0; 175 | } -------------------------------------------------------------------------------- /me5413_world/src/path_tracker_node.cpp: -------------------------------------------------------------------------------- 1 | #include "me5413_world/path_tracker_node.hpp" 2 | #include "me5413_world/math_utils.hpp" 3 | 4 | namespace me5413_world 5 | { 6 | 7 | // Dynamic Parameters 8 | double SPEED_TARGET; 9 | double PID_Kp, PID_Ki, PID_Kd; 10 | double ROBOT_LENGTH; 11 | bool DEFAULT_LOOKAHEAD_DISTANCE; 12 | bool PARAMS_UPDATED; 13 | 14 | void dynamicParamCallback(me5413_world::path_trackerConfig& config, uint32_t level) 15 | { 16 | SPEED_TARGET = config.speed_target; 17 | PID_Kp = config.PID_Kp; 18 | PID_Ki = config.PID_Ki; 19 | PID_Kd = config.PID_Kd; 20 | ROBOT_LENGTH = config.robot_length; 21 | DEFAULT_LOOKAHEAD_DISTANCE = config.lookahead_distance; 22 | 23 | PARAMS_UPDATED = true; 24 | }; 25 | 26 | PathTrackerNode::PathTrackerNode() : tf2_listener_(tf2_buffer_) 27 | { 28 | f = boost::bind(&dynamicParamCallback, _1, _2); 29 | server.setCallback(f); 30 | 31 | this->sub_robot_odom_ = nh_.subscribe("/gazebo/ground_truth/state", 1, &PathTrackerNode::robotOdomCallback, this); 32 | this->sub_local_path_ = nh_.subscribe("/me5413_world/planning/local_path", 1, &PathTrackerNode::localPathCallback, this); 33 | this->pub_cmd_vel_ = nh_.advertise("/jackal_velocity_controller/cmd_vel", 1); 34 | 35 | // Initialization 36 | this->robot_frame_ = "base_link"; 37 | this->world_frame_ = "world"; 38 | 39 | this->pid_ = control::PID(0.1, 1.0, -1.0, PID_Kp, PID_Ki, PID_Kd); 40 | }; 41 | 42 | void PathTrackerNode::localPathCallback(const nav_msgs::Path::ConstPtr& path) 43 | { 44 | // Calculate absolute errors (wrt to world frame) 45 | this->pose_world_goal_ = path->poses[11].pose; 46 | this->pub_cmd_vel_.publish(computeControlOutputs(this->odom_world_robot_, this->pose_world_goal_)); 47 | 48 | return; 49 | }; 50 | 51 | void PathTrackerNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom) 52 | { 53 | this->world_frame_ = odom->header.frame_id; 54 | this->robot_frame_ = odom->child_frame_id; 55 | this->odom_world_robot_ = *odom.get(); 56 | 57 | return; 58 | }; 59 | 60 | geometry_msgs::Twist PathTrackerNode::computeControlOutputs(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) 61 | { 62 | // Velocity 63 | tf2::Vector3 robot_vel; 64 | tf2::fromMsg(this->odom_world_robot_.twist.twist.linear, robot_vel); 65 | const double velocity = robot_vel.length(); 66 | 67 | // Update PID controller parameters if they are updated dynamically 68 | if (PARAMS_UPDATED) 69 | { 70 | this->pid_.updateSettings(PID_Kp, PID_Ki, PID_Kd); 71 | PARAMS_UPDATED = false; 72 | } 73 | 74 | // Compute linear speed using PID controller 75 | double target_speed = SPEED_TARGET; 76 | double linear_speed = this->pid_.calculate(target_speed, velocity); 77 | 78 | //Implement Pure Pursuit Controller for Steering 79 | double steering = computeSteering(odom_robot, pose_goal); 80 | 81 | geometry_msgs::Twist cmd_vel; 82 | cmd_vel.linear.x = linear_speed; 83 | cmd_vel.angular.z = steering; 84 | 85 | // std::cout << "robot velocity is " << velocity << " throttle is " << cmd_vel.linear.x << std::endl; 86 | // std::cout << "lateral error is " << lat_error << " heading_error is " << heading_error << " steering is " << cmd_vel.angular.z << std::endl; 87 | 88 | return cmd_vel; 89 | } 90 | 91 | double PathTrackerNode::computeSteering(const nav_msgs::Odometry& odom_robot, const geometry_msgs::Pose& pose_goal) 92 | { 93 | // Compute heading error 94 | double yaw_robot = tf2::getYaw(odom_robot.pose.pose.orientation); 95 | double yaw_goal = tf2::getYaw(pose_goal.orientation); 96 | double heading_error = unifyAngleRange(yaw_goal - yaw_robot); // Ensure heading error is within [-pi, pi] 97 | 98 | // Compute lateral error 99 | tf2::Vector3 point_robot, point_goal; 100 | tf2::fromMsg(odom_robot.pose.pose.position, point_robot); 101 | tf2::fromMsg(pose_goal.position, point_goal); 102 | double dx = point_goal.getX() - point_robot.getX(); 103 | double dy = point_goal.getY() - point_robot.getY(); 104 | double alpha = std::atan2(dy, dx) - yaw_robot; 105 | 106 | // Compute lookahead distance 107 | double lookahead_distance = computeLookaheadDistance(odom_robot); 108 | 109 | // Compute desired steering angle using the pure pursuit formula 110 | double steering = std::atan2(2.0 * ROBOT_LENGTH * std::sin(alpha), lookahead_distance); 111 | 112 | // Incorporate heading error 113 | steering += heading_error; 114 | 115 | // Ensure steering angle is within [-pi, pi] 116 | steering = unifyAngleRange(steering); 117 | 118 | return steering; 119 | } 120 | 121 | double PathTrackerNode::computeLookaheadDistance(const nav_msgs::Odometry& odom_robot) 122 | { 123 | double velocity = odom_robot.twist.twist.linear.x; 124 | return std::max(1.0, velocity * DEFAULT_LOOKAHEAD_DISTANCE); 125 | } 126 | 127 | } // namespace me5413_world 128 | 129 | int main(int argc, char** argv) 130 | { 131 | ros::init(argc, argv, "path_tracker_node"); 132 | me5413_world::PathTrackerNode path_tracker_node; 133 | ros::spin(); // spin the ros node. 134 | return 0; 135 | } -------------------------------------------------------------------------------- /me5413_world/worlds/me5413_planning.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 0 17 | 0 18 | 0 19 | 20 | 21 | 22 | 1 23 | 24 | 25 | 26 | 27 | 0 0 1 28 | 100 100 29 | 30 | 31 | 32 | 33 | 34 | 100 35 | 50 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 10 47 | 48 | 49 | 0 50 | 51 | 52 | 0 0 1 53 | 100 100 54 | 55 | 56 | 57 | 61 | 62 | 63 | 0 64 | 0 65 | 0 66 | 67 | 68 | 0 0 -9.8 69 | 6e-06 2.3e-05 -4.2e-05 70 | 71 | 72 | 0.001 73 | 1 74 | 1000 75 | 76 | 77 | 0.4 0.4 0.4 1 78 | 0.7 0.7 0.7 1 79 | 1 80 | 81 | 82 | 83 | EARTH_WGS84 84 | 0 85 | 0 86 | 0 87 | 0 88 | 89 | 90 | 109 991000000 91 | 38 686185124 92 | 1709730761 14639134 93 | 38670 94 | 95 | 0 0 0 0 -0 0 96 | 1 1 1 97 | 98 | 0 0 0 0 -0 0 99 | 0 0 0 0 -0 0 100 | 0 0 0 0 -0 0 101 | 0 0 0 0 -0 0 102 | 103 | 104 | 105 | 0 0 10 0 -0 0 106 | 107 | 108 | 109 | 1 110 | 111 | 112 | 113 | 114 | 20 20 0.1 115 | 116 | 117 | 10 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 0 133 | 134 | 135 | 20 20 0.1 136 | 137 | 138 | 139 | 144 | 145 | 146 | 0 147 | 0 148 | 0 149 | 150 | 0 0 0 0 0 0 151 | 152 | 153 | 154 | 5 -5 2 0 0.275643 2.35619 155 | orbit 156 | perspective 157 | 158 | 159 | 160 | 161 | --------------------------------------------------------------------------------