├── srv └── PlannerGetProgress.srv ├── mbf_plugin.xml ├── recovery_plugin.xml ├── msg └── PID.msg ├── .gitignore ├── README.md ├── include └── ftc_local_planner │ ├── backward_forward_recovery.h │ ├── oscillation_detector.h │ └── ftc_planner.h ├── package.xml ├── CMakeLists.txt ├── cfg └── FTCPlanner.cfg └── src ├── backward_forward_recovery.cpp ├── oscillation_detector.cpp └── ftc_planner.cpp /srv/PlannerGetProgress.srv: -------------------------------------------------------------------------------- 1 | --- 2 | uint32 index -------------------------------------------------------------------------------- /mbf_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a local planner using a FTC algorithmus. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /recovery_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A simple recovery behavior plugin to try driving directly backwards and forwards. 5 | 6 | 7 | -------------------------------------------------------------------------------- /msg/PID.msg: -------------------------------------------------------------------------------- 1 | time stamp 2 | 3 | # P set point 4 | float32 kp_lon_set 5 | float32 kp_lat_set 6 | float32 kp_ang_set 7 | 8 | # I set point 9 | float32 ki_lon_set 10 | float32 ki_lat_set 11 | float32 ki_ang_set 12 | 13 | # D set point 14 | float32 kd_lon_set 15 | float32 kd_lat_set 16 | float32 kd_ang_set 17 | 18 | # errors 19 | float32 lon_err 20 | float32 lat_err 21 | float32 ang_err 22 | 23 | # speeds 24 | float32 ang_speed 25 | float32 lin_speed -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # FTCLocalPlanner 2 | 3 | This repository contains a very simple "follow the carrot" local planner implementation. 4 | 5 | ## Description 6 | The planner follows a given path as exactly as possible. It calculate the position of the carrot by 7 | taking robots velocity limits into account. If deviation between carrot and robot is above a given 8 | threshold, the planner stops. 9 | 10 | Also it checks for collisions of carrot with obstacles as well as collisions of robot footprint at actual robot pose. It doesn't implement obstacle aviodance but obstacle detection. Following situation will cause the 11 | planner to exit: 12 | - goal reached 13 | - goal (pose) timeout reached 14 | - deviation between carrot and robot 15 | - collision of carrot with obstacles 16 | - collision of robot footprint with obstacles 17 | 18 | ## Published Topics 19 | - **`global_point`** (geometry_msgs/PoseStamped) pose of carrot 20 | - **`global_plan`** (nav_msgs/Path) global path to follow 21 | - **`debug_pid`** (ftc_local_planner/PID) debug information of PID calculation 22 | - **`costmap_marker`** (visualization_msgs/Marker) debug information of costmap check 23 | 24 | -------------------------------------------------------------------------------- /include/ftc_local_planner/backward_forward_recovery.h: -------------------------------------------------------------------------------- 1 | #ifndef BACKWARD_FORWARD_RECOVERY_H 2 | #define BACKWARD_FORWARD_RECOVERY_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace ftc_local_planner 12 | { 13 | 14 | class BackwardForwardRecovery : public nav_core::RecoveryBehavior 15 | { 16 | public: 17 | BackwardForwardRecovery(); 18 | void initialize(std::string name, tf2_ros::Buffer* tf, 19 | costmap_2d::Costmap2DROS* global_costmap, 20 | costmap_2d::Costmap2DROS* local_costmap); 21 | void runBehavior(); 22 | 23 | private: 24 | bool attemptMove(double distance, bool forward); 25 | bool isPositionValid(double x, double y); 26 | 27 | std::string name_; 28 | bool initialized_; 29 | tf2_ros::Buffer* tf_; 30 | costmap_2d::Costmap2DROS* global_costmap_, *local_costmap_; 31 | ros::Publisher cmd_vel_pub_; 32 | double max_distance_; 33 | double linear_vel_; 34 | double check_frequency_; 35 | unsigned char max_cost_threshold_; 36 | ros::Duration timeout_; 37 | }; 38 | 39 | } // namespace ftc_local_planner 40 | 41 | #endif // BACKWARD_FORWARD_RECOVERY_H -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | ftc_local_planner 3 | 1.0.1 4 | A simple FTC Planner implementation 5 | Clemens Elflein 6 | Clemens Elflein 7 | BSD 8 | 9 | catkin 10 | cmake_modules 11 | costmap_2d 12 | dynamic_reconfigure 13 | eigen 14 | nav_core 15 | nav_msgs 16 | pluginlib 17 | pcl_conversions 18 | roscpp 19 | tf 20 | tf2_geometry_msgs 21 | tf2_ros 22 | mbf_costmap_core 23 | message_runtime 24 | std_msgs 25 | message_generation 26 | tf2_eigen 27 | 28 | tf2_ros 29 | tf2_geometry_msgs 30 | costmap_2d 31 | dynamic_reconfigure 32 | eigen 33 | nav_core 34 | nav_msgs 35 | pluginlib 36 | roscpp 37 | tf 38 | mbf_costmap_core 39 | message_runtime 40 | std_msgs 41 | tf2_eigen 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ftc_local_planner) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | costmap_2d 7 | dynamic_reconfigure 8 | nav_core 9 | nav_msgs 10 | pluginlib 11 | roscpp 12 | tf 13 | tf2 14 | tf2_geometry_msgs 15 | tf2_ros 16 | geometry_msgs 17 | std_msgs 18 | message_generation 19 | ) 20 | 21 | 22 | find_package(Eigen3 REQUIRED) 23 | include_directories( 24 | include 25 | ${catkin_INCLUDE_DIRS} 26 | ${Eigen3_INCLUDE_DIRS} 27 | ) 28 | add_definitions(${Eigen3_DEFINITIONS}) 29 | 30 | 31 | # dynamic reconfigure 32 | generate_dynamic_reconfigure_options( 33 | cfg/FTCPlanner.cfg 34 | ) 35 | 36 | ## Generate services in the 'srv' folder 37 | add_service_files( 38 | FILES 39 | PlannerGetProgress.srv 40 | ) 41 | 42 | add_message_files( 43 | FILES 44 | PID.msg 45 | ) 46 | 47 | generate_messages( 48 | DEPENDENCIES 49 | std_msgs 50 | ) 51 | 52 | 53 | catkin_package( 54 | INCLUDE_DIRS include 55 | LIBRARIES ftc_local_planner 56 | CATKIN_DEPENDS 57 | dynamic_reconfigure 58 | pluginlib 59 | roscpp 60 | tf 61 | tf2_geometry_msgs 62 | tf2_ros 63 | ) 64 | 65 | catkin_package( 66 | CATKIN_DEPENDS message_runtime 67 | ) 68 | 69 | add_library(ftc_local_planner 70 | src/ftc_planner.cpp 71 | src/oscillation_detector.cpp) 72 | target_link_libraries(ftc_local_planner ${catkin_LIBRARIES}) 73 | add_dependencies(ftc_local_planner ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 74 | add_dependencies(ftc_local_planner ftc_local_planner_gencfg) 75 | add_dependencies(ftc_local_planner ftc_local_planner_generate_messages_cpp) 76 | 77 | add_library(backward_forward_recovery src/backward_forward_recovery.cpp) 78 | target_link_libraries(backward_forward_recovery ${catkin_LIBRARIES}) 79 | 80 | install(TARGETS ftc_local_planner backward_forward_recovery 81 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 82 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 83 | ) 84 | 85 | install(FILES blp_plugin.xml 86 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 87 | ) 88 | 89 | install(DIRECTORY include/${PROJECT_NAME}/ 90 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 91 | PATTERN ".svn" EXCLUDE 92 | ) 93 | 94 | -------------------------------------------------------------------------------- /cfg/FTCPlanner.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | grp_cp = gen.add_group("ControlPoint", type="tab") 8 | grp_cp.add("speed_fast", double_t, 0, "The max speed with which the control point is moved along the path if the straight distance is larger than speed_fast_threshold", 0.5, 0, 2.0) 9 | grp_cp.add("speed_fast_threshold", double_t, 0, "Min straight distance in front of the robot for fast speed to be used", 1.5, 0, 5.0) 10 | grp_cp.add("speed_fast_threshold_angle", double_t, 0, "Max angle between current pose and pose on path to still count as straight line", 5.0, 0, 45.0) 11 | grp_cp.add("speed_slow", double_t, 0, "Slow speed", 0.2, 0, 2.0) 12 | grp_cp.add("speed_angular", double_t, 0, "The max angular speed with which the control point is moved along the path", 20.0, 1.0, 150.0) 13 | grp_cp.add("acceleration", double_t, 0, "Acceleration for speed transition", 1.0, 0, 10.0) 14 | 15 | grp_pid = gen.add_group("PID", type="tab") 16 | grp_pid.add("kp_lon", double_t, 0, "KP for speed controller lon", 1.0, 0.0, 50.0) 17 | grp_pid.add("ki_lon", double_t, 0, "KI for speed controller lon", 0.0, 0.0, 1.0) 18 | grp_pid.add("ki_lon_max", double_t, 0, "KI windup for speed controller lon", 10.0, 0.0, 100.0) 19 | grp_pid.add("kd_lon", double_t, 0, "KD for speed controller lon", 0.0, 0.0, 1.0) 20 | grp_pid.add("ki_lat", double_t, 0, "KI for speed controller lat", 0.0, 0.0, 5.0) 21 | grp_pid.add("ki_lat_max", double_t, 0, "KI windup for speed controller lat", 10.0, 0.0, 500.0) 22 | grp_pid.add("kp_lat", double_t, 0, "KP for speed controller lat", 1.0, 0.0, 250.0) 23 | grp_pid.add("kd_lat", double_t, 0, "KD for speed controller lat", 0.0, 0.0, 25.0) 24 | grp_pid.add("kp_ang", double_t, 0, "KP for angle controller", 1.0, 0.0, 50.0) 25 | grp_pid.add("ki_ang", double_t, 0, "KI for angle controller", 0.0, 0.0, 5.0) 26 | grp_pid.add("ki_ang_max", double_t, 0, "KI windup for speed controller angle", 10.0, 0.0, 500.0) 27 | grp_pid.add("kd_ang", double_t, 0, "KD for angle controller", 0.0, 0.0, 5.0) 28 | 29 | grp_bot = gen.add_group("Robot", type="tab") 30 | grp_bot.add("max_cmd_vel_speed", double_t, 0, "Max speed to send to the controller", 2.0, 0.0, 5.0) 31 | grp_bot.add("max_cmd_vel_ang", double_t, 0, "Max angular speed to send to the controller", 2.0, 0.0, 5.0) 32 | grp_bot.add("max_goal_distance_error", double_t, 0, "Wait for robot to get closer than max_goal_distance_error to the goal before setting goal as finished", 1.0, 0.0, 3.0) 33 | grp_bot.add("max_goal_angle_error", double_t, 0, "Wait for robot to get a better angle than max_goal_angle_error before setting goal as finished", 10.0, 0.0, 360.0) 34 | grp_bot.add("goal_timeout", double_t, 0, "Timeout for max_goal_distance_error and max_goal_angle_error", 5.0, 0.0, 100.0) 35 | grp_bot.add("max_follow_distance", double_t, 0, "Stop controller if robot is further away than max_follow_distance (i.e. crash detection)", 1.0, 0.0, 3.0) 36 | 37 | 38 | 39 | gen.add("forward_only", bool_t, 0, "Only allow forward driving", True) 40 | gen.add("restore_defaults", bool_t, 0, " Load default config", False) 41 | gen.add("debug_pid", bool_t, 0, " Emit debug_pid topic", False) 42 | 43 | # Recovery 44 | grp_recovery = gen.add_group("recovery", type="tab") 45 | grp_recovery.add("oscillation_recovery", bool_t, 0, 46 | "Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).", 47 | True) 48 | grp_recovery.add("oscillation_v_eps", double_t, 0, "Oscillation velocity eps", 5.0, 0.0, 100.0) 49 | grp_recovery.add("oscillation_omega_eps", double_t, 0, "Oscillation omega eps", 5.0, 0.0, 100.0) 50 | grp_recovery.add("oscillation_recovery_min_duration", double_t, 0, "duration until recovery in s", 5.0, 0.0, 100.0) 51 | 52 | # Obstacles 53 | grp_obstacles = gen.add_group("obstacles", type="tab") 54 | grp_obstacles.add("check_obstacles", bool_t, 0, "check and stop for obstacles in path", True) 55 | grp_obstacles.add("obstacle_lookahead", int_t, 0, "how many path segments should be used for collision check", 5,0,20) 56 | grp_obstacles.add("obstacle_footprint", bool_t, 0, "check footprint at actual pose for collision", True) 57 | grp_obstacles.add("debug_obstacle", bool_t, 0, "debug obstacle check as marker array", True) 58 | 59 | exit(gen.generate("ftc_local_planner", "ftc_local_planner", "FTCPlanner")) 60 | -------------------------------------------------------------------------------- /src/backward_forward_recovery.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | namespace ftc_local_planner 8 | { 9 | 10 | BackwardForwardRecovery::BackwardForwardRecovery() 11 | : initialized_(false), 12 | max_distance_(0.5), 13 | linear_vel_(0.3), 14 | check_frequency_(10.0), 15 | max_cost_threshold_(costmap_2d::INSCRIBED_INFLATED_OBSTACLE-10), 16 | timeout_(ros::Duration(3.0)) {} 17 | 18 | void BackwardForwardRecovery::initialize(std::string name, tf2_ros::Buffer* tf, 19 | costmap_2d::Costmap2DROS* global_costmap, 20 | costmap_2d::Costmap2DROS* local_costmap) 21 | { 22 | if(!initialized_){ 23 | name_ = name; 24 | tf_ = tf; 25 | global_costmap_ = global_costmap; 26 | local_costmap_ = local_costmap; 27 | 28 | ros::NodeHandle private_nh("~/" + name_); 29 | cmd_vel_pub_ = private_nh.advertise("/cmd_vel", 1); 30 | 31 | private_nh.param("max_distance", max_distance_, 0.5); 32 | private_nh.param("linear_vel", linear_vel_, 0.3); 33 | private_nh.param("check_frequency", check_frequency_, 10.0); 34 | int temp_threshold; 35 | private_nh.param("max_cost_threshold", temp_threshold, static_cast(costmap_2d::INSCRIBED_INFLATED_OBSTACLE-10)); 36 | max_cost_threshold_ = static_cast(temp_threshold); 37 | 38 | double timeout_seconds; 39 | private_nh.param("timeout", timeout_seconds, 3.0); 40 | timeout_ = ros::Duration(timeout_seconds); 41 | 42 | initialized_ = true; 43 | } 44 | else{ 45 | ROS_ERROR("You should not call initialize twice on this object, doing nothing"); 46 | } 47 | } 48 | 49 | void BackwardForwardRecovery::runBehavior() 50 | { 51 | if (!initialized_) 52 | { 53 | ROS_ERROR("This object must be initialized before runBehavior is called"); 54 | return; 55 | } 56 | 57 | ROS_WARN("Running Backward/Forward recovery behavior"); 58 | 59 | if (attemptMove(max_distance_, false)) { 60 | ROS_INFO("Successfully moved backwards"); 61 | return; 62 | } 63 | 64 | if (attemptMove(max_distance_, true)) { 65 | ROS_INFO("Successfully moved forwards"); 66 | return; 67 | } 68 | 69 | ROS_WARN("Backward/Forward recovery behavior failed to move in either direction"); 70 | } 71 | 72 | bool BackwardForwardRecovery::attemptMove(double distance, bool forward) 73 | { 74 | geometry_msgs::PoseStamped start_pose; 75 | local_costmap_->getRobotPose(start_pose); 76 | 77 | ros::Rate rate(check_frequency_); 78 | geometry_msgs::Twist cmd_vel; 79 | cmd_vel.linear.x = forward ? linear_vel_ : -linear_vel_; 80 | 81 | double moved_distance = 0.0; 82 | ros::Time start_time = ros::Time::now(); 83 | while (moved_distance < distance && (ros::Time::now() - start_time) < timeout_) 84 | { 85 | geometry_msgs::PoseStamped current_pose; 86 | local_costmap_->getRobotPose(current_pose); 87 | 88 | moved_distance = std::hypot( 89 | current_pose.pose.position.x - start_pose.pose.position.x, 90 | current_pose.pose.position.y - start_pose.pose.position.y 91 | ); 92 | 93 | if (!isPositionValid(current_pose.pose.position.x, current_pose.pose.position.y)) 94 | { 95 | ROS_WARN("Reached maximum allowed cost after moving %.2f meters", moved_distance); 96 | cmd_vel.linear.x = 0; 97 | cmd_vel_pub_.publish(cmd_vel); 98 | return false; 99 | } 100 | 101 | cmd_vel_pub_.publish(cmd_vel); 102 | rate.sleep(); 103 | } 104 | 105 | cmd_vel.linear.x = 0; 106 | cmd_vel_pub_.publish(cmd_vel); 107 | 108 | if (moved_distance >= distance) { 109 | ROS_INFO("%s movement completed successfully", forward ? "Forward" : "Backward"); 110 | return true; 111 | } else { 112 | ROS_WARN("%s movement timed out after %.2f seconds", forward ? "Forward" : "Backward", timeout_.toSec()); 113 | return false; 114 | } 115 | } 116 | 117 | bool BackwardForwardRecovery::isPositionValid(double x, double y) 118 | { 119 | unsigned int mx, my; 120 | if (local_costmap_->getCostmap()->worldToMap(x, y, mx, my)) 121 | { 122 | unsigned char cost = local_costmap_->getCostmap()->getCost(mx, my); 123 | return (cost <= max_cost_threshold_); 124 | } 125 | return false; 126 | } 127 | 128 | } // namespace ftc_local_planner 129 | 130 | PLUGINLIB_EXPORT_CLASS(ftc_local_planner::BackwardForwardRecovery, nav_core::RecoveryBehavior) -------------------------------------------------------------------------------- /src/oscillation_detector.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | 42 | namespace ftc_local_planner 43 | { 44 | 45 | // ============== FailureDetector Implementation =================== 46 | 47 | void FailureDetector::update(const geometry_msgs::TwistStamped& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps) 48 | { 49 | if (buffer_.capacity() == 0) 50 | return; 51 | 52 | VelMeasurement measurement; 53 | measurement.v = twist.twist.linear.x; // just consider linear velocity in x-direction in the robot frame for now 54 | measurement.omega = twist.twist.angular.z; 55 | if (measurement.v > 0 && v_max>0) 56 | measurement.v /= v_max; 57 | else if (measurement.v < 0 && v_backwards_max > 0) 58 | measurement.v /= v_backwards_max; 59 | 60 | if (omega_max > 0) 61 | measurement.omega /= omega_max; 62 | 63 | buffer_.push_back(measurement); 64 | 65 | 66 | 67 | // immediately compute new state 68 | detect(v_eps, omega_eps); 69 | } 70 | 71 | void FailureDetector::clear() 72 | { 73 | buffer_.clear(); 74 | oscillating_ = false; 75 | } 76 | 77 | bool FailureDetector::isOscillating() const 78 | { 79 | return oscillating_; 80 | } 81 | 82 | bool FailureDetector::detect(double v_eps, double omega_eps) 83 | { 84 | oscillating_ = false; 85 | 86 | if (buffer_.size() < buffer_.capacity()/2) // we start detecting only as soon as we have the buffer filled at least half 87 | return false; 88 | 89 | double n = (double)buffer_.size(); 90 | 91 | // compute mean for v and omega 92 | double v_mean=0; 93 | double omega_mean=0; 94 | int omega_zero_crossings = 0; 95 | for (int i=0; i < n; ++i) 96 | { 97 | v_mean += buffer_[i].v; 98 | omega_mean += buffer_[i].omega; 99 | if ( i>0 && sign(buffer_[i].omega) != sign(buffer_[i-1].omega) ) 100 | ++omega_zero_crossings; 101 | } 102 | v_mean /= n; 103 | omega_mean /= n; 104 | 105 | if (std::abs(v_mean) < v_eps && std::abs(omega_mean) < omega_eps && omega_zero_crossings>1 ) 106 | { 107 | oscillating_ = true; 108 | } 109 | // ROS_INFO_STREAM("v: " << std::abs(v_mean) << ", omega: " << std::abs(omega_mean) << ", zero crossings: " << omega_zero_crossings); 110 | return oscillating_; 111 | } 112 | 113 | int FailureDetector::sign(double x) { 114 | if (x > 0) 115 | return 1; 116 | else if (x < 0) 117 | return -1; 118 | else 119 | return 0; 120 | } 121 | 122 | 123 | } // namespace ftc_local_planner -------------------------------------------------------------------------------- /include/ftc_local_planner/oscillation_detector.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #ifndef OSCILLATION_DETECTOR_H__ 40 | #define OSCILLATION_DETECTOR_H__ 41 | 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | namespace ftc_local_planner 48 | { 49 | 50 | 51 | /** 52 | * @class FailureDetector 53 | * @brief This class implements methods in order to detect if the robot got stucked or is oscillating 54 | * 55 | * The StuckDetector analyzes the last N commanded velocities in order to detect whether the robot 56 | * might got stucked or oscillates between left/right/forward/backwards motions. 57 | * 58 | * Taken from teb_local_planner 59 | */ 60 | class FailureDetector 61 | { 62 | public: 63 | 64 | /** 65 | * @brief Default constructor 66 | */ 67 | FailureDetector() {} 68 | 69 | /** 70 | * @brief destructor. 71 | */ 72 | ~FailureDetector() {} 73 | 74 | /** 75 | * @brief Set buffer length (measurement history) 76 | * @param length number of measurements to be kept 77 | */ 78 | void setBufferLength(int length) {buffer_.set_capacity(length);} 79 | 80 | /** 81 | * @brief Add a new twist measurement to the internal buffer and compute a new decision 82 | * @param twist geometry_msgs::TwistStamped velocity information 83 | * @param v_max maximum forward translational velocity 84 | * @param v_backwards_max maximum backward translational velocity 85 | * @param omega_max maximum angular velocity 86 | * @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1) 87 | * @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1) 88 | */ 89 | void update(const geometry_msgs::TwistStamped &twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps); 90 | 91 | /** 92 | * @brief Check if the robot got stucked 93 | * 94 | * This call does not compute the actual decision, 95 | * since it is computed within each update() invocation. 96 | * @return true if the robot got stucked, false otherwise. 97 | */ 98 | bool isOscillating() const; 99 | 100 | /** 101 | * @brief Clear the current internal state 102 | * 103 | * This call also resets the internal buffer 104 | */ 105 | void clear(); 106 | 107 | protected: 108 | 109 | /** Variables to be monitored */ 110 | struct VelMeasurement 111 | { 112 | double v = 0; 113 | double omega = 0; 114 | }; 115 | 116 | /** 117 | * @brief Detect if the robot got stucked based on the current buffer content 118 | * 119 | * Afterwards the status might be checked using gotStucked(); 120 | * @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1) 121 | * @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1) 122 | * @return true if the robot got stucked, false otherwise 123 | */ 124 | bool detect(double v_eps, double omega_eps); 125 | 126 | private: 127 | 128 | boost::circular_buffer buffer_; //!< Circular buffer to store the last measurements @see setBufferLength 129 | bool oscillating_ = false; //!< Current state: true if robot is oscillating 130 | static inline int sign(double x); 131 | 132 | }; 133 | 134 | 135 | } // namespace ftc_local_planner 136 | 137 | #endif /* OSCILLATION_DETECTOR_H__ */ -------------------------------------------------------------------------------- /include/ftc_local_planner/ftc_planner.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef FTC_LOCAL_PLANNER_FTC_PLANNER_H_ 3 | #define FTC_LOCAL_PLANNER_FTC_PLANNER_H_ 4 | 5 | #include 6 | #include "ftc_local_planner/PlannerGetProgress.h" 7 | #include "ftc_local_planner/oscillation_detector.h" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "tf2_eigen/tf2_eigen.h" 23 | #include 24 | #include 25 | 26 | namespace ftc_local_planner 27 | { 28 | 29 | class FTCPlanner : public mbf_costmap_core::CostmapController 30 | { 31 | 32 | enum PlannerState 33 | { 34 | PRE_ROTATE, 35 | FOLLOWING, 36 | WAITING_FOR_GOAL_APPROACH, 37 | POST_ROTATE, 38 | FINISHED 39 | }; 40 | 41 | private: 42 | ros::ServiceServer progress_server; 43 | // State tracking 44 | PlannerState current_state; 45 | ros::Time state_entered_time; 46 | 47 | bool is_crashed; 48 | 49 | dynamic_reconfigure::Server *reconfig_server; 50 | 51 | tf2_ros::Buffer *tf_buffer; 52 | costmap_2d::Costmap2DROS *costmap; 53 | costmap_2d::Costmap2D* costmap_map_; 54 | 55 | std::vector global_plan; 56 | ros::Publisher global_point_pub; 57 | ros::Publisher global_plan_pub; 58 | ros::Publisher progress_pub; 59 | ros::Publisher obstacle_marker_pub; 60 | 61 | FTCPlannerConfig config; 62 | 63 | Eigen::Affine3d current_control_point; 64 | 65 | /** 66 | * PID State 67 | */ 68 | double lat_error, lon_error, angle_error = 0.0; 69 | double last_lon_error = 0.0; 70 | double last_lat_error = 0.0; 71 | double last_angle_error = 0.0; 72 | double i_lon_error = 0.0; 73 | double i_lat_error = 0.0; 74 | double i_angle_error = 0.0; 75 | ros::Time last_time; 76 | 77 | /** 78 | * Speed ramp for acceleration and deceleration 79 | */ 80 | double current_movement_speed; 81 | 82 | /** 83 | * State for point interpolation 84 | */ 85 | uint32_t current_index; 86 | double current_progress; 87 | Eigen::Affine3d local_control_point; 88 | 89 | /** 90 | * Private members 91 | */ 92 | ros::Publisher pubPid; 93 | FailureDetector failure_detector_; //!< Detect if the robot got stucked 94 | ros::Time time_last_oscillation_; //!< Store at which time stamp the last oscillation was detected 95 | bool oscillation_detected_ = false; 96 | bool oscillation_warning_ = false; 97 | 98 | double distanceLookahead(); 99 | PlannerState update_planner_state(); 100 | void update_control_point(double dt); 101 | void calculate_velocity_commands(double dt, geometry_msgs::TwistStamped &cmd_vel); 102 | 103 | /** 104 | * @brief check for obstacles in path as well as collision at actual pose 105 | * @param max_points number of path segments (of global path) to check 106 | * @return true if collision will happen. 107 | */ 108 | bool checkCollision(int max_points); 109 | 110 | /** 111 | * @brief check if robot oscillates (only angular). Can be used to do some recovery 112 | * @param cmd_vel last velocity message send to robot 113 | * @return true if robot oscillates 114 | */ 115 | bool checkOscillation(geometry_msgs::TwistStamped &cmd_vel); 116 | 117 | /** 118 | * @brief publish obstacles on path as marker array. 119 | * @brief If obstacle_points contains more elements than maxID, marker gets published and 120 | * @brief cleared afterwards. 121 | * @param obstacle_points already collected points to visualize 122 | * @param x X position in costmap 123 | * @param y Y position in costmap 124 | * @param cost cost value of cell 125 | * @param maxIDs num of markers before publishing 126 | * @return sum of `values`, or 0.0 if `values` is empty. 127 | */ 128 | void debugObstacle(visualization_msgs::Marker &obstacle_points, double x, double y, unsigned char cost, int maxIDs); 129 | 130 | double time_in_current_state() 131 | { 132 | return (ros::Time::now() - state_entered_time).toSec(); 133 | } 134 | 135 | void reconfigureCB(FTCPlannerConfig &config, uint32_t level); 136 | 137 | public: 138 | FTCPlanner(); 139 | 140 | bool getProgress(ftc_local_planner::PlannerGetProgressRequest &req, ftc_local_planner::PlannerGetProgressResponse &res); 141 | 142 | bool setPlan(const std::vector &plan) override; 143 | 144 | void initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros) override; 145 | 146 | ~FTCPlanner() override; 147 | 148 | uint32_t 149 | computeVelocityCommands(const geometry_msgs::PoseStamped &pose, const geometry_msgs::TwistStamped &velocity, 150 | geometry_msgs::TwistStamped &cmd_vel, std::string &message) override; 151 | 152 | bool isGoalReached(double dist_tolerance, double angle_tolerance) override; 153 | 154 | bool cancel() override; 155 | }; 156 | }; 157 | #endif 158 | -------------------------------------------------------------------------------- /src/ftc_planner.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | #include "mbf_msgs/ExePathAction.h" 6 | 7 | PLUGINLIB_EXPORT_CLASS(ftc_local_planner::FTCPlanner, mbf_costmap_core::CostmapController) 8 | 9 | #define RET_SUCCESS 0 10 | #define RET_COLLISION 104 11 | #define RET_BLOCKED 109 12 | 13 | namespace ftc_local_planner 14 | { 15 | 16 | FTCPlanner::FTCPlanner() 17 | { 18 | } 19 | 20 | void FTCPlanner::initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros) 21 | { 22 | ros::NodeHandle private_nh("~/" + name); 23 | 24 | progress_server = private_nh.advertiseService( 25 | "planner_get_progress", &FTCPlanner::getProgress, this); 26 | 27 | global_point_pub = private_nh.advertise("global_point", 1); 28 | global_plan_pub = private_nh.advertise("global_plan", 1, true); 29 | obstacle_marker_pub = private_nh.advertise("costmap_marker", 10); 30 | 31 | costmap = costmap_ros; 32 | costmap_map_ = costmap->getCostmap(); 33 | tf_buffer = tf; 34 | 35 | // Parameter for dynamic reconfigure 36 | reconfig_server = new dynamic_reconfigure::Server(private_nh); 37 | dynamic_reconfigure::Server::CallbackType cb = boost::bind(&FTCPlanner::reconfigureCB, this, 38 | _1, _2); 39 | reconfig_server->setCallback(cb); 40 | 41 | current_state = PRE_ROTATE; 42 | 43 | // PID Debugging topic 44 | if (config.debug_pid) 45 | { 46 | pubPid = private_nh.advertise("debug_pid", 1, true); 47 | } 48 | 49 | // Recovery behavior initialization 50 | failure_detector_.setBufferLength(std::round(config.oscillation_recovery_min_duration * 10)); 51 | 52 | ROS_INFO("FTCLocalPlannerROS: Version 2 Init."); 53 | } 54 | 55 | void FTCPlanner::reconfigureCB(FTCPlannerConfig &c, uint32_t level) 56 | { 57 | if (c.restore_defaults) 58 | { 59 | reconfig_server->getConfigDefault(c); 60 | c.restore_defaults = false; 61 | } 62 | config = c; 63 | 64 | // just to be sure 65 | current_movement_speed = config.speed_slow; 66 | 67 | // set recovery behavior 68 | failure_detector_.setBufferLength(std::round(config.oscillation_recovery_min_duration * 10)); 69 | } 70 | 71 | bool FTCPlanner::setPlan(const std::vector &plan) 72 | { 73 | current_state = PRE_ROTATE; 74 | state_entered_time = ros::Time::now(); 75 | is_crashed = false; 76 | 77 | global_plan = plan; 78 | current_index = 0; 79 | current_progress = 0.0; 80 | 81 | last_time = ros::Time::now(); 82 | current_movement_speed = config.speed_slow; 83 | 84 | lat_error = 0.0; 85 | lon_error = 0.0; 86 | angle_error = 0.0; 87 | i_lon_error = 0.0; 88 | i_lat_error = 0.0; 89 | i_angle_error = 0.0; 90 | 91 | nav_msgs::Path path; 92 | 93 | if (global_plan.size() > 2) 94 | { 95 | // duplicate last point 96 | global_plan.push_back(global_plan.back()); 97 | // give second from last point last oriantation as the point before that 98 | global_plan[global_plan.size() - 2].pose.orientation = global_plan[global_plan.size() - 3].pose.orientation; 99 | path.header = plan.front().header; 100 | path.poses = plan; 101 | } 102 | else 103 | { 104 | ROS_WARN_STREAM("FTCLocalPlannerROS: Global plan was too short. Need a minimum of 3 poses - Cancelling."); 105 | current_state = FINISHED; 106 | state_entered_time = ros::Time::now(); 107 | } 108 | global_plan_pub.publish(path); 109 | 110 | ROS_INFO_STREAM("FTCLocalPlannerROS: Got new global plan with " << plan.size() << " points."); 111 | 112 | return true; 113 | } 114 | 115 | FTCPlanner::~FTCPlanner() 116 | { 117 | if (reconfig_server != nullptr) 118 | { 119 | delete reconfig_server; 120 | reconfig_server = nullptr; 121 | } 122 | } 123 | 124 | double FTCPlanner::distanceLookahead() 125 | { 126 | if (global_plan.size() < 2) 127 | { 128 | return 0; 129 | } 130 | Eigen::Quaternion current_rot(current_control_point.linear()); 131 | double lookahead_distance = 0.0; 132 | Eigen::Affine3d last_straight_point = current_control_point; 133 | Eigen::Affine3d current_point; 134 | for (uint32_t i = current_index + 1; i < global_plan.size(); i++) 135 | { 136 | tf2::fromMsg(global_plan[i].pose, current_point); 137 | 138 | // check, if direction is the same. if so, we add the distance 139 | Eigen::Quaternion rot2(current_point.linear()); 140 | 141 | if (lookahead_distance > config.speed_fast_threshold || 142 | abs(rot2.angularDistance(current_rot)) > config.speed_fast_threshold_angle * (M_PI / 180.0)) 143 | { 144 | break; 145 | } 146 | 147 | lookahead_distance += (current_point.translation() - last_straight_point.translation()).norm(); 148 | last_straight_point = current_point; 149 | 150 | } 151 | 152 | return lookahead_distance; 153 | } 154 | 155 | uint32_t FTCPlanner::computeVelocityCommands(const geometry_msgs::PoseStamped &pose, 156 | const geometry_msgs::TwistStamped &velocity, 157 | geometry_msgs::TwistStamped &cmd_vel, std::string &message) 158 | { 159 | 160 | ros::Time now = ros::Time::now(); 161 | double dt = now.toSec() - last_time.toSec(); 162 | last_time = now; 163 | 164 | if (is_crashed) 165 | { 166 | cmd_vel.twist.linear.x = 0; 167 | cmd_vel.twist.angular.z = 0; 168 | return RET_COLLISION; 169 | } 170 | 171 | if (current_state == FINISHED) 172 | { 173 | cmd_vel.twist.linear.x = 0; 174 | cmd_vel.twist.angular.z = 0; 175 | return RET_SUCCESS; 176 | } 177 | 178 | // We're not crashed and not finished. 179 | // First, we update the control point if needed. This is needed since we need the local_control_point to calculate the next state. 180 | update_control_point(dt); 181 | // Then, update the planner state. 182 | auto new_planner_state = update_planner_state(); 183 | if (new_planner_state != current_state) 184 | { 185 | ROS_INFO_STREAM("FTCLocalPlannerROS: Switching to state " << new_planner_state); 186 | state_entered_time = ros::Time::now(); 187 | current_state = new_planner_state; 188 | } 189 | 190 | if (checkCollision(config.obstacle_lookahead)) 191 | { 192 | cmd_vel.twist.linear.x = 0; 193 | cmd_vel.twist.angular.z = 0; 194 | is_crashed = true; 195 | return RET_BLOCKED; 196 | } 197 | 198 | // Finally, we calculate the velocity commands. 199 | calculate_velocity_commands(dt, cmd_vel); 200 | 201 | if (is_crashed) 202 | { 203 | cmd_vel.twist.linear.x = 0; 204 | cmd_vel.twist.angular.z = 0; 205 | return RET_COLLISION; 206 | } 207 | 208 | return RET_SUCCESS; 209 | } 210 | 211 | 212 | bool FTCPlanner::isGoalReached(double dist_tolerance, double angle_tolerance) 213 | { 214 | return current_state == FINISHED && !is_crashed; 215 | } 216 | 217 | bool FTCPlanner::cancel() 218 | { 219 | ROS_WARN_STREAM("FTCLocalPlannerROS: FTC planner was cancelled."); 220 | current_state = FINISHED; 221 | state_entered_time = ros::Time::now(); 222 | return true; 223 | } 224 | 225 | FTCPlanner::PlannerState FTCPlanner::update_planner_state() 226 | { 227 | switch (current_state) 228 | { 229 | case PRE_ROTATE: 230 | { 231 | if (time_in_current_state() > config.goal_timeout) 232 | { 233 | ROS_ERROR_STREAM("FTCLocalPlannerROS: Error reaching goal. config.goal_timeout (" << config.goal_timeout << ") reached - Timeout in PRE_ROTATE phase."); 234 | is_crashed = true; 235 | return FINISHED; 236 | } 237 | if (abs(angle_error) * (180.0 / M_PI) < config.max_goal_angle_error) 238 | { 239 | ROS_INFO_STREAM("FTCLocalPlannerROS: PRE_ROTATE finished. Starting following"); 240 | return FOLLOWING; 241 | } 242 | } 243 | break; 244 | case FOLLOWING: 245 | { 246 | double distance = local_control_point.translation().norm(); 247 | // check for crash 248 | if (distance > config.max_follow_distance) 249 | { 250 | ROS_ERROR_STREAM("FTCLocalPlannerROS: Robot is far away from global plan. distance (" << distance << ") > config.max_follow_distance (" << config.max_follow_distance << ") It probably has crashed."); 251 | is_crashed = true; 252 | return FINISHED; 253 | } 254 | 255 | // check if we're done following 256 | if (current_index == global_plan.size() - 2) 257 | { 258 | ROS_INFO_STREAM("FTCLocalPlannerROS: switching planner to position mode"); 259 | return WAITING_FOR_GOAL_APPROACH; 260 | } 261 | } 262 | break; 263 | case WAITING_FOR_GOAL_APPROACH: 264 | { 265 | double distance = local_control_point.translation().norm(); 266 | if (time_in_current_state() > config.goal_timeout) 267 | { 268 | ROS_WARN_STREAM("FTCLocalPlannerROS: Could not reach goal position. config.goal_timeout (" << config.goal_timeout << ") reached - Attempting final rotation anyways."); 269 | return POST_ROTATE; 270 | } 271 | if (distance < config.max_goal_distance_error) 272 | { 273 | ROS_INFO_STREAM("FTCLocalPlannerROS: Reached goal position."); 274 | return POST_ROTATE; 275 | } 276 | } 277 | break; 278 | case POST_ROTATE: 279 | { 280 | if (time_in_current_state() > config.goal_timeout) 281 | { 282 | ROS_WARN_STREAM("FTCLocalPlannerROS: Could not reach goal rotation. config.goal_timeout (" << config.goal_timeout << ") reached"); 283 | return FINISHED; 284 | } 285 | if (abs(angle_error) * (180.0 / M_PI) < config.max_goal_angle_error) 286 | { 287 | ROS_INFO_STREAM("FTCLocalPlannerROS: POST_ROTATE finished."); 288 | return FINISHED; 289 | } 290 | } 291 | break; 292 | case FINISHED: 293 | { 294 | // Nothing to do here 295 | } 296 | break; 297 | } 298 | 299 | return current_state; 300 | } 301 | 302 | void FTCPlanner::update_control_point(double dt) 303 | { 304 | 305 | switch (current_state) 306 | { 307 | case PRE_ROTATE: 308 | tf2::fromMsg(global_plan[0].pose, current_control_point); 309 | break; 310 | case FOLLOWING: 311 | { 312 | // Normal planner operation 313 | double straight_dist = distanceLookahead(); 314 | double speed; 315 | if (straight_dist >= config.speed_fast_threshold) 316 | { 317 | speed = config.speed_fast; 318 | } 319 | else 320 | { 321 | speed = config.speed_slow; 322 | } 323 | 324 | if (speed > current_movement_speed) 325 | { 326 | // accelerate 327 | current_movement_speed += dt * config.acceleration; 328 | if (current_movement_speed > speed) 329 | current_movement_speed = speed; 330 | } 331 | else if (speed < current_movement_speed) 332 | { 333 | // decelerate 334 | current_movement_speed -= dt * config.acceleration; 335 | if (current_movement_speed < speed) 336 | current_movement_speed = speed; 337 | } 338 | 339 | double distance_to_move = dt * current_movement_speed; 340 | double angle_to_move = dt * config.speed_angular * (M_PI / 180.0); 341 | 342 | Eigen::Affine3d nextPose, currentPose; 343 | while (angle_to_move > 0 && distance_to_move > 0 && current_index < global_plan.size() - 2) 344 | { 345 | 346 | tf2::fromMsg(global_plan[current_index].pose, currentPose); 347 | tf2::fromMsg(global_plan[current_index + 1].pose, nextPose); 348 | 349 | double pose_distance = (nextPose.translation() - currentPose.translation()).norm(); 350 | 351 | Eigen::Quaternion current_rot(currentPose.linear()); 352 | Eigen::Quaternion next_rot(nextPose.linear()); 353 | 354 | double pose_distance_angular = current_rot.angularDistance(next_rot); 355 | 356 | if (pose_distance <= 0.0) 357 | { 358 | ROS_WARN_STREAM("FTCLocalPlannerROS: Skipping duplicate point in global plan."); 359 | current_index++; 360 | continue; 361 | } 362 | 363 | double remaining_distance_to_next_pose = pose_distance * (1.0 - current_progress); 364 | double remaining_angular_distance_to_next_pose = pose_distance_angular * (1.0 - current_progress); 365 | 366 | if (remaining_distance_to_next_pose < distance_to_move && 367 | remaining_angular_distance_to_next_pose < angle_to_move) 368 | { 369 | // we need to move further than the remaining distance_to_move. Skip to the next point and decrease distance_to_move. 370 | current_progress = 0.0; 371 | current_index++; 372 | distance_to_move -= remaining_distance_to_next_pose; 373 | angle_to_move -= remaining_angular_distance_to_next_pose; 374 | } 375 | else 376 | { 377 | // we cannot reach the next point yet, so we update the percentage 378 | double current_progress_distance = 379 | (pose_distance * current_progress + distance_to_move) / pose_distance; 380 | double current_progress_angle = 381 | (pose_distance_angular * current_progress + angle_to_move) / pose_distance_angular; 382 | current_progress = fmin(current_progress_angle, current_progress_distance); 383 | if (current_progress > 1.0) 384 | { 385 | ROS_WARN_STREAM("FTCLocalPlannerROS: FTC PLANNER: Progress > 1.0"); 386 | // current_progress = 1.0; 387 | } 388 | distance_to_move = 0; 389 | angle_to_move = 0; 390 | } 391 | } 392 | 393 | tf2::fromMsg(global_plan[current_index].pose, currentPose); 394 | tf2::fromMsg(global_plan[current_index + 1].pose, nextPose); 395 | // interpolate between points 396 | Eigen::Quaternion rot1(currentPose.linear()); 397 | Eigen::Quaternion rot2(nextPose.linear()); 398 | 399 | Eigen::Vector3d trans1 = currentPose.translation(); 400 | Eigen::Vector3d trans2 = nextPose.translation(); 401 | 402 | Eigen::Affine3d result; 403 | result.translation() = (1.0 - current_progress) * trans1 + current_progress * trans2; 404 | result.linear() = rot1.slerp(current_progress, rot2).toRotationMatrix(); 405 | 406 | current_control_point = result; 407 | } 408 | break; 409 | case POST_ROTATE: 410 | tf2::fromMsg(global_plan[global_plan.size() - 1].pose, current_control_point); 411 | break; 412 | case WAITING_FOR_GOAL_APPROACH: 413 | break; 414 | case FINISHED: 415 | break; 416 | } 417 | 418 | { 419 | geometry_msgs::PoseStamped viz; 420 | viz.header = global_plan[current_index].header; 421 | viz.pose = tf2::toMsg(current_control_point); 422 | global_point_pub.publish(viz); 423 | } 424 | auto map_to_base = tf_buffer->lookupTransform("base_link", "map", ros::Time(), ros::Duration(1.0)); 425 | tf2::doTransform(current_control_point, local_control_point, map_to_base); 426 | 427 | lat_error = local_control_point.translation().y(); 428 | lon_error = local_control_point.translation().x(); 429 | angle_error = local_control_point.rotation().eulerAngles(0, 1, 2).z(); 430 | } 431 | 432 | void FTCPlanner::calculate_velocity_commands(double dt, geometry_msgs::TwistStamped &cmd_vel) 433 | { 434 | // check, if we're completely done 435 | if (current_state == FINISHED || is_crashed) 436 | { 437 | cmd_vel.twist.linear.x = 0; 438 | cmd_vel.twist.angular.z = 0; 439 | return; 440 | } 441 | 442 | i_lon_error += lon_error * dt; 443 | i_lat_error += lat_error * dt; 444 | i_angle_error += angle_error * dt; 445 | 446 | if (i_lon_error > config.ki_lon_max) 447 | { 448 | i_lon_error = config.ki_lon_max; 449 | } 450 | else if (i_lon_error < -config.ki_lon_max) 451 | { 452 | i_lon_error = -config.ki_lon_max; 453 | } 454 | if (i_lat_error > config.ki_lat_max) 455 | { 456 | i_lat_error = config.ki_lat_max; 457 | } 458 | else if (i_lat_error < -config.ki_lat_max) 459 | { 460 | i_lat_error = -config.ki_lat_max; 461 | } 462 | if (i_angle_error > config.ki_ang_max) 463 | { 464 | i_angle_error = config.ki_ang_max; 465 | } 466 | else if (i_angle_error < -config.ki_ang_max) 467 | { 468 | i_angle_error = -config.ki_ang_max; 469 | } 470 | 471 | double d_lat = (lat_error - last_lat_error) / dt; 472 | double d_lon = (lon_error - last_lon_error) / dt; 473 | double d_angle = (angle_error - last_angle_error) / dt; 474 | 475 | last_lat_error = lat_error; 476 | last_lon_error = lon_error; 477 | last_angle_error = angle_error; 478 | 479 | // allow linear movement only if in following state 480 | 481 | if (current_state == FOLLOWING) 482 | { 483 | double lin_speed = lon_error * config.kp_lon + i_lon_error * config.ki_lon + d_lon * config.kd_lon; 484 | if (lin_speed < 0 && config.forward_only) 485 | { 486 | lin_speed = 0; 487 | } 488 | else 489 | { 490 | if (lin_speed > config.max_cmd_vel_speed) 491 | { 492 | lin_speed = config.max_cmd_vel_speed; 493 | } 494 | else if (lin_speed < -config.max_cmd_vel_speed) 495 | { 496 | lin_speed = -config.max_cmd_vel_speed; 497 | } 498 | 499 | if (lin_speed < 0) 500 | { 501 | lat_error *= -1.0; 502 | } 503 | } 504 | cmd_vel.twist.linear.x = lin_speed; 505 | } 506 | else 507 | { 508 | cmd_vel.twist.linear.x = 0.0; 509 | } 510 | 511 | if (current_state == FOLLOWING) 512 | { 513 | 514 | double ang_speed = angle_error * config.kp_ang + i_angle_error * config.ki_ang + d_angle * config.kd_ang + 515 | lat_error * config.kp_lat + i_lat_error * config.ki_lat + d_lat * config.kd_lat; 516 | 517 | if (ang_speed > config.max_cmd_vel_ang) 518 | { 519 | ang_speed = config.max_cmd_vel_ang; 520 | } 521 | else if (ang_speed < -config.max_cmd_vel_ang) 522 | { 523 | ang_speed = -config.max_cmd_vel_ang; 524 | } 525 | 526 | cmd_vel.twist.angular.z = ang_speed; 527 | } 528 | else 529 | { 530 | double ang_speed = angle_error * config.kp_ang + i_angle_error * config.ki_ang + d_angle * config.kd_ang; 531 | if (ang_speed > config.max_cmd_vel_ang) 532 | { 533 | ang_speed = config.max_cmd_vel_ang; 534 | } 535 | else if (ang_speed < -config.max_cmd_vel_ang) 536 | { 537 | ang_speed = -config.max_cmd_vel_ang; 538 | } 539 | 540 | cmd_vel.twist.angular.z = ang_speed; 541 | 542 | // check if robot oscillates 543 | bool is_oscillating = checkOscillation(cmd_vel); 544 | if (is_oscillating) 545 | { 546 | ang_speed = config.max_cmd_vel_ang; 547 | cmd_vel.twist.angular.z = ang_speed; 548 | } 549 | } 550 | 551 | if (config.debug_pid) 552 | { 553 | ftc_local_planner::PID debugPidMsg; 554 | debugPidMsg.kp_lon_set = lon_error; 555 | 556 | // proportional 557 | debugPidMsg.kp_lat_set = lat_error * config.kp_lat; 558 | debugPidMsg.kp_lon_set = lon_error * config.kp_lon; 559 | debugPidMsg.kp_ang_set = angle_error * config.kp_ang; 560 | 561 | // integral 562 | debugPidMsg.ki_lat_set = i_lat_error * config.ki_lat; 563 | debugPidMsg.ki_lon_set = i_lon_error * config.ki_lon; 564 | debugPidMsg.ki_ang_set = i_angle_error * config.ki_ang; 565 | 566 | // diff 567 | debugPidMsg.kd_lat_set = d_lat * config.kd_lat; 568 | debugPidMsg.kd_lon_set = d_lon * config.kd_lon; 569 | debugPidMsg.kd_ang_set = d_angle * config.kd_ang; 570 | 571 | // errors 572 | debugPidMsg.lon_err = lon_error; 573 | debugPidMsg.lat_err = lat_error; 574 | debugPidMsg.ang_err = angle_error; 575 | 576 | // speeds 577 | debugPidMsg.ang_speed = cmd_vel.twist.angular.z; 578 | debugPidMsg.lin_speed = cmd_vel.twist.linear.x; 579 | 580 | pubPid.publish(debugPidMsg); 581 | } 582 | } 583 | 584 | bool FTCPlanner::getProgress(PlannerGetProgressRequest &req, PlannerGetProgressResponse &res) 585 | { 586 | res.index = current_index; 587 | return true; 588 | } 589 | 590 | bool FTCPlanner::checkCollision(int max_points) 591 | { 592 | unsigned int x; 593 | unsigned int y; 594 | 595 | std::vector footprint; 596 | visualization_msgs::Marker obstacle_marker; 597 | 598 | if (!config.check_obstacles) 599 | { 600 | return false; 601 | } 602 | // maximal costs 603 | unsigned char previous_cost = 255; 604 | // ensure look ahead not out of plan 605 | if (global_plan.size() < max_points) 606 | { 607 | max_points = global_plan.size(); 608 | } 609 | 610 | // calculate cost of footprint at robots actual pose 611 | if (config.obstacle_footprint) 612 | { 613 | costmap->getOrientedFootprint(footprint); 614 | for (int i = 0; i < footprint.size(); i++) 615 | { 616 | // check cost of each point of footprint 617 | if (costmap_map_->worldToMap(footprint[i].x, footprint[i].y, x, y)) 618 | { 619 | unsigned char costs = costmap_map_->getCost(x, y); 620 | if (costs >= costmap_2d::LETHAL_OBSTACLE) 621 | { 622 | ROS_WARN("FTCLocalPlannerROS: Possible collision of footprint at actual pose. Stop local planner."); 623 | return true; 624 | } 625 | } 626 | } 627 | } 628 | 629 | for (int i = 0; i < max_points; i++) 630 | { 631 | geometry_msgs::PoseStamped x_pose; 632 | int index = current_index + i; 633 | if (index > global_plan.size()) 634 | { 635 | index = global_plan.size(); 636 | } 637 | x_pose = global_plan[index]; 638 | 639 | if (costmap_map_->worldToMap(x_pose.pose.position.x, x_pose.pose.position.y, x, y)) 640 | { 641 | unsigned char costs = costmap_map_->getCost(x, y); 642 | if (config.debug_obstacle) 643 | { 644 | debugObstacle(obstacle_marker, x, y, costs, max_points); 645 | } 646 | // Near at obstacel 647 | if (costs > 0) 648 | { 649 | // Possible collision 650 | if (costs > 127 && costs > previous_cost) 651 | { 652 | ROS_WARN("FTCLocalPlannerROS: Possible collision. Stop local planner."); 653 | return true; 654 | } 655 | } 656 | previous_cost = costs; 657 | } 658 | } 659 | return false; 660 | } 661 | 662 | bool FTCPlanner::checkOscillation(geometry_msgs::TwistStamped &cmd_vel) 663 | { 664 | bool oscillating = false; 665 | // detect and resolve oscillations 666 | if (config.oscillation_recovery) 667 | { 668 | // oscillating = true; 669 | double max_vel_theta = config.max_cmd_vel_ang; 670 | double max_vel_current = config.max_cmd_vel_speed; 671 | 672 | failure_detector_.update(cmd_vel, config.max_cmd_vel_speed, config.max_cmd_vel_speed, max_vel_theta, 673 | config.oscillation_v_eps, config.oscillation_omega_eps); 674 | 675 | oscillating = failure_detector_.isOscillating(); 676 | 677 | if (oscillating) // we are currently oscillating 678 | { 679 | if (!oscillation_detected_) // do we already know that robot oscillates? 680 | { 681 | time_last_oscillation_ = ros::Time::now(); // save time when oscillation was detected 682 | oscillation_detected_ = true; 683 | } 684 | // calculate duration of actual oscillation 685 | bool oscillation_duration_timeout = !((ros::Time::now() - time_last_oscillation_).toSec() < config.oscillation_recovery_min_duration); // check how long we oscillate 686 | if (oscillation_duration_timeout) 687 | { 688 | if (!oscillation_warning_) // ensure to send warning just once instead of spamming around 689 | { 690 | ROS_WARN("FTCLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization)."); 691 | oscillation_warning_ = true; 692 | } 693 | return true; 694 | } 695 | return false; // oscillating but timeout not reached 696 | } 697 | else 698 | { 699 | // not oscillating 700 | time_last_oscillation_ = ros::Time::now(); // save time when oscillation was detected 701 | oscillation_detected_ = false; 702 | oscillation_warning_ = false; 703 | return false; 704 | } 705 | } 706 | return false; // no check for oscillation 707 | } 708 | 709 | void FTCPlanner::debugObstacle(visualization_msgs::Marker &obstacle_points, double x, double y, unsigned char cost, int maxIDs) 710 | { 711 | if (obstacle_points.points.empty()) 712 | { 713 | obstacle_points.header.frame_id = costmap->getGlobalFrameID(); 714 | obstacle_points.header.stamp = ros::Time::now(); 715 | obstacle_points.action = visualization_msgs::Marker::ADD; 716 | obstacle_points.pose.orientation.w = 1.0; 717 | obstacle_points.type = visualization_msgs::Marker::POINTS; 718 | obstacle_points.scale.x = 0.2; 719 | obstacle_points.scale.y = 0.2; 720 | } 721 | obstacle_points.id = obstacle_points.points.size() + 1; 722 | 723 | if (cost < 127) 724 | { 725 | obstacle_points.color.g = 1.0f; 726 | } 727 | 728 | if (cost >= 127 && cost < 255) 729 | { 730 | obstacle_points.color.r = 1.0f; 731 | } 732 | obstacle_points.color.a = 1.0; 733 | geometry_msgs::Point p; 734 | costmap_map_->mapToWorld(x, y, p.x, p.y); 735 | p.z = 0; 736 | 737 | obstacle_points.points.push_back(p); 738 | if (obstacle_points.points.size() >= maxIDs || cost > 0) 739 | { 740 | obstacle_marker_pub.publish(obstacle_points); 741 | obstacle_points.points.clear(); 742 | } 743 | } 744 | } 745 | --------------------------------------------------------------------------------