Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Introducing dynamic obstacle support (rst-tu-dortmund#54)
* teb_local_planner uses getObstacles() instead of getPolygons() * Added script for publishing dynamic obstacles * teb_local_planner::Obstacle centroidVelocity is set using costmap_converter, custom_obstacle_msgs or the test_optim_node * Option 2 implemented: Dynamic obstacle evasion based on sum of dt * Made EdgeDynamicObstacle a UnaryEdge * Temporary commit for testing purposes * Added check for safety * Added methods for calculating the spatiotemporal distance between obstacles and robot to obstacles.h and robot_footprint_model.h * Footprints of robot and obstacle are now considered for distance estimation during optimization * Updated ROS_ASSERTs for EdgeDynamicObstacle * Added buffer zone around predicted locations of dynamic obstacles: New parameters dynamic_obstacle_inflation_dist, weight_dynamic_obstacle_inflation * Static obstacle edges are not added for dynamic obstacles anymore * Visualization for predicted positions of dynamic point obstacles added. TODO: Line/Polygon * Enabled homotopy class planning in the test_optim_node * Outsourced GraphSearch from HomotopyClassPlanner to own class. * Predicted obstacles are no longer visualized * simple_exploration in rqt_reconfigure working again * Added dynamic_reconfigure switch 'include_dynamic_obstacles' * Revert "Removed TebConfig dependency in TebVisualization" This reverts commit c33f6b8. * Added function to access predicted positions of dynamic obstacles * Changed frame information in publish_dynamic_obstacle * Added dynamic_reconfigure option for visualization with time as z-axis (currently only for point obstacles) * Added equivalence relation for 3d homotopy class planning based on Ampere's law (Bhattacharya, 2011) * Added time information from the teb to the calculation of the 3d H-Signature * Obstacles far away from the trajectory are ignored in trajectory planning * Bugfix: Correct timediff is used for H-signature calculation * Temporary TEBs are constructed, timediffs of these are used to calculate the H-Signature and evaluate, whether they constitute a new Equivalence class * Changed some parameters * Better initialization of a teb with less than min_samples vertices * Fix: Homotopy classes are not altered during optimization anymore due to too few poses between start and goal * TEBs are only added to the trajectory container, if they constitute a new equivalence class * Looping trajectories are deleted from the set of candidates * Added dynamic obstacles to test_optim_node * (Commented) option to save the Hessian * Remove moving dynamic obstacle from test_optim_node * FeedbackMsg includes a ObstacleMsg instead of a polygon * ObstacleMsg removed from package since it is now part of the costmap_converter package. * trajectory resize (autoResize) updated: the new version inserts and removes poses until convergence * h-signature implementations moved to a separate header. * trajectory initialization now uses a max_vel_x parameter instead of the desired time difference in order to give the optimizer a better warm start. Old methods are marked as deprecated. * default config updated: dynamic obstacles deactivated. 3d homotopy class planning is now always active if dynamic obstacles are activated * inplace rotations removed from trajectory initialization to improve convergence speed of the optimizer * Homotopy graph instantiation moved to initialize method. Simple graph exploration must now be turned on prior to node start. Related parameters removed from dynamic reconfigure. This commit fixes the 'freezed' sampling of waypoints in the graph. * obstacle msg type adapted to new format in costmap_converter. Own obstacle message type reverted for backward compatibility with custom obstacles. * Added Subscribers for obstacle velocities to the test_optim_node (cherry picked from commit a50584c9261bd703b8f328933cf72deac53e5106) * the robot's odom topic is now passed to the costmap_converter via class method * teb_local_planner::ObstacleMsg removed in favor of costmap_converter::ObstacleArrayMsg. This also requires custom obstacle publishers to update to the new format * replaced remaining teb_local_planner/ObstacleMsg with costmap_converter/ObstacleArrayMsg * code cleanup concerning prediction of obstacles * custom weight added to dynamic obstacle edges * the equivalence class can now be accessed via public api (read-only) * the "new" trajectory resizing method is only activated now, if "include_dynamic_obstacles" is set to true. We introduced the non-fast mode with the support of dynamic obstacles (which leads to better results in terms of x-y-t homotopy planning). However, we have not tested this mode intensively yet, so we keep the legacy fast mode as default until we finish our tests.
- Loading branch information
