# TGH-Planner **Repository Path**: Sytx_1/TGH-Planner ## Basic Information - **Project Name**: TGH-Planner - **Description**: No description available - **Primary Language**: Unknown - **License**: GPL-3.0 - **Default Branch**: main - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 0 - **Forks**: 0 - **Created**: 2025-12-10 - **Last Updated**: 2025-12-10 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # TGH-Planner & EGVG TGH, at the global level, maintains a global topological path container. At the local level, it considers nonholonomic dynamics and unknown obstacle risks in local trajectory optimization to ensure safety. EGVG is an improved GVG (Generalized Voronoi Graph) with stronger spatial representation capabilities. Then, we plan multiple topological trajectories on EGVG to provide global guidance. In this repository, TGH and EGVG work together. - Topology-Guided Hierarchical Planner for Nonholonomic Robots in 2D Unknown and Complex Environments (Accpted T-Mech, 2025). - Enveloped Generalized Voronoi Graph and Multi-Topology Path Planning for Navigation in Unknown Environments (Submitted T-Mech, 2025). PDF Link: [TGH](https://www.techrxiv.org/users/897374/articles/1274757-tgh-planner-topology-guided-hierarchical-planner-for-nonholonomic-robots-in-unknown-and-complex-environments?commit=1f84764f4edc345d91a809fd2cc0ea9d6597c29c), [EGVG](https://www.techrxiv.org/users/897374/articles/1346772-enveloped-generalized-voronoi-graph-and-multi-topology-path-planning-for-navigation-in-unknown-environments). Videoļ¼š[TGH-bilibili](https://www.bilibili.com/video/BV1xeWvznE8J?spm_id_from=333.788.player.switch&vd_source=b0464106123611f8d997cd304ba81efe), [EGVG-bilibili](https://www.bilibili.com/video/BV1zCWYzvE4W/?spm_id_from=333.1387.homepage.video_card.click&vd_source=b0464106123611f8d997cd304ba81efe). TGH focuses on generating executable and safe trajectories for non-holonomic robots.

TGH

EGVG focuses on providing efficient global guidance for navigation in unknown environments.

EGVG

Please kindly star :star: this project if it helps you. We take great efforts to develope and maintain it :grin::grin:. Our work is based on the the excellent work of [FAST-Planner](https://github.com/HKUST-Aerial-Robotics/Fast-Planner). ## 1. Build and Start **Dependencies:** Ubuntn20.04, ROS1, OpenCV. Install required tools: ```bash sudo apt-get install ros-noetic-navigation \ ros-noetic-gazebo-* \ ros-noetic-gazebo-ros-control* \ ros-noetic-controller-* \ ros-noetic-tf2-* \ ros-noetic-octomap-* \ ros-noetic-velodyne-* \ ros-noetic-pointgrey-camera-description \ ros-noetic-twist-mux \ ros-noetic-teleop-twist-joy \ ros-noetic-lms1xx \ ros-noetic-interactive-marker-twist-server \ libgoogle-glog-dev \ libignition-common3-graphics-dev \ libignition-common3-profiler-dev \ python3-tk \ xterm ``` Download [gazebo_models](https://github.com/osrf/gazebo_models) in advance, and put them in directory `~/.gazebo/models`. **Step1**: ``` cd ${YOUR_WORKSPACE_PATH}/src git clone https://github.com/qimao7213/TGH-Planner.git ``` **Step2**: Unzip the **3rd.zip** to a different path of your choice, and run the following codes for each folder: ``` mkdir build && cd build && cmake .. make sudo make install ``` **Step3**: Change the *nlopt path* in "*fast_planner/bspline_opt/CMakeLists.txt Line 19*" to your path. Change the *FilePath* in "*fast_planner/plan_manage/kino_replan.launch Line 5*" to your path if you want to save trajectory and odometry information. **Step4**: ``` catkin_make ``` If you meet: ``` Could not find a package configuration file provided by "bspline" with any of the following names... ``` run ``` catkin_make -DCATKIN_WHITELIST_PACKAGES=bspline ``` and then ``` catkin_make -DCATKIN_WHITELIST_PACKAGES= ``` **Step5**: Open one terminals and run: For TGH: ``` roslaunch plan_manage kino_replan.launch B_Spline_LocalPlanner:=true sim_pose:=true ``` For EGVG ``` roslaunch plan_manage kino_replan.launch B_Spline_LocalPlanner:=false sim_pose:=false ``` Then you can select a goal in Rviz and the robot will move. ### **Algorithm Configuration** At the beginning of `kino_replan.launch`, there are three parameters: `Diff_Model`, `B_Spline_LocalPlanner`, and `sim_pose`. Their meanings are as follows: - `Diff_Model=true`: The control command is ROS's `/cmd_vel` (linear and angular velocity). Otherwise, the control command is for the bicycle model (acceleration and front wheel steering angle). - `B_Spline_LocalPlanner=true`: The local optimization uses *B-spline curves*. Otherwise, the local control uses the *CMU local planner*. - `sim_pose=true`: The robot's pose is generated by the Runge-Kutta integration. Otherwise, the robot's pose is generated by sending `/cmd_vel` to the Jackal robot, and the pose is derived from Gazebo ground truth. ### **Generate /cmd_vel Commands** If `B_Spline_LocalPlanner=false`, the CMU local planner generates `/cmd_vel`. Otherwise, the local planner uses NMPC implemented in *car_mpc_nodelet.cpp*. Originally, it is based on the bicycle model, using NMPC to track the B-spline trajectory and generate control commands (acceleration and front wheel steering angle). Currently, there are two ways to generate `/cmd_vel` commands: 1. Convert the control commands generated by NMPC into `/cmd_vel` (this is already implemented and published). 2. Modify `if(0)` to `if(1)` at **[car_mpc_nodelet.cpp](https://github.com/qimao7213/TGH-Planner/blob/main/car_simulator/car_mpc/src/car_mpc_nodelet.cpp#L152)** in the code, which will directly generate `/cmd_vel` commands from the B-spline trajectory. However, the **cmd\_vel** generated by both methods is unstable. Therefore, implementing an **NMPC for the differential drive model** is on the to-do list. **Note:** If you are using **fastlio2** as the pose estimation algorithm, make sure that the odometry information it publishes contains both velocity and angular velocity data. ## 2. Some Utils ### Predifined Waypoints Difine a set of waypoints in "*[waypts_2d.yaml](fast_planner/plan_manage/config/waypts_2d.yaml)*", and set "*[waypoint_type](https://github.com/qimao7213/TGH-Planner/blob/main/TGH_Planner/plan_manage/launch/kino_replan.launch#L109)*" from "manual-lonely-waypoint" to "waypts-target-2D". Then when you set any goal in Rviz, the robot will traverse the predefined waypoints. ### **Services** Open ros service gui and you can see some services: ``` cd ${YOUR_WORKSPACE_PATH}/src source devel/setup.bash rosrun rqt_service_caller rqt_service_caller ``` - /car_simulator/set_init_pose Set {x, y, yaw} pose to your robot. If the yaw > 360, the robot will be reset to the initial pose. - /planning/reset_env Reset the global map. ### **Record B-Spline Trajectory** Set "*[save_traj_info](https://github.com/qimao7213/TGH-Planner/blob/main/TGH_Planner/plan_manage/launch/kino_algorithm.xml#L118)*" to **true**, the latest optimized B-Spline Trajectories will be recored to "*Utils/bspline_analysis*". Use the "*.py*" functions to analyse the B-Spline Trajectories. ### **Draw Robot Trajectory on Map** Use [collision_map_creator_plugin](https://github.com/tik0/collision_map_creator_plugin) to transform a Gazebo world to a "*.png*" 2D map. Example code: ``` ./request_publisher "(-50,-10)(10,10)" 1 0.02 $(pwd)/map.png ``` ## 3. Deployment on A Real Robot We need **depth image**, **camera pose** and ?????. I will update this section later... ## 4. I will continue to improve this repository and enhance the code in the future.