├── .github └── workflows │ └── industrial_ci_action.yml ├── README.md ├── graceful_controller ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── graceful_controller │ │ └── graceful_controller.hpp ├── package.xml └── src │ └── graceful_controller.cpp └── graceful_controller_ros ├── CHANGELOG.rst ├── CMakeLists.txt ├── include └── graceful_controller_ros │ ├── graceful_controller_ros.hpp │ ├── orientation_tools.hpp │ └── visualization.hpp ├── package.xml ├── plugins.xml ├── src ├── graceful_controller_ros.cpp ├── orientation_tools.cpp └── visualization.cpp └── test ├── config.yaml ├── graceful_controller_tests.cpp ├── graceful_controller_tests_launch.py └── orientation_tools_tests.cpp /.github/workflows/industrial_ci_action.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: [ ros2 ] 6 | pull_request: 7 | branches: [ ros2 ] 8 | 9 | jobs: 10 | industrial_ci: 11 | strategy: 12 | matrix: 13 | env: 14 | - {ROS_DISTRO: humble, ROS_REPO: main} 15 | runs-on: ubuntu-latest 16 | steps: 17 | - uses: actions/checkout@v1 18 | - uses: 'ros-industrial/industrial_ci@master' 19 | env: ${{matrix.env}} 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Graceful Controller 2 | 3 | This implement a controller based on "A Smooth Control Law for Graceful 4 | Motion of Differential Wheeled Mobile Robots in 2D Environments" by Park 5 | and Kuipers, ICRA 2011. 6 | 7 | Currently this does not implement section IV.B of the paper (which 8 | describes how to switch between poses along a path). Instead this controller 9 | has a naive approach which attempts to find the farthest pose in the path 10 | which is both A) less than some maximum lookahead distance away, and B) 11 | reachable (without collision) using our control law (as determined by 12 | a forward simulation). We call this the **target_pose**. 13 | 14 | There is a more detailed write up of the motiviation behind creating 15 | yet another local controller on 16 | [my blog](http://www.robotandchisel.com/2024/10/14/graceful-controller/) 17 | 18 | ## ROS2 Topics 19 | 20 | As with nearly all navigation local controllers, our controller outputs 21 | both a global path and a local path as a _nav_msgs::Path_. 22 | 23 | The "local path" is derived through a forward simulation of where 24 | the robot is expected to travel by iteratively applying the control law. 25 | Unlike the DWA controller, our forward simulation is NOT based on applying 26 | the same command throughout a fixed simulation time, but rather based 27 | on calling our control law multiple times as we forward simulate our 28 | path to the "target pose". As long as the acceleration limits are 29 | correctly specified, and the robot controllers can execute the velocities 30 | commanded, this will be the true path that the robot will follow. 31 | 32 | An additional topic that is unique to our controller is the "target_pose" 33 | topic which is a _geometry_msgs::PoseStamped_. This is published every 34 | time we compute the command velocity for the robot and tells you which 35 | pose in the global plan we are using for computing the control law. It 36 | is potentially helpful in debugging parameter tuning issues. 37 | 38 | ## Parameters 39 | 40 | The underlying control law has several parameters which are best described 41 | by the original paper, these include: 42 | 43 | * **k1** - controls convergence of control law (slow subsystem). A value of 0 reduces the controller to pure waypoint-following with no curvature. For a high k1 value, theta will be reduced faster than r. 44 | * **k2** - controls convergence of control law (fast subsystem). A higher value of k2 will reduce the distance of the path to the target, thus decreasing the path's curvature. 45 | * **lambda** - controls speed scaling based on curvature. A higher value of lambda results in more sharply peaked curves. 46 | * **beta** - controls speed scaling based on curvature. A higher value of beta lets the robot's velocity drop more quickly as K increases. K is the curvature of the path resulting from the control law (based on k1 and k2). 47 | 48 | Several parameters are used for selecting and simulating the target pose used 49 | to compute the control law: 50 | 51 | * **max_lookahead** - the target pose cannot be further than this distance 52 | away from the robot. A good starting value for this is usually about 1m. 53 | Using poses that are further away will generally result in smoother 54 | operations, but simulating poses that are very far away can result in 55 | reduced performance, especially in tight or cluttered environments. 56 | If the controller cannot forward simulate to a pose this far away without 57 | colliding, it will iteratively select a target pose that is closer to the 58 | robot. 59 | * **min_lookhead** - the target pose cannot be closer than this distance 60 | away from the robot. This parameter avoids instability when an unexpected 61 | obstacle appears in the path of the robot by returning failure, which 62 | typically triggers replanning. 63 | * **acc_dt** - this parameter is used to set the maximum velocity that the 64 | control law can use when generating velocities during the path simulation. 65 | 66 | There are several major "features" that are optional and configured through 67 | one or more parameters: 68 | 69 | * **initial_rotate_tolerance** - when the robot is pointed in a very 70 | different direction from the path, the control law (depending on k1 and k2) 71 | may generate large sweeping arcs. To avoid this potentially undesired behavior 72 | when the robot begins executing a new path, the controller can produce an 73 | initial in-place rotation to get the robot headed towards the path before it 74 | starts using the control law to follow it. The initial rotate tolerance is 75 | the maximum angular difference (in radians) between robot heading and the 76 | target pose. This feature can be disabled by setting _initial_rotate_tolerance_ 77 | to 0.0. 78 | * **prefer_final_rotation** - similar to the initial rotation issue many 79 | plans may cause large arcs at the end of a path because the orientation of 80 | the final pose is significantly different from the direction the robot would 81 | drive to follow the path as planned. By setting _prefer_final_rotation_ to 82 | true, the orientation of the final pose will be ignored and the robot will 83 | more closely follow the path as planned and then produce a final in-place 84 | rotation to align the robot heading with the goal. 85 | * **latch_xy_goal_tolerance** - similar to many other local controllers in ROS, 86 | this will prevent hunting around the goal by latching the XY portion of the 87 | goal when the robot is within the goal tolerance. The robot will then rotate 88 | to the final heading (while possibly being outside of the real XY goal tolerance). 89 | * **compute_orientations** - if the global planner does not compute orientations 90 | for the poses in the path, this should be set to true. 91 | * **use_orientation_filter** - since the controller is highly dependent on the 92 | angular heading of the target pose it is important the angular values are 93 | smooth. Some global planners may produce very unsmooth headings due to 94 | discretization errors. This optional filuter can be used to smooth 95 | out the orientations of the global path based on the **yaw_filter_tolerance** 96 | and **yaw_gap_tolerance**. 97 | * **yaw_filter_tolerance** - a higher value here allows a path to be more 98 | zig-zag, a lower filter value will filter out poses whose headings diverge 99 | from an overall "beeline" between the poses around it. units: radians 100 | * **yaw_gap_tolerance** - this is the maximum distance between poses, and 101 | so even if a pose exceeds the filter tolerance it will not be removed 102 | if the gap between the pose before and after it would exceed this value. 103 | units: meters. 104 | * **max_x_to_max_theta_scale_factor** - This limits the actual maximum angular 105 | velocity relative to the current maximum x velocity (which is possibly 106 | changing according to the max_vel_x ROS topic). At any moment in time, the 107 | maximum angular velocity is the minimum of the absolute max (max_vel_theta, 108 | see below) and this parameter multiplied by the current maximum x velocity. 109 | This allows enforcing slower rotation in circumstances with lower linear 110 | velocity limits. This parameter's default value is high, so whne not 111 | configured, angular velocity will be limited only by max_vel_theta. 112 | Units: rad/m, technically. 113 | 114 | Most of the above parameters can be left to their defaults and work well 115 | on a majority of robots. The following parameters, however, really do 116 | depend on the robot platform itself and must be set as accurately as 117 | possible: 118 | 119 | * **acc_lim_x** - this is likely the **most important** parameter to set 120 | properly. If the acceleration limits are overestimated the robot will not 121 | be able to follow the generated commands. During braking, this can cause 122 | the robot to crash into obstacles. Units: meters/sec^2. 123 | * **acc_lim_theta** - the same notes as for _acc_lim_x_, but in rad/sec^2. 124 | * **decel_lim_x** - (new in 0.4.3) this acceleration limit is used only 125 | within the underlying control law. It controls how quickly the robot slows 126 | as it approaches the goal (or an obstacle blocking the path, which causes 127 | the target_pose to be closer). If this is left as the default of 0.0, the 128 | controller will use the same acc_lim_x. Units: meters/sec^2. 129 | * **max_vel_x** - this is the maximum velocity the controller is allowed to 130 | specify. Units: meters/sec. 131 | * **min_vel_x** - when the commanded velocities have a high curvature, the 132 | robot is naturally slowed down (see the ICRA paper for details). The minimum 133 | velocity is the lowest velocity that will be generated when not executing 134 | an in-place rotation. Most robots have some amount of friction in their 135 | drivetrain and casters and cannot accurately move forward at speeds below 136 | some threshold, especially if also trying to turn slightly at the same time. 137 | Units: meters/sec. 138 | * **max_vel_theta** - the absolute maximum rotation velocity that will be 139 | produced by the control law. Units: rad/sec. 140 | * **min_in_place_vel_theta** - this is the minimum rotation velocity that 141 | will be used for in-place rotations. As with _min_vel_x_, this is largely 142 | hardware dependent since the robot has some minimum rotational velocity 143 | that can be reliably controlled. Units: rad/sec. 144 | * **xy_goal_tolerance** - this is the linear distance within which we consider 145 | the goal reached. When _prefer_final_rotation_ is set to true, we will only 146 | generate final rotations once this tolerance is met. Units: meters. 147 | * **yaw_goal_tolerance** - this is the angular distance within which we consider 148 | the goal reached. Units: radians. 149 | 150 | A final feature of the controller is footprint inflation at higher speeds. This 151 | helps avoid collisions by increasing the padding around the robot as the speed 152 | increases. This behavior is controlled by three parameters: 153 | 154 | * **scaling_vel_x** - above this speed, the footprint will be scaled up. 155 | units: meters/sec. 156 | * **scaling_factor** - this is how much the footprint will be scaled when 157 | we are moving at max_vel_x. The actual footprint size will be: 158 | 1.0 + scaling_factor * (vel - scaling_vel_x) / (max_vel_x - scaling_vel_x). 159 | By default, this is set to 0.0 and thus disabled. 160 | * **scaling_step** - this is how much we will drop the simulated velocity 161 | when retrying a particular target_pose. 162 | 163 | Example: our robot has a max velocity of 1.0 meters/second, **scaling_vel_x** 164 | of 0.5 meters/second, and a **scaling_factor** of 1.0. For a particular 165 | target_pose, if we are simulating the trajectory at 1.0 meters/second, then 166 | the footprint will be multiplied in size by a factor of 2.0. Suppose this 167 | triggers a collision in simulation of the path and that our **scaling_step** 168 | is 0.1 meters/second. The controller will re-simulate at 0.9 meters/second, 169 | and now our footprint will only be scaled by a factor of 1.8. If this were 170 | not collision free, the controller would re-simulate with a velocity of 171 | 0.8 meters/second and a scaling of 1.6. 172 | 173 | ## Example Config 174 | 175 | There is an example configuration for running this controller on the UBR1 176 | in ROS2 in the 177 | [ubr_reloaded](https://github.com/mikeferguson/ubr_reloaded/tree/ros2) 178 | repository. 179 | 180 | ## Licensing 181 | 182 | There are two ROS packages in this repo, with different licenses, since 183 | we're really trying to blend two license-incompatible code bases: 184 | 185 | * graceful_controller - is LGPL licensed and is the low-level control 186 | law implementation, as originally implemented in the 187 | [fetch_open_auto_dock](https://github.com/fetchrobotics/fetch_open_auto_dock) 188 | package. 189 | * graceful_controller_ros - is BSD licensed and is the actual ROS wrapper 190 | and glue logic to connect to the ROS Navigation stack. It leverages code 191 | from base_local_planner and dwa_local_planner packages in the 192 | [ROS Navigation](https://github.com/ros-planning/navigation) stack. 193 | -------------------------------------------------------------------------------- /graceful_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package graceful_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.2 (2022-03-29) 6 | ------------------ 7 | 8 | 0.4.1 (2022-03-11) 9 | ------------------ 10 | 11 | 0.4.0 (2021-06-14) 12 | ------------------ 13 | 14 | 0.3.1 (2021-05-27) 15 | ------------------ 16 | 17 | 0.3.0 (2021-01-14) 18 | ------------------ 19 | 20 | 0.2.2 (2021-01-13) 21 | ------------------ 22 | 23 | 0.2.1 (2021-01-11) 24 | ------------------ 25 | * update maintainer email 26 | * Contributors: Michael Ferguson 27 | 28 | 0.2.0 (2021-01-11) 29 | ------------------ 30 | * Initial release 31 | * Contributors: Michael Ferguson 32 | -------------------------------------------------------------------------------- /graceful_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(graceful_controller) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(angles REQUIRED) 20 | 21 | include_directories( 22 | include 23 | ) 24 | 25 | add_library(graceful_controller SHARED 26 | src/graceful_controller.cpp 27 | ) 28 | target_link_libraries(graceful_controller 29 | ${catkin_LIBRARIES} 30 | ) 31 | ament_target_dependencies(graceful_controller 32 | angles 33 | ) 34 | 35 | install( 36 | DIRECTORY include/ 37 | DESTINATION include 38 | ) 39 | 40 | install(DIRECTORY include/ 41 | DESTINATION include 42 | ) 43 | 44 | install(TARGETS graceful_controller 45 | ARCHIVE DESTINATION lib 46 | LIBRARY DESTINATION lib 47 | RUNTIME DESTINATION lib/graceful_controller 48 | ) 49 | 50 | ament_export_include_directories(include) 51 | ament_export_libraries(graceful_controller) 52 | ament_package() 53 | -------------------------------------------------------------------------------- /graceful_controller/include/graceful_controller/graceful_controller.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021-2022 Michael Ferguson 3 | * Copyright 2015 Fetch Robotics Inc 4 | * Author: Michael Ferguson 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU Lesser General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #ifndef GRACEFUL_CONTROLLER_HPP 21 | #define GRACEFUL_CONTROLLER_HPP 22 | 23 | #include 24 | #include 25 | 26 | namespace graceful_controller 27 | { 28 | 29 | class GracefulController 30 | { 31 | public: 32 | /** 33 | * @brief Constructor of the controller. 34 | * @param k1 Ratio of rate of change of theta to rate of change of R. 35 | * @param k2 How quickly we converge to the slow manifold. 36 | * @param min_abs_velocity The minimum absolute velocity in the linear direction. 37 | * @param max_abs_velocity The maximum absolute velocity in the linear direction. 38 | * @param max_decel The maximum deceleration in the linear direction. 39 | * @param max_abs_angular_velocity The maximum absolute velocity in the angular direction. 40 | */ 41 | GracefulController(double k1, 42 | double k2, 43 | double min_abs_velocity, 44 | double max_abs_velocity, 45 | double max_decel, 46 | double max_abs_angular_velocity, 47 | double beta, 48 | double lambda); 49 | 50 | /** 51 | * @brief Implements something loosely based on "A Smooth Control Law for 52 | * Graceful Motion of Differential Wheeled Mobile Robots in 2D Environments" 53 | * by Park and Kuipers, ICRA 2011 54 | * @param x The x coordinate of the goal, relative to robot base link. 55 | * @param y The y coordinate of the goal, relative to robot base link. 56 | * @param theta The angular orientation of the goal, relative to robot base link. 57 | * @param vel_x The computed command velocity in the linear direction. 58 | * @param vel_th The computed command velocity in the angular direction. 59 | * @param backward_motion Flag to indicate that the robot should move backward. False by default. 60 | * @returns true if there is a solution. 61 | */ 62 | bool approach(const double x, const double y, const double theta, 63 | double& vel_x, double& vel_th, bool backward_motion=false); 64 | 65 | /** 66 | * @brief Update the velocity limits. 67 | * @param min_abs_velocity The minimum absolute velocity in the linear direction. 68 | * @param max_abs_velocity The maximum absolute velocity in the linear direction. 69 | * @param max_abs_angular_velocity The maximum absolute velocity in the angular direction. 70 | */ 71 | void setVelocityLimits(const double min_abs_velocity, 72 | const double max_abs_velocity, 73 | const double max_abs_angular_velocity); 74 | 75 | private: 76 | /* 77 | * Parameters for approach controller 78 | */ 79 | double k1_; // ratio in change of theta to rate of change in r 80 | double k2_; // speed at which we converge to slow system 81 | double min_abs_velocity_; 82 | double max_abs_velocity_; 83 | double max_decel_; 84 | double max_abs_angular_velocity_; 85 | double beta_; // how fast velocity drops as k increases 86 | double lambda_; // controls speed scaling based on curvature. A higher value of lambda results in more sharply peaked curves 87 | double dist_; // used to create the tracking line 88 | }; 89 | 90 | using GracefulControllerPtr = std::shared_ptr; 91 | 92 | } // namespace graceful_controller 93 | 94 | #endif // GRACEFUL_CONTROLLER_HPP 95 | -------------------------------------------------------------------------------- /graceful_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | graceful_controller 4 | 0.4.2 5 | 6 | A controller. 7 | 8 | 9 | Michael Ferguson 10 | Michael Ferguson 11 | 12 | LGPLv3 13 | 14 | ament_cmake 15 | 16 | angles 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /graceful_controller/src/graceful_controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2021-2022 Michael Ferguson 3 | * Copyright 2015 Fetch Robotics Inc 4 | * Author: Michael Ferguson 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU Lesser General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public License 17 | * along with this program. If not, see . 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace graceful_controller 30 | { 31 | 32 | GracefulController::GracefulController(double k1, double k2, 33 | double min_abs_velocity, double max_abs_velocity, 34 | double max_decel, 35 | double max_abs_angular_velocity, 36 | double beta, double lambda) 37 | { 38 | k1_ = k1; 39 | k2_ = k2; 40 | min_abs_velocity_ = min_abs_velocity; 41 | max_abs_velocity_ = max_abs_velocity; 42 | max_decel_ = max_decel; 43 | max_abs_angular_velocity_ = max_abs_angular_velocity; 44 | beta_ = beta; 45 | lambda_ = lambda; 46 | } 47 | 48 | // x, y, theta are relative to base location and orientation 49 | bool GracefulController::approach(const double x, const double y, const double theta, 50 | double& vel_x, double& vel_th, bool backward_motion) 51 | { 52 | // Distance to goal 53 | double r = std::sqrt(x * x + y * y); 54 | 55 | // Orientation base frame relative to r_ 56 | double delta = (backward_motion) ? std::atan2(-y, -x) : std::atan2(-y, x); 57 | 58 | // Determine orientation of goal frame relative to r_ 59 | double theta2 = angles::normalize_angle(theta + delta); 60 | 61 | // Compute the virtual control 62 | double a = std::atan(-k1_ * theta2); 63 | // Compute curvature (k) 64 | double k = -1.0/r * (k2_ * (delta - a) + (1 + (k1_/(1+((k1_*theta2)*(k1_*theta2)))))*sin(delta)); 65 | 66 | // Compute max_velocity based on curvature 67 | double v = max_abs_velocity_ / (1 + beta_ * std::pow(fabs(k), lambda_)); 68 | // Limit velocity based on approaching target 69 | double approach_limit = std::sqrt(2 * max_decel_ * r); 70 | v = std::min(v, approach_limit); 71 | v = std::min(std::max(v, min_abs_velocity_), max_abs_velocity_); 72 | if (backward_motion) 73 | { 74 | v *= -1; // reverse linear velocity direction for backward motion 75 | } 76 | 77 | // Compute angular velocity 78 | double w = k * v; 79 | // Bound angular velocity 80 | double bounded_w = std::min(max_abs_angular_velocity_, std::max(-max_abs_angular_velocity_, w)); 81 | // Make sure that if we reduce w, we reduce v so that kurvature is still followed 82 | if (w != 0.0) 83 | { 84 | v *= (bounded_w/w); 85 | } 86 | 87 | // Send command to base 88 | vel_x = v; 89 | vel_th = bounded_w; 90 | return true; 91 | } 92 | 93 | void GracefulController::setVelocityLimits( 94 | const double min_abs_velocity, 95 | const double max_abs_velocity, 96 | const double max_abs_angular_velocity) 97 | { 98 | min_abs_velocity_ = min_abs_velocity; 99 | max_abs_velocity_ = max_abs_velocity; 100 | max_abs_angular_velocity_ = max_abs_angular_velocity; 101 | } 102 | 103 | } // namespace graceful_controller 104 | -------------------------------------------------------------------------------- /graceful_controller_ros/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package graceful_controller_ros 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.2 (2022-03-29) 6 | ------------------ 7 | * fix goal tolerance reset (`#29 `_) 8 | * add latch_xy_goal_tolerance parameter (`#28 `_) 9 | * fix orientation issue with collision checking (`#27 `_) 10 | * Contributors: Michael Ferguson 11 | 12 | 0.4.1 (2022-03-11) 13 | ------------------ 14 | * add ability to disable orientation filtering (`#26 `_) 15 | also make everything dynamic reconfigurable 16 | * don't crash when path is empty (`#25 `_) 17 | * add min_lookahead parameter (`#24 `_) 18 | when the pose is very close to the robot, we can get 19 | some instability (rapid side-to-side movement due to 20 | large angular errors over small linear distances). by 21 | adding this parameter, we can instead fallback to 22 | recovery behaviors and find a better path. 23 | * fix occasional boost::lock crash (`#23 `_) 24 | * limit the distance between poses as filter runs (`#21 `_) 25 | * improve the orientation filter, add tests (`#20 `_) 26 | * Contributors: Michael Ferguson 27 | 28 | 0.4.0 (2021-06-14) 29 | ------------------ 30 | * add usage documentation (`#19 `_) 31 | also remove unused min_vel_theta parameter 32 | * implement changes from review (`#18 `_) 33 | * Add collision checking during final rotation 34 | * Add comments about getRobotVel returning velocities 35 | * Rename path to simulated_path for clarity 36 | * Rename pose to target_pose and goal_pose (now separate variables) 37 | * Get rid of magic number for when to use final rotation 38 | * simulate intial rotation properly (`#17 `_) 39 | previously, this simulated the big arc that we wouldn't follow anyways. 40 | this could cause the robot to get stuck in corners or other tight areas. 41 | * Contributors: Michael Ferguson 42 | 43 | 0.3.1 (2021-05-27) 44 | ------------------ 45 | * add feature for final rotation (`#15 `_) 46 | if the final pose orientation is very different from the end 47 | of the path, we get a big sweeping turn. this feature uses 48 | the previous orientation to avoid that sweeping turn and 49 | instead does a final in-place rotation. Add filter and pose 50 | publisher. 51 | * not sure how build is working without this being executable 52 | * Contributors: Michael Ferguson 53 | 54 | 0.3.0 (2021-01-14) 55 | ------------------ 56 | * unitialized limits causes test failures (`#14 `_) 57 | limits does not initialize prune_plan to a value, 58 | causes flaky test when it ends up true (would also 59 | probably be bad on a real robot) 60 | * Contributors: Michael Ferguson 61 | 62 | 0.2.2 (2021-01-13) 63 | ------------------ 64 | * support robot footprint (`#13 `_) 65 | * cleanup parameters (`#12 `_) 66 | * drop unused parameters 67 | * manage parameters directly 68 | * Contributors: Michael Ferguson 69 | 70 | 0.2.1 (2021-01-11) 71 | ------------------ 72 | * update maintainer email 73 | * fix the buildfarm build (`#8 `_) 74 | * Contributors: Michael Ferguson 75 | 76 | 0.2.0 (2021-01-11) 77 | ------------------ 78 | * Initial release 79 | * Contributors: Michael Ferguson 80 | -------------------------------------------------------------------------------- /graceful_controller_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(graceful_controller_ros) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(angles REQUIRED) 20 | find_package(geometry_msgs REQUIRED) 21 | find_package(graceful_controller REQUIRED) 22 | find_package(nav_2d_utils REQUIRED) 23 | find_package(nav_msgs REQUIRED) 24 | find_package(nav2_core REQUIRED) 25 | find_package(nav2_costmap_2d REQUIRED) 26 | find_package(nav2_util REQUIRED) 27 | find_package(pluginlib REQUIRED) 28 | find_package(rclcpp REQUIRED) 29 | find_package(std_msgs REQUIRED) 30 | find_package(tf2_geometry_msgs REQUIRED) 31 | find_package(tf2_ros REQUIRED) 32 | find_package(visualization_msgs REQUIRED) 33 | 34 | include_directories(include) 35 | 36 | add_library(graceful_controller_ros SHARED 37 | src/graceful_controller_ros.cpp 38 | src/orientation_tools.cpp 39 | src/visualization.cpp 40 | ) 41 | ament_target_dependencies(graceful_controller_ros 42 | angles 43 | geometry_msgs 44 | graceful_controller 45 | nav_2d_utils 46 | nav_msgs 47 | nav2_core 48 | nav2_costmap_2d 49 | nav2_util 50 | pluginlib 51 | rclcpp 52 | std_msgs 53 | tf2_geometry_msgs 54 | tf2_ros 55 | visualization_msgs 56 | ) 57 | 58 | if(BUILD_TESTING) 59 | find_package(ament_cmake_gtest REQUIRED) 60 | 61 | ament_add_gtest(orientation_filter_tests 62 | src/orientation_tools.cpp 63 | test/orientation_tools_tests.cpp 64 | ) 65 | target_link_libraries(orientation_filter_tests 66 | graceful_controller_ros 67 | ) 68 | 69 | ament_add_gtest_executable(graceful_controller_tests 70 | test/graceful_controller_tests.cpp 71 | ) 72 | ament_target_dependencies(graceful_controller_tests 73 | angles 74 | geometry_msgs 75 | graceful_controller 76 | nav_2d_utils 77 | nav_msgs 78 | nav2_core 79 | nav2_costmap_2d 80 | pluginlib 81 | rclcpp 82 | std_msgs 83 | tf2_geometry_msgs 84 | tf2_ros 85 | visualization_msgs 86 | ) 87 | target_link_libraries(graceful_controller_tests 88 | graceful_controller_ros 89 | ) 90 | 91 | ament_add_test(test_graceful_controller 92 | GENERATE_RESULT_FOR_RETURN_CODE_ZERO 93 | COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/graceful_controller_tests_launch.py" 94 | WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" 95 | ENV 96 | CONFIG_DIR=${CMAKE_CURRENT_SOURCE_DIR}/test 97 | TEST_EXECUTABLE=$ 98 | ) 99 | endif() 100 | 101 | install(TARGETS graceful_controller_ros 102 | ARCHIVE DESTINATION lib 103 | LIBRARY DESTINATION lib 104 | RUNTIME DESTINATION lib/graceful_controller_ros 105 | ) 106 | 107 | ament_export_include_directories(include) 108 | ament_export_libraries(graceful_controller_ros) 109 | ament_export_dependencies( 110 | geometry_msgs 111 | graceful_controller 112 | nav_2d_utils 113 | nav_msgs 114 | nav2_core 115 | nav2_costmap_2d 116 | nav2_util 117 | pluginlib 118 | rclcpp 119 | std_msgs 120 | tf2_geometry_msgs 121 | tf2_ros 122 | visualization_msgs 123 | ) 124 | pluginlib_export_plugin_description_file(nav2_core plugins.xml) 125 | ament_package() 126 | -------------------------------------------------------------------------------- /graceful_controller_ros/include/graceful_controller_ros/graceful_controller_ros.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2021-2023, Michael Ferguson 6 | * Copyright (c) 2009, Willow Garage, Inc. 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 Willow Garage, Inc. 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: Eitan Marder-Eppstein, Michael Ferguson 37 | *********************************************************************/ 38 | 39 | #ifndef GRACEFUL_CONTROLLER_ROS_GRACEFUL_CONTROLLER_ROS_HPP 40 | #define GRACEFUL_CONTROLLER_ROS_GRACEFUL_CONTROLLER_ROS_HPP 41 | 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include "graceful_controller_ros/orientation_tools.hpp" 53 | #include "graceful_controller_ros/visualization.hpp" 54 | 55 | namespace graceful_controller 56 | { 57 | class GracefulControllerROS : public nav2_core::Controller 58 | { 59 | public: 60 | GracefulControllerROS(); 61 | virtual ~GracefulControllerROS(); 62 | 63 | /** 64 | * @brief Constructs the local planner 65 | * @param node WeakPtr to the Lifecycle node 66 | * @param name The name to give this instance of the local planner 67 | * @param tf A pointer to a transform buffer 68 | * @param costmap_ros The cost map to use for assigning costs to local plans 69 | */ 70 | virtual void configure( 71 | const rclcpp_lifecycle::LifecycleNode::WeakPtr& node, 72 | std::string name, std::shared_ptr tf, 73 | std::shared_ptr costmap_ros); 74 | 75 | /** 76 | * @brief Method to cleanup resources. 77 | */ 78 | virtual void cleanup(); 79 | 80 | /** 81 | * @brief Method to active planner and any threads involved in execution. 82 | */ 83 | virtual void activate(); 84 | 85 | /** 86 | * @brief Method to deactive planner and any threads involved in execution. 87 | */ 88 | virtual void deactivate(); 89 | 90 | /** 91 | * @brief Calculates the best command given the current pose and velocity 92 | * 93 | * It is presumed that the global plan is already set. 94 | * @param pose Current robot pose 95 | * @param velocity Current robot velocity 96 | * @param goal_checker Pointer to the current goal checker the task is utilizing 97 | * @return The best command for the robot to drive 98 | */ 99 | virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( 100 | const geometry_msgs::msg::PoseStamped& robot_pose, 101 | const geometry_msgs::msg::Twist& cmd_vel, 102 | nav2_core::GoalChecker * goal_checker); 103 | 104 | /** 105 | * @brief Set the plan that the local planner is following 106 | * @param plan The plan to pass to the local planner 107 | */ 108 | virtual void setPlan(const nav_msgs::msg::Path & path); 109 | 110 | /** 111 | * @brief Rotate the robot towards a goal. 112 | * @param pose The pose should always be in base frame! 113 | * @param velocity Current robot velocity. 114 | * @param cmd_vel The returned command velocity. 115 | * @returns The computed angular error. 116 | */ 117 | double rotateTowards(const geometry_msgs::msg::PoseStamped& pose, 118 | const geometry_msgs::msg::Twist& velocity, 119 | geometry_msgs::msg::TwistStamped& cmd_vel); 120 | 121 | /** 122 | * @brief Rotate the robot towards an angle. 123 | * @param yaw The angle to rotate. 124 | * @param velocity Current robot velocity. 125 | * @param cmd_vel The returned command velocity. 126 | */ 127 | void rotateTowards(double yaw, 128 | const geometry_msgs::msg::Twist& velocity, 129 | geometry_msgs::msg::TwistStamped& cmd_vel); 130 | 131 | /** 132 | * @brief Limits the maximum linear speed of the robot. 133 | * @param speed_limit expressed in absolute value (in m/s) 134 | * or in percentage from maximum robot speed. 135 | * @param percentage Setting speed limit in percentage if true 136 | * or in absolute values in false case. 137 | */ 138 | virtual void setSpeedLimit(const double& speed_limit, const bool& percentage); 139 | 140 | private: 141 | /** 142 | * @brief Simulate a path. 143 | * @param target_pose Pose to simulate towards. 144 | * @param velocity Current robot velocity. 145 | * @param cmd_vel The returned command to execute. 146 | * @returns True if the path is valid. 147 | */ 148 | bool simulate( 149 | const geometry_msgs::msg::PoseStamped& target_pose, 150 | const geometry_msgs::msg::Twist& velocity, 151 | geometry_msgs::msg::TwistStamped& cmd_vel); 152 | 153 | rclcpp_lifecycle::LifecycleNode::WeakPtr node_; 154 | rclcpp::Clock::SharedPtr clock_; 155 | std::string name_; 156 | 157 | std::shared_ptr> global_plan_pub_; 158 | std::shared_ptr> local_plan_pub_; 159 | std::shared_ptr> target_pose_pub_; 160 | std::shared_ptr> collision_points_pub_; 161 | 162 | bool initialized_; 163 | GracefulControllerPtr controller_; 164 | 165 | std::shared_ptr buffer_; 166 | std::shared_ptr costmap_ros_; 167 | geometry_msgs::msg::TransformStamped robot_to_costmap_transform_; 168 | nav_msgs::msg::Path global_plan_; 169 | 170 | // Parameters 171 | std::mutex config_mutex_; 172 | double max_vel_x_; 173 | double max_vel_x_limited_; 174 | double min_vel_x_; 175 | double max_vel_theta_; 176 | double max_vel_theta_limited_; 177 | double max_x_to_max_theta_scale_factor_; 178 | double min_in_place_vel_theta_; 179 | double acc_lim_x_; 180 | double acc_lim_theta_; 181 | double decel_lim_x_; 182 | double scaling_vel_x_; 183 | double scaling_factor_; 184 | double scaling_step_; 185 | double min_lookahead_; 186 | double max_lookahead_; 187 | double resolution_; 188 | double acc_dt_; 189 | double yaw_filter_tolerance_; 190 | double yaw_gap_tolerance_; 191 | bool prefer_final_rotation_; 192 | bool compute_orientations_; 193 | bool use_orientation_filter_; 194 | 195 | // Goal tolerance 196 | bool latch_xy_goal_tolerance_; 197 | bool goal_tolerance_met_; 198 | 199 | // Controls initial rotation towards path 200 | double initial_rotate_tolerance_; 201 | bool has_new_path_; 202 | 203 | // Optional visualization of colliding and non-colliding points checked 204 | visualization_msgs::msg::MarkerArray* collision_points_; 205 | 206 | geometry_msgs::msg::PoseStamped robot_pose_; 207 | }; 208 | 209 | /** 210 | * @brief Compute distance of poses along a path. Assumes poses are in robot-centric frame. 211 | * @param poses The poses that form the path. 212 | * @param distances Computed distance for each pose from the robot, along the path. 213 | * Returned by reference. 214 | */ 215 | void computeDistanceAlongPath(const std::vector& poses, 216 | std::vector& distances); 217 | 218 | } // namespace graceful_controller 219 | 220 | #endif // GRACEFUL_CONTROLLER_ROS_GRACEFUL_CONTROLLER_ROS_HPP 221 | -------------------------------------------------------------------------------- /graceful_controller_ros/include/graceful_controller_ros/orientation_tools.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2021-2022, Michael Ferguson 6 | * Copyright (c) 2009, Willow Garage, Inc. 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 Willow Garage, Inc. 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: Michael Ferguson 37 | *********************************************************************/ 38 | 39 | #ifndef GRACEFUL_CONTROLLER_ROS_ORIENTATION_TOOLS_HPP 40 | #define GRACEFUL_CONTROLLER_ROS_ORIENTATION_TOOLS_HPP 41 | 42 | #include 43 | #include 44 | 45 | namespace graceful_controller 46 | { 47 | 48 | /** 49 | * @brief Add orientation to each pose in a path. 50 | * @param path The path to have orientations added. 51 | * @returns The oriented path. 52 | */ 53 | nav_msgs::msg::Path addOrientations(const nav_msgs::msg::Path& path); 54 | 55 | /** 56 | * @brief Filter a path for orientation noise. 57 | * @param path The path to be filtered. 58 | * @param yaw_tolerance Maximum deviation allowed before a pose is filtered. 59 | * @param gap_tolerance Maximum distance between poses in the filtered path. 60 | * @returns The filtered path. 61 | */ 62 | nav_msgs::msg::Path applyOrientationFilter(const nav_msgs::msg::Path& path, 63 | double yaw_tolerance, 64 | double gap_tolerance); 65 | 66 | } // namespace graceful_controller 67 | 68 | #endif // GRACEFUL_CONTROLLER_ROS_ORIENTATION_TOOLS_HPP 69 | -------------------------------------------------------------------------------- /graceful_controller_ros/include/graceful_controller_ros/visualization.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, Michael Ferguson 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #ifndef GRACEFUL_CONTROLLER_ROS_VISUALIZATION_HPP 37 | #define GRACEFUL_CONTROLLER_ROS_VISUALIZATION_HPP 38 | 39 | #include 40 | 41 | /** 42 | * @brief Add a visualization point to a marker. 43 | */ 44 | void addPointMarker(double x, double y, bool colliding, 45 | visualization_msgs::msg::MarkerArray* msg); 46 | 47 | #endif // GRACEFUL_CONTROLLER_ROS_VISUALIZATION_HPP 48 | -------------------------------------------------------------------------------- /graceful_controller_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | graceful_controller_ros 4 | 0.4.2 5 | 6 | A controller. Some say it might be graceful. 7 | 8 | 9 | Michael Ferguson 10 | Michael Ferguson 11 | 12 | BSD 13 | 14 | ament_cmake 15 | 16 | angles 17 | geometry_msgs 18 | graceful_controller 19 | nav_2d_utils 20 | nav_msgs 21 | nav2_core 22 | nav2_costmap_2d 23 | nav2_util 24 | pluginlib 25 | rclcpp 26 | std_msgs 27 | tf2_geometry_msgs 28 | tf2_ros 29 | visualization_msgs 30 | 31 | ament_cmake_gtest 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /graceful_controller_ros/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a local planner based on "A Smooth Control Law for Graceful Motion of Differential Wheeled Mobile Robots in 2D Environments" by Park and Kuipers, ICRA 2011 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /graceful_controller_ros/src/graceful_controller_ros.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2021-2023, Michael Ferguson 6 | * Copyright (c) 2009, Willow Garage, Inc. 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 Willow Garage, Inc. 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: Eitan Marder-Eppstein, Michael Ferguson 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include "graceful_controller_ros/graceful_controller_ros.hpp" 48 | 49 | using rclcpp_lifecycle::LifecyclePublisher; 50 | using nav2_util::declare_parameter_if_not_declared; 51 | 52 | namespace graceful_controller 53 | { 54 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("graceful_controller"); 55 | 56 | double sign(double x) 57 | { 58 | return x < 0.0 ? -1.0 : 1.0; 59 | } 60 | 61 | /** 62 | * @brief Collision check the robot pose 63 | * @param x The robot x coordinate in costmap.global frame 64 | * @param y The robot y coordinate in costmap.global frame 65 | * @param theta The robot rotation in costmap.global frame 66 | * @param viz Optional message for visualizing collisions 67 | * @param inflation Ratio to expand the footprint 68 | */ 69 | bool isColliding(double x, double y, double theta, 70 | std::shared_ptr costmap, 71 | visualization_msgs::msg::MarkerArray* viz, double inflation = 1.0) 72 | { 73 | unsigned mx, my; 74 | if (!costmap->getCostmap()->worldToMap(x, y, mx, my)) 75 | { 76 | RCLCPP_DEBUG(LOGGER, "Path is off costmap (%f,%f)", x, y); 77 | addPointMarker(x, y, true, viz); 78 | return true; 79 | } 80 | 81 | if (inflation < 1.0) 82 | { 83 | RCLCPP_WARN(LOGGER, "Inflation ratio cannot be less than 1.0"); 84 | inflation = 1.0; 85 | } 86 | 87 | // Get footprint (centered around robot) 88 | std::vector spec = costmap->getRobotFootprint(); 89 | 90 | // Expand footprint by desired infation 91 | for (size_t i = 0; i < spec.size(); ++i) 92 | { 93 | spec[i].x *= inflation; 94 | spec[i].y *= inflation; 95 | } 96 | 97 | // Transform footprint to robot pose 98 | std::vector footprint; 99 | nav2_costmap_2d::transformFootprint(x, y, theta, spec, footprint); 100 | 101 | // If our footprint is less than 4 corners, treat as circle 102 | if (footprint.size() < 4) 103 | { 104 | if (costmap->getCostmap()->getCost(mx, my) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) 105 | { 106 | RCLCPP_DEBUG(LOGGER, "Collision along path at (%f,%f)", x, y); 107 | addPointMarker(x, y, true, viz); 108 | return true; 109 | } 110 | // Done collison checking 111 | return false; 112 | } 113 | 114 | // Do a complete collision check of the footprint boundary 115 | for (size_t i = 0; i < footprint.size(); ++i) 116 | { 117 | unsigned x0, y0, x1, y1; 118 | if (!costmap->getCostmap()->worldToMap(footprint[i].x, footprint[i].y, x0, y0)) 119 | { 120 | RCLCPP_DEBUG(LOGGER, "Footprint point %lu is off costmap", i); 121 | addPointMarker(footprint[i].x, footprint[i].y, true, viz); 122 | return true; 123 | } 124 | addPointMarker(footprint[i].x, footprint[i].y, false, viz); 125 | 126 | size_t next = (i + 1) % footprint.size(); 127 | if (!costmap->getCostmap()->worldToMap(footprint[next].x, footprint[next].y, x1, y1)) 128 | { 129 | RCLCPP_DEBUG(LOGGER, "Footprint point %lu is off costmap", next); 130 | addPointMarker(footprint[next].x, footprint[next].y, true, viz); 131 | return true; 132 | } 133 | addPointMarker(footprint[next].x, footprint[next].y, false, viz); 134 | 135 | for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) 136 | { 137 | if (costmap->getCostmap()->getCost(line.getX(), line.getY()) >= nav2_costmap_2d::LETHAL_OBSTACLE) 138 | { 139 | RCLCPP_DEBUG(LOGGER, "Collision along path at (%f,%f)", x, y); 140 | return true; 141 | } 142 | } 143 | } 144 | 145 | // Not colliding 146 | return false; 147 | } 148 | 149 | GracefulControllerROS::GracefulControllerROS() : initialized_(false), has_new_path_(false), collision_points_(NULL) 150 | { 151 | } 152 | 153 | GracefulControllerROS::~GracefulControllerROS() 154 | { 155 | if (collision_points_) 156 | { 157 | delete collision_points_; 158 | } 159 | } 160 | 161 | void GracefulControllerROS::configure( 162 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & weak_node, 163 | std::string name, std::shared_ptr tf, 164 | std::shared_ptr costmap_ros) 165 | { 166 | if (initialized_) 167 | { 168 | RCLCPP_ERROR(LOGGER, "This planner has already been initialized, doing nothing."); 169 | return; 170 | } 171 | 172 | // Save important things 173 | node_ = weak_node; 174 | buffer_ = tf; 175 | costmap_ros_ = costmap_ros; 176 | name_ = name; 177 | 178 | auto node = node_.lock(); 179 | if (!node) 180 | { 181 | throw std::runtime_error{"Failed to lock node"}; 182 | } 183 | 184 | clock_ = node->get_clock(); 185 | 186 | // Setup parameters 187 | declare_parameter_if_not_declared(node, name_ + ".max_vel_x", rclcpp::ParameterValue(0.5)); 188 | declare_parameter_if_not_declared(node, name_ + ".min_vel_x", rclcpp::ParameterValue(0.1)); 189 | declare_parameter_if_not_declared(node, name_ + ".max_vel_theta", rclcpp::ParameterValue(1.0)); 190 | declare_parameter_if_not_declared(node, name_ + ".min_in_place_vel_theta", rclcpp::ParameterValue(0.4)); 191 | declare_parameter_if_not_declared(node, name_ + ".min_x_to_max_theta_scale_factor", rclcpp::ParameterValue(100.0)); 192 | declare_parameter_if_not_declared(node, name_ + ".acc_lim_x", rclcpp::ParameterValue(2.5)); 193 | declare_parameter_if_not_declared(node, name_ + ".acc_lim_theta", rclcpp::ParameterValue(3.2)); 194 | declare_parameter_if_not_declared(node, name_ + ".acc_dt", rclcpp::ParameterValue(0.25)); 195 | declare_parameter_if_not_declared(node, name_ + ".decel_lim_x", rclcpp::ParameterValue(0.0)); 196 | declare_parameter_if_not_declared(node, name_ + ".max_lookahead", rclcpp::ParameterValue(1.0)); 197 | declare_parameter_if_not_declared(node, name_ + ".min_lookahead", rclcpp::ParameterValue(0.25)); 198 | declare_parameter_if_not_declared(node, name_ + ".initial_rotate_tolerance", rclcpp::ParameterValue(0.1)); 199 | declare_parameter_if_not_declared(node, name_ + ".prefer_final_rotation", rclcpp::ParameterValue(false)); 200 | declare_parameter_if_not_declared(node, name_ + ".compute_orientations", rclcpp::ParameterValue(true)); 201 | declare_parameter_if_not_declared(node, name_ + ".use_orientation_filter", rclcpp::ParameterValue(false)); 202 | declare_parameter_if_not_declared(node, name_ + ".yaw_filter_tolerance", rclcpp::ParameterValue(0.785)); 203 | declare_parameter_if_not_declared(node, name_ + ".yaw_gap_tolerance", rclcpp::ParameterValue(0.25)); 204 | declare_parameter_if_not_declared(node, name_ + ".latch_xy_goal_tolerance", rclcpp::ParameterValue(false)); 205 | declare_parameter_if_not_declared(node, name_ + ".publish_collision_points", rclcpp::ParameterValue(false)); 206 | declare_parameter_if_not_declared(node, name_ + ".k1", rclcpp::ParameterValue(2.0)); 207 | declare_parameter_if_not_declared(node, name_ + ".k2", rclcpp::ParameterValue(1.0)); 208 | declare_parameter_if_not_declared(node, name_ + ".beta", rclcpp::ParameterValue(0.4)); 209 | declare_parameter_if_not_declared(node, name_ + ".lambda", rclcpp::ParameterValue(2.0)); 210 | declare_parameter_if_not_declared(node, name_ + ".scaling_vel_x", rclcpp::ParameterValue(0.5)); 211 | declare_parameter_if_not_declared(node, name_ + ".scaling_factor", rclcpp::ParameterValue(0.0)); 212 | declare_parameter_if_not_declared(node, name_ + ".scaling_step", rclcpp::ParameterValue(0.1)); 213 | 214 | node->get_parameter(name_ + ".max_vel_x", max_vel_x_); 215 | node->get_parameter(name_ + ".min_vel_x", min_vel_x_); 216 | node->get_parameter(name_ + ".max_vel_theta", max_vel_theta_); 217 | node->get_parameter(name_ + ".min_in_place_vel_theta", min_in_place_vel_theta_); 218 | node->get_parameter(name_ + ".max_x_to_max_theta_scale_factor", max_x_to_max_theta_scale_factor_); 219 | node->get_parameter(name_ + ".acc_lim_x", acc_lim_x_); 220 | node->get_parameter(name_ + ".acc_lim_theta", acc_lim_theta_); 221 | node->get_parameter(name_ + ".acc_dt", acc_dt_); 222 | node->get_parameter(name_ + ".decel_lim_x", decel_lim_x_); 223 | node->get_parameter(name_ + ".max_lookahead", max_lookahead_); 224 | node->get_parameter(name_ + ".min_lookahead", min_lookahead_); 225 | node->get_parameter(name_ + ".initial_rotate_tolerance", initial_rotate_tolerance_); 226 | node->get_parameter(name_ + ".prefer_final_rotation", prefer_final_rotation_); 227 | node->get_parameter(name_ + ".compute_orientations", compute_orientations_); 228 | node->get_parameter(name_ + ".use_orientation_filter", use_orientation_filter_); 229 | node->get_parameter(name_ + ".yaw_filter_tolerance", yaw_filter_tolerance_); 230 | node->get_parameter(name_ + ".yaw_gap_tolerance", yaw_gap_tolerance_); 231 | node->get_parameter(name_ + ".latch_xy_goal_tolerance", latch_xy_goal_tolerance_); 232 | node->get_parameter(name_ + ".scaling_vel_x_", scaling_vel_x_); 233 | node->get_parameter(name_ + ".scaling_factor", scaling_factor_); 234 | node->get_parameter(name_ + ".scaling_step", scaling_step_); 235 | resolution_ = costmap_ros_->getCostmap()->getResolution(); 236 | double k1, k2, beta, lambda; 237 | node->get_parameter(name_ + ".k1", k1); 238 | node->get_parameter(name_ + ".k2", k2); 239 | node->get_parameter(name_ + ".beta", beta); 240 | node->get_parameter(name_ + ".lambda", lambda); 241 | 242 | // Set initial velocity limit 243 | max_vel_x_limited_ = max_vel_x_; 244 | 245 | if (max_x_to_max_theta_scale_factor_ < 0.001) 246 | { 247 | // If max_x_to_max_theta_scale_factor not specified, use a high value so it has no functional impact 248 | max_x_to_max_theta_scale_factor_ = 100.0; 249 | } 250 | 251 | // Limit maximum angular velocity proportional to maximum linear velocity 252 | max_vel_theta_limited_ = max_vel_x_limited_ * max_x_to_max_theta_scale_factor_; 253 | max_vel_theta_limited_ = std::min(max_vel_theta_limited_, max_vel_theta_); 254 | 255 | // Publishers (same topics as DWA/TrajRollout) 256 | global_plan_pub_ = node->create_publisher(name_ + "/global_plan", 1); 257 | local_plan_pub_ = node->create_publisher(name_ + "/local_plan", 1); 258 | target_pose_pub_ = node->create_publisher(name_ + "/target_pose", 1); 259 | 260 | bool publish_collision_points; 261 | node->get_parameter(name_ + ".publish_collision_points", publish_collision_points); 262 | if (publish_collision_points) 263 | { 264 | // Create publisher 265 | collision_points_pub_ = node->create_publisher(name_ + "/collision_points", 1); 266 | 267 | // Create message to publish 268 | collision_points_ = new visualization_msgs::msg::MarkerArray(); 269 | } 270 | 271 | if (decel_lim_x_ < 0.001) 272 | { 273 | // If decel limit not specified, use accel_limit 274 | decel_lim_x_ = acc_lim_x_; 275 | } 276 | 277 | controller_ = std::make_shared(k1, 278 | k2, 279 | min_vel_x_, 280 | max_vel_x_, 281 | decel_lim_x_, 282 | max_vel_theta_, 283 | beta, 284 | lambda); 285 | 286 | initialized_ = true; 287 | } 288 | 289 | void GracefulControllerROS::cleanup() 290 | { 291 | global_plan_pub_.reset(); 292 | local_plan_pub_.reset(); 293 | target_pose_pub_.reset(); 294 | collision_points_pub_.reset(); 295 | } 296 | 297 | void GracefulControllerROS::activate() 298 | { 299 | global_plan_pub_->on_activate(); 300 | local_plan_pub_->on_activate(); 301 | target_pose_pub_->on_activate(); 302 | if (collision_points_) 303 | { 304 | collision_points_pub_->on_activate(); 305 | } 306 | has_new_path_ = false; 307 | } 308 | 309 | void GracefulControllerROS::deactivate() 310 | { 311 | global_plan_pub_->on_deactivate(); 312 | local_plan_pub_->on_deactivate(); 313 | target_pose_pub_->on_deactivate(); 314 | if (collision_points_) 315 | { 316 | collision_points_pub_->on_deactivate(); 317 | } 318 | } 319 | 320 | geometry_msgs::msg::TwistStamped GracefulControllerROS::computeVelocityCommands( 321 | const geometry_msgs::msg::PoseStamped& robot_pose, 322 | const geometry_msgs::msg::Twist& velocity, 323 | nav2_core::GoalChecker * goal_checker) 324 | { 325 | geometry_msgs::msg::TwistStamped cmd_vel; 326 | 327 | if (!initialized_) 328 | { 329 | RCLCPP_ERROR(LOGGER, "Controller is not initialized, call configure() before using this planner"); 330 | return cmd_vel; 331 | } 332 | 333 | // Do this here to avoid segfault if not initialized 334 | cmd_vel.header.frame_id = robot_pose.header.frame_id; 335 | cmd_vel.header.stamp = clock_->now(); 336 | 337 | // Lock the mutex 338 | std::lock_guard lock(config_mutex_); 339 | 340 | // Publish the global plan 341 | global_plan_pub_->publish(global_plan_); 342 | 343 | // Get transforms 344 | geometry_msgs::msg::TransformStamped plan_to_robot; 345 | try 346 | { 347 | plan_to_robot = buffer_->lookupTransform(costmap_ros_->getBaseFrameID(), 348 | global_plan_.header.frame_id, 349 | tf2::TimePointZero); 350 | } 351 | catch (tf2::TransformException& ex) 352 | { 353 | RCLCPP_ERROR(LOGGER, "Could not transform to %s", costmap_ros_->getBaseFrameID().c_str()); 354 | return cmd_vel; 355 | } 356 | 357 | try 358 | { 359 | robot_to_costmap_transform_ = buffer_->lookupTransform(costmap_ros_->getGlobalFrameID(), 360 | costmap_ros_->getBaseFrameID(), 361 | tf2::TimePointZero); 362 | } 363 | catch (tf2::TransformException& ex) 364 | { 365 | RCLCPP_ERROR(LOGGER, "Could not transform to %s", costmap_ros_->getGlobalFrameID().c_str()); 366 | return cmd_vel; 367 | } 368 | 369 | // Get the overall goal (in the robot frame) 370 | geometry_msgs::msg::PoseStamped goal_pose = global_plan_.poses.back(); 371 | tf2::doTransform(goal_pose, goal_pose, plan_to_robot); 372 | 373 | // Get goal tolerances 374 | geometry_msgs::msg::Pose pose_tolerance; 375 | geometry_msgs::msg::Twist velocity_tolerance; 376 | goal_checker->getTolerances(pose_tolerance, velocity_tolerance); 377 | 378 | // Compute distance to goal 379 | double dist_to_goal = std::hypot(goal_pose.pose.position.x, goal_pose.pose.position.y); 380 | 381 | // If we've reached the XY goal tolerance, just rotate 382 | if (dist_to_goal < pose_tolerance.position.x || goal_tolerance_met_) 383 | { 384 | // Reached goal, latch if desired 385 | goal_tolerance_met_ = latch_xy_goal_tolerance_; 386 | // Compute velocity required to rotate towards goal 387 | rotateTowards(tf2::getYaw(goal_pose.pose.orientation), velocity, cmd_vel); 388 | // Check for collisions between our current pose and goal 389 | double yaw_delta = tf2::getYaw(goal_pose.pose.orientation); 390 | size_t num_steps = fabs(yaw_delta) / 0.1; 391 | // Need to check at least the end pose 392 | num_steps = std::max(static_cast(1), num_steps); 393 | // If we fail to generate an in place rotation, maybe we need to move along path a bit more 394 | bool collision_free = true; 395 | for (size_t i = 1; i <= num_steps; ++i) 396 | { 397 | double step = static_cast(i) / static_cast(num_steps); 398 | double yaw = step * yaw_delta; 399 | if (isColliding(robot_pose.pose.position.x, robot_pose.pose.position.y, yaw, costmap_ros_, collision_points_)) 400 | { 401 | RCLCPP_ERROR(LOGGER, "Unable to rotate in place due to collision"); 402 | if (collision_points_ && !collision_points_->markers.empty()) 403 | { 404 | collision_points_->markers[0].header.stamp = clock_->now(); 405 | collision_points_pub_->publish(*collision_points_); 406 | } 407 | // Reset to zero velocity 408 | cmd_vel.twist = geometry_msgs::msg::Twist(); 409 | collision_free = false; 410 | break; 411 | } 412 | } 413 | if (collision_free) 414 | { 415 | // Safe to rotate, execute computed command 416 | return cmd_vel; 417 | } 418 | // Otherwise, fall through and try to get closer to goal in XY 419 | } 420 | 421 | // Get controller max velocity based on current speed 422 | double max_vel_x = max_vel_x_limited_; 423 | if (velocity.linear.x > max_vel_x) 424 | { 425 | // If our velocity limit has recently changed, 426 | // decelerate towards desired max_vel_x while still respecting acceleration limits 427 | double decelerating_max_vel_x = velocity.linear.x - (decel_lim_x_ * acc_dt_); 428 | max_vel_x = std::max(max_vel_x, decelerating_max_vel_x); 429 | max_vel_x = std::max(max_vel_x, min_vel_x_); 430 | } 431 | else 432 | { 433 | // Otherwise, allow up to max acceleration 434 | max_vel_x = velocity.linear.x + (acc_lim_x_ * acc_dt_); 435 | max_vel_x = std::max(min_vel_x_, std::min(max_vel_x, max_vel_x_limited_)); 436 | } 437 | 438 | // Compute distance along path 439 | std::vector target_poses; 440 | std::vector target_distances; 441 | for (auto pose : global_plan_.poses) 442 | { 443 | // Transform potential target pose into base_link 444 | geometry_msgs::msg::PoseStamped transformed_pose; 445 | tf2::doTransform(pose, transformed_pose, plan_to_robot); 446 | target_poses.push_back(transformed_pose); 447 | } 448 | computeDistanceAlongPath(target_poses, target_distances); 449 | 450 | // Work back from the end of plan to find valid target pose 451 | for (int i = global_plan_.poses.size() - 1; i >= 0; --i) 452 | { 453 | // Underlying control law needs a single target pose, which should: 454 | // * Be as far away as possible from the robot (for smoothness) 455 | // * But no further than the max_lookahed_ distance 456 | // * Be feasible to reach in a collision free manner 457 | geometry_msgs::msg::PoseStamped target_pose = target_poses[i]; 458 | double dist_to_target = target_distances[i]; 459 | 460 | // Continue if target_pose is too far away from robot 461 | if (dist_to_target > max_lookahead_) 462 | { 463 | continue; 464 | } 465 | 466 | if (dist_to_goal < max_lookahead_) 467 | { 468 | if (prefer_final_rotation_) 469 | { 470 | // Avoid unstability and big sweeping turns at the end of paths by 471 | // ignoring final heading 472 | double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x); 473 | target_pose.pose.orientation.z = sin(yaw / 2.0); 474 | target_pose.pose.orientation.w = cos(yaw / 2.0); 475 | } 476 | } 477 | else if (dist_to_target < min_lookahead_) 478 | { 479 | // Make sure target is far enough away to avoid instability 480 | break; 481 | } 482 | 483 | // Iteratively try to find a path, incrementally reducing the velocity 484 | double sim_velocity = max_vel_x; 485 | do 486 | { 487 | // Configure controller max velocity 488 | controller_->setVelocityLimits(min_vel_x_, sim_velocity, max_vel_theta_limited_); 489 | // Actually simulate our path 490 | if (simulate(target_pose, velocity, cmd_vel)) 491 | { 492 | // Have valid command 493 | return cmd_vel; 494 | } 495 | // Reduce velocity and try again for same target_pose 496 | sim_velocity -= scaling_step_; 497 | } 498 | while (sim_velocity >= scaling_vel_x_); 499 | } 500 | 501 | RCLCPP_ERROR(LOGGER, "No pose in path was reachable"); 502 | cmd_vel.twist.linear.x = 0.0; 503 | cmd_vel.twist.angular.z = 0.0; 504 | return cmd_vel; 505 | } 506 | 507 | bool GracefulControllerROS::simulate( 508 | const geometry_msgs::msg::PoseStamped& target_pose, 509 | const geometry_msgs::msg::Twist& velocity, 510 | geometry_msgs::msg::TwistStamped& cmd_vel) 511 | { 512 | // Simulated path (for debugging/visualization) 513 | nav_msgs::msg::Path simulated_path; 514 | // Should we simulate rotation initially 515 | bool sim_initial_rotation_ = has_new_path_ && initial_rotate_tolerance_ > 0.0; 516 | // Clear any previous visualizations 517 | if (collision_points_) 518 | { 519 | collision_points_->markers.resize(0); 520 | } 521 | // Get control and path, iteratively 522 | while (true) 523 | { 524 | // The error between current robot pose and the target pose 525 | geometry_msgs::msg::PoseStamped error = target_pose; 526 | 527 | // Extract error_angle 528 | double error_angle = tf2::getYaw(error.pose.orientation); 529 | 530 | // Move origin to our current simulated pose 531 | if (!simulated_path.poses.empty()) 532 | { 533 | double x = error.pose.position.x - simulated_path.poses.back().pose.position.x; 534 | double y = error.pose.position.y - simulated_path.poses.back().pose.position.y; 535 | 536 | double theta = -tf2::getYaw(simulated_path.poses.back().pose.orientation); 537 | error.pose.position.x = x * cos(theta) - y * sin(theta); 538 | error.pose.position.y = y * cos(theta) + x * sin(theta); 539 | 540 | error_angle += theta; 541 | error.pose.orientation.z = sin(error_angle / 2.0); 542 | error.pose.orientation.w = cos(error_angle / 2.0); 543 | } 544 | 545 | // Compute commands 546 | double vel_x, vel_th; 547 | if (sim_initial_rotation_) 548 | { 549 | geometry_msgs::msg::TwistStamped rotation; 550 | if (fabs(rotateTowards(error, velocity, rotation)) < initial_rotate_tolerance_) 551 | { 552 | if (simulated_path.poses.empty()) 553 | { 554 | // Current robot pose satisifies initial rotate tolerance 555 | RCLCPP_INFO(LOGGER, "Done rotating towards path"); 556 | has_new_path_ = false; 557 | } 558 | sim_initial_rotation_ = false; 559 | } 560 | vel_x = rotation.twist.linear.x; 561 | vel_th = rotation.twist.angular.z; 562 | } 563 | 564 | if (!sim_initial_rotation_) 565 | { 566 | if (!controller_->approach(error.pose.position.x, error.pose.position.y, error_angle, 567 | vel_x, vel_th)) 568 | { 569 | RCLCPP_ERROR(LOGGER, "Unable to compute approach"); 570 | return false; 571 | } 572 | } 573 | 574 | if (simulated_path.poses.empty()) 575 | { 576 | // First iteration of simulation, store our commands to the robot 577 | cmd_vel.twist.linear.x = vel_x; 578 | cmd_vel.twist.angular.z = vel_th; 579 | } 580 | else if (std::hypot(error.pose.position.x, error.pose.position.y) < resolution_) 581 | { 582 | // We've simulated to the desired pose, can return this result 583 | local_plan_pub_->publish(simulated_path); 584 | target_pose_pub_->publish(target_pose); 585 | // Publish visualization if desired 586 | if (collision_points_ && !collision_points_->markers.empty()) 587 | { 588 | collision_points_->markers[0].header.stamp = clock_->now(); 589 | collision_points_pub_->publish(*collision_points_); 590 | } 591 | return true; 592 | } 593 | 594 | // Forward simulate command 595 | geometry_msgs::msg::PoseStamped next_pose; 596 | next_pose.header.frame_id = costmap_ros_->getBaseFrameID(); 597 | if (simulated_path.poses.empty()) 598 | { 599 | // Initialize at origin 600 | next_pose.pose.orientation.w = 1.0; 601 | } 602 | else 603 | { 604 | // Start at last pose 605 | next_pose = simulated_path.poses.back(); 606 | } 607 | 608 | // Generate next pose 609 | double dt = (vel_x > 0.0) ? resolution_ / vel_x : 0.1; 610 | double yaw = tf2::getYaw(next_pose.pose.orientation); 611 | next_pose.pose.position.x += dt * vel_x * cos(yaw); 612 | next_pose.pose.position.y += dt * vel_x * sin(yaw); 613 | yaw += dt * vel_th; 614 | next_pose.pose.orientation.z = sin(yaw / 2.0); 615 | next_pose.pose.orientation.w = cos(yaw / 2.0); 616 | simulated_path.poses.push_back(next_pose); 617 | 618 | // Compute footprint scaling 619 | double footprint_scaling = 1.0; 620 | if (vel_x > scaling_vel_x_) 621 | { 622 | // Scaling = (vel_x - scaling_vel_x) / (max_vel_x - scaling_vel_x) 623 | // NOTE: max_vel_x_ is possibly changing from ROS topic 624 | double ratio = max_vel_x_limited_ - scaling_vel_x_; 625 | // Avoid divide by zero 626 | if (ratio > 0) 627 | { 628 | ratio = (vel_x - scaling_vel_x_) / ratio; 629 | footprint_scaling += ratio * scaling_factor_; 630 | } 631 | } 632 | 633 | // Check next pose for collision 634 | tf2::doTransform(next_pose, next_pose, robot_to_costmap_transform_); 635 | if (isColliding(next_pose.pose.position.x, next_pose.pose.position.y, tf2::getYaw(next_pose.pose.orientation), 636 | costmap_ros_, collision_points_, footprint_scaling)) 637 | { 638 | // Publish visualization if desired 639 | if (collision_points_ && !collision_points_->markers.empty()) 640 | { 641 | collision_points_->markers[0].header.stamp = clock_->now(); 642 | collision_points_pub_->publish(*collision_points_); 643 | } 644 | // Reason will be printed in function 645 | return false; 646 | } 647 | } 648 | 649 | // Really shouldn't hit this 650 | RCLCPP_ERROR(LOGGER, "Did not reach target_pose, but stopped simulating?"); 651 | return false; 652 | } 653 | 654 | void GracefulControllerROS::setPlan(const nav_msgs::msg::Path & path) 655 | { 656 | if (!initialized_) 657 | { 658 | RCLCPP_ERROR(LOGGER, "Controller is not initialized, call initialize() before using this controller"); 659 | return; 660 | } 661 | 662 | // We need orientations on our poses 663 | nav_msgs::msg::Path oriented_plan; 664 | if (compute_orientations_) 665 | { 666 | oriented_plan = addOrientations(path); 667 | } 668 | else 669 | { 670 | oriented_plan = path; 671 | } 672 | 673 | // Filter noisy orientations (if desired) 674 | nav_msgs::msg::Path filtered_plan; 675 | if (use_orientation_filter_) 676 | { 677 | filtered_plan = applyOrientationFilter(oriented_plan, yaw_filter_tolerance_, yaw_gap_tolerance_); 678 | } 679 | else 680 | { 681 | filtered_plan = oriented_plan; 682 | } 683 | 684 | // Store the plan for computeVelocityCommands 685 | global_plan_ = filtered_plan; 686 | has_new_path_ = true; 687 | goal_tolerance_met_ = false; 688 | RCLCPP_INFO(LOGGER, "Recieved a new path with %lu points in the %s frame", filtered_plan.poses.size(), 689 | global_plan_.header.frame_id.c_str()); 690 | } 691 | 692 | double GracefulControllerROS::rotateTowards( 693 | const geometry_msgs::msg::PoseStamped& pose, 694 | const geometry_msgs::msg::Twist& velocity, 695 | geometry_msgs::msg::TwistStamped& cmd_vel) 696 | { 697 | // Determine error 698 | double yaw = 0.0; 699 | if (std::hypot(pose.pose.position.x, pose.pose.position.y) > 0.5) 700 | { 701 | // Goal is far away, point towards goal 702 | yaw = std::atan2(pose.pose.position.y, pose.pose.position.x); 703 | } 704 | else 705 | { 706 | // Goal is nearby, align heading 707 | yaw = tf2::getYaw(pose.pose.orientation); 708 | } 709 | 710 | RCLCPP_DEBUG(LOGGER, "Rotating towards goal, error = %f", yaw); 711 | 712 | // Compute command velocity 713 | rotateTowards(yaw, velocity, cmd_vel); 714 | 715 | // Return error 716 | return yaw; 717 | } 718 | 719 | void GracefulControllerROS::rotateTowards( 720 | double yaw, 721 | const geometry_msgs::msg::Twist& velocity, 722 | geometry_msgs::msg::TwistStamped& cmd_vel) 723 | { 724 | // Determine max velocity based on current speed 725 | double max_vel_th = max_vel_theta_limited_; 726 | if (acc_dt_ > 0.0) 727 | { 728 | double abs_vel = fabs(velocity.angular.z); 729 | double acc_limited = abs_vel + (acc_lim_theta_ * acc_dt_); 730 | max_vel_th = std::min(max_vel_th, acc_limited); 731 | max_vel_th = std::max(max_vel_th, min_in_place_vel_theta_); 732 | } 733 | 734 | cmd_vel.twist.linear.x = 0.0; 735 | cmd_vel.twist.angular.z = std::sqrt(2 * acc_lim_theta_ * fabs(yaw)); 736 | cmd_vel.twist.angular.z = sign(yaw) * std::min(max_vel_th, std::max(min_in_place_vel_theta_, cmd_vel.twist.angular.z)); 737 | } 738 | 739 | void GracefulControllerROS::setSpeedLimit(const double& speed_limit, const bool& percentage) 740 | { 741 | // Lock the mutex 742 | std::lock_guard lock(config_mutex_); 743 | 744 | if (percentage) 745 | { 746 | max_vel_x_limited_ = std::max(speed_limit * max_vel_x_, min_vel_x_); 747 | } 748 | else 749 | { 750 | max_vel_x_limited_ = std::max(speed_limit, min_vel_x_); 751 | } 752 | // Limit maximum angular velocity proportional to maximum linear velocity 753 | max_vel_theta_limited_ = max_vel_x_limited_ * max_x_to_max_theta_scale_factor_; 754 | max_vel_theta_limited_ = std::min(max_vel_theta_limited_, max_vel_theta_); 755 | } 756 | 757 | void computeDistanceAlongPath(const std::vector& poses, 758 | std::vector& distances) 759 | { 760 | distances.resize(poses.size()); 761 | 762 | // First compute distance from robot to pose 763 | for (size_t i = 0; i < poses.size(); ++i) 764 | { 765 | // Determine distance from robot to pose 766 | distances[i] = std::hypot(poses[i].pose.position.x, poses[i].pose.position.y); 767 | } 768 | 769 | // Find the closest target pose 770 | auto closest = std::min_element(std::begin(distances), std::end(distances)); 771 | 772 | // Sum distances between poses, starting with the closest pose 773 | // Yes, the poses behind the robot will still use euclidean distance from robot, but we don't use those anyways 774 | for (size_t i = std::distance(std::begin(distances), closest) + 1; i < distances.size(); ++i) 775 | { 776 | distances[i] = distances[i - 1] + 777 | std::hypot(poses[i].pose.position.x - poses[i - 1].pose.position.x, 778 | poses[i].pose.position.y - poses[i - 1].pose.position.y); 779 | } 780 | } 781 | 782 | } // namespace graceful_controller 783 | 784 | #include 785 | PLUGINLIB_EXPORT_CLASS(graceful_controller::GracefulControllerROS, nav2_core::Controller) 786 | -------------------------------------------------------------------------------- /graceful_controller_ros/src/orientation_tools.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2021-2022, Michael Ferguson 6 | * Copyright (c) 2009, Willow Garage, Inc. 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 Willow Garage, Inc. 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: Michael Ferguson 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | #include 42 | #include // getYaw 43 | #include "graceful_controller_ros/orientation_tools.hpp" 44 | 45 | namespace graceful_controller 46 | { 47 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("graceful_controller"); 48 | 49 | // Helper function to get yaw if "from" were to point towards "to" 50 | double getRelativeYaw(const geometry_msgs::msg::PoseStamped& from, const geometry_msgs::msg::PoseStamped& to) 51 | { 52 | double dx = to.pose.position.x - from.pose.position.x; 53 | double dy = to.pose.position.y - from.pose.position.y; 54 | return std::atan2(dy, dx); 55 | } 56 | 57 | void setYaw(geometry_msgs::msg::PoseStamped& pose, double yaw) 58 | { 59 | pose.pose.orientation.z = sin(yaw / 2.0); 60 | pose.pose.orientation.w = cos(yaw / 2.0); 61 | } 62 | 63 | nav_msgs::msg::Path addOrientations(const nav_msgs::msg::Path& path) 64 | { 65 | nav_msgs::msg::Path oriented_path; 66 | oriented_path.header = path.header; 67 | oriented_path.poses.resize(path.poses.size()); 68 | if (path.poses.empty()) 69 | { 70 | // This really shouldn't happen 71 | return oriented_path; 72 | } 73 | 74 | // The last pose will already be oriented since it is our goal 75 | oriented_path.poses.back() = path.poses.back(); 76 | 77 | // For each pose, point at the next one 78 | for (size_t i = 0; i < oriented_path.poses.size() - 1; ++i) 79 | { 80 | oriented_path.poses[i] = path.poses[i]; 81 | double yaw = getRelativeYaw(path.poses[i], path.poses[i + 1]); 82 | setYaw(oriented_path.poses[i], yaw); 83 | } 84 | 85 | return oriented_path; 86 | } 87 | 88 | nav_msgs::msg::Path applyOrientationFilter(const nav_msgs::msg::Path& path, 89 | double yaw_tolerance, double gap_tolerance) 90 | { 91 | nav_msgs::msg::Path filtered_path; 92 | filtered_path.header = path.header; 93 | filtered_path.poses.reserve(path.poses.size()); 94 | if (path.poses.empty()) 95 | { 96 | // This really shouldn't happen 97 | return filtered_path; 98 | } 99 | 100 | // Always keep the first pose 101 | filtered_path.poses.push_back(path.poses.front()); 102 | 103 | // Possibly filter some intermediate poses 104 | for (size_t i = 1; i < path.poses.size() - 1; ++i) 105 | { 106 | // Get the yaw angle if the previous pose were to be pointing at this pose 107 | // We need to recompute because we might have dropped poses 108 | double yaw_previous = getRelativeYaw(filtered_path.poses.back(), path.poses[i]); 109 | 110 | // Get the yaw angle of this pose pointing at next pose 111 | double yaw_this = tf2::getYaw(path.poses[i].pose.orientation); 112 | 113 | // Get the yaw angle if previous pose were to be pointing at next pose, filtering this pose 114 | double yaw_without = getRelativeYaw(filtered_path.poses.back(), path.poses[i + 1]); 115 | 116 | // Determine if this pose is far off a direct line between previous and next pose 117 | if (fabs(angles::shortest_angular_distance(yaw_previous, yaw_without)) < yaw_tolerance && 118 | fabs(angles::shortest_angular_distance(yaw_this, yaw_without)) < yaw_tolerance) 119 | { 120 | // Update previous heading in case we dropped some poses 121 | setYaw(filtered_path.poses.back(), yaw_previous); 122 | // Add this pose to the filtered plan 123 | filtered_path.poses.push_back(path.poses[i]); 124 | } 125 | else if (std::hypot(path.poses[i].pose.position.x - filtered_path.poses.back().pose.position.x, 126 | path.poses[i].pose.position.y - filtered_path.poses.back().pose.position.y) >= gap_tolerance) 127 | { 128 | RCLCPP_DEBUG(LOGGER, "Including pose %lu to meet max_separation_dist", i); 129 | // Update previous heading in case we dropped some poses 130 | setYaw(filtered_path.poses.back(), yaw_previous); 131 | // Add this pose to the filtered plan 132 | filtered_path.poses.push_back(path.poses[i]); 133 | } 134 | else 135 | { 136 | // Sorry pose, the plan is better without you :( 137 | RCLCPP_DEBUG(LOGGER, "Filtering pose %lu", i); 138 | } 139 | } 140 | 141 | // Reset heading of what will be penultimate pose, in case we dropped some poses 142 | setYaw(filtered_path.poses.back(), getRelativeYaw(filtered_path.poses.back(), path.poses.back())); 143 | 144 | // Always add the last pose, since this is our goal 145 | filtered_path.poses.push_back(path.poses.back()); 146 | 147 | RCLCPP_DEBUG(LOGGER, "Filtered %lu points from plan", path.poses.size() - filtered_path.poses.size()); 148 | return filtered_path; 149 | } 150 | 151 | } // namespace graceful_controller 152 | -------------------------------------------------------------------------------- /graceful_controller_ros/src/visualization.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, Michael Ferguson 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #include "graceful_controller_ros/visualization.hpp" 37 | 38 | void addPointMarker(double x, double y, bool colliding, visualization_msgs::msg::MarkerArray* msg) 39 | { 40 | if (!msg) 41 | { 42 | // Nothing to do, just return 43 | return; 44 | } 45 | 46 | if (msg->markers.empty()) 47 | { 48 | // Add the marker 49 | msg->markers.resize(1); 50 | msg->markers[0].type = msg->markers[0].POINTS; 51 | msg->markers[0].header.frame_id = "odom"; 52 | msg->markers[0].pose.orientation.w = 1.0; 53 | msg->markers[0].scale.x = 0.02; 54 | msg->markers[0].scale.y = 0.02; 55 | msg->markers[0].scale.z = 0.02; 56 | } 57 | 58 | geometry_msgs::msg::Point point; 59 | point.x = x; 60 | point.y = y; 61 | point.z = 0.0; 62 | msg->markers[0].points.push_back(point); 63 | 64 | std_msgs::msg::ColorRGBA color; 65 | if (colliding) 66 | { 67 | // Colliding points are RED 68 | color.r = 1; 69 | color.g = 0; 70 | color.b = 0; 71 | } 72 | else 73 | { 74 | // Non-colliding points are GREEN 75 | color.r = 0; 76 | color.g = 1; 77 | color.b = 0; 78 | } 79 | // All points are solid colored 80 | color.a = 1; 81 | msg->markers[0].colors.push_back(color); 82 | } 83 | -------------------------------------------------------------------------------- /graceful_controller_ros/test/config.yaml: -------------------------------------------------------------------------------- 1 | graceful_controller_tests: 2 | ros__parameters: 3 | graceful_controller: 4 | max_vel_x: 1.0 5 | min_vel_x: 0.25 6 | 7 | max_vel_theta: 2.5 8 | min_vel_theta: 0.3 9 | min_in_place_vel_theta: 0.6 10 | 11 | acc_lim_x: 0.5 12 | acc_lim_theta: 1.0 13 | 14 | max_lookahead: 1.0 15 | initial_rotate_tolerance: 0.25 16 | 17 | costmap: 18 | costmap: 19 | ros__parameters: 20 | update_frequency: 5.0 21 | publish_frequency: 2.0 22 | global_frame: odom 23 | robot_base_frame: base_link 24 | rolling_window: True 25 | robot_radius: 0.25 26 | width: 4 27 | height: 4 28 | resolution: 0.05 29 | plugins: ["static_map", "inflation_layer"] 30 | static_map: 31 | plugin: "nav2_costmap_2d::StaticLayer" 32 | map_subscribe_transient_local: True 33 | map_topic: /map 34 | inflation_layer: 35 | plugin: "nav2_costmap_2d::InflationLayer" 36 | robot_radius: 0.25 37 | inflation_radius: 0.5 38 | -------------------------------------------------------------------------------- /graceful_controller_ros/test/graceful_controller_tests.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2021-2023, Michael Ferguson 6 | * Copyright (c) 2009, Willow Garage, Inc. 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 Willow Garage, Inc. 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: Michael Ferguson 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | // Only needed for computeDistanceAlongPath tests 49 | #include 50 | 51 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("graceful_controller_tests"); 52 | 53 | class GoalCheckerFixture : public nav2_core::GoalChecker 54 | { 55 | public: 56 | virtual void initialize( 57 | const rclcpp_lifecycle::LifecycleNode::WeakPtr&, 58 | const std::string&, 59 | const std::shared_ptr) 60 | { 61 | // Do nothing 62 | } 63 | 64 | virtual void reset() 65 | { 66 | // Do nothing 67 | } 68 | 69 | virtual bool isGoalReached( 70 | const geometry_msgs::msg::Pose&, const geometry_msgs::msg::Pose&, 71 | const geometry_msgs::msg::Twist&) 72 | { 73 | return true; 74 | } 75 | 76 | virtual bool getTolerances( 77 | geometry_msgs::msg::Pose& pose_tolerance, 78 | geometry_msgs::msg::Twist&) 79 | { 80 | pose_tolerance.position.x = 0.125; 81 | return true; 82 | } 83 | }; 84 | 85 | class ControllerFixture : public rclcpp_lifecycle::LifecycleNode 86 | { 87 | public: 88 | ControllerFixture() : 89 | LifecycleNode("graceful_conroller_tests"), 90 | loader_("nav2_core", "nav2_core::Controller"), 91 | costmap_ros_(NULL), 92 | shutdown_(false) 93 | { 94 | buffer_.reset(new tf2_ros::Buffer(this->get_clock())); 95 | listener_.reset(new tf2_ros::TransformListener(*buffer_)); 96 | broadcaster_.reset(new tf2_ros::TransformBroadcaster(this)); 97 | } 98 | 99 | bool setup(bool do_init = true) 100 | { 101 | // ROS topics to run the test 102 | rclcpp::QoS map_qos(rclcpp::KeepLast(1)); 103 | map_qos.transient_local(); 104 | map_qos.reliable(); 105 | map_pub_ = this->create_publisher("/map", map_qos); 106 | 107 | // Need to start publishing odom before we initialize the costmap 108 | resetMap(); 109 | resetPose(); 110 | setSimVelocity(0.0, 0.0); 111 | thread_ = new std::thread(std::bind(&ControllerFixture::updateThread, this)); 112 | 113 | // Costmap for testing 114 | costmap_ros_.reset(new nav2_costmap_2d::Costmap2DROS("costmap")); 115 | 116 | // Controller instance 117 | try 118 | { 119 | controller_ = loader_.createSharedInstance("graceful_controller/GracefulControllerROS"); 120 | } 121 | catch (const pluginlib::PluginlibException& ex) 122 | { 123 | RCLCPP_FATAL(LOGGER, "Failed to create the controller. Exception: %s", ex.what()); 124 | return false; 125 | } 126 | 127 | if (do_init) 128 | { 129 | initialize(); 130 | } 131 | 132 | return true; 133 | } 134 | 135 | ~ControllerFixture() 136 | { 137 | costmap_ros_->deactivate(); 138 | controller_->deactivate(); 139 | // Release the controller 140 | controller_.reset(); 141 | // Stop our own update thread 142 | shutdown_ = true; 143 | thread_->join(); 144 | delete thread_; 145 | } 146 | 147 | void initialize() 148 | { 149 | // Configure everything 150 | this->configure(); 151 | costmap_ros_->configure(); 152 | controller_->configure(this->shared_from_this(), 153 | "graceful_controller", 154 | buffer_, 155 | costmap_ros_); 156 | // Activate everything 157 | this->activate(); 158 | costmap_ros_->activate(); 159 | controller_->activate(); 160 | } 161 | 162 | std::shared_ptr getController() 163 | { 164 | return controller_; 165 | } 166 | 167 | std::shared_ptr getCostmap() 168 | { 169 | return costmap_ros_; 170 | } 171 | 172 | void resetMap() 173 | { 174 | map_.header.frame_id = "map"; 175 | map_.info.resolution = 0.05; 176 | map_.info.width = 100; 177 | map_.info.height = 100; 178 | map_.info.origin.position.x = -2.5; 179 | map_.info.origin.position.y = -2.5; 180 | 181 | map_.data.resize(100 * 100); 182 | for (size_t i = 0; i < map_.data.size(); ++i) 183 | { 184 | map_.data[i] = 0; 185 | } 186 | } 187 | 188 | bool markMap(double x, double y) 189 | { 190 | int ix = (x - map_.info.origin.position.x) / map_.info.resolution; 191 | int iy = (y - map_.info.origin.position.y) / map_.info.resolution; 192 | 193 | if (ix < 0 || ix >= static_cast(map_.info.width) || 194 | iy < 0 || iy >= static_cast(map_.info.height)) 195 | { 196 | return false; 197 | } 198 | 199 | map_.data[ix + (iy * map_.info.width)] = 100; 200 | map_pub_->publish(map_); 201 | return true; 202 | } 203 | 204 | void resetPose() 205 | { 206 | odom_.header.frame_id = "odom"; 207 | odom_.child_frame_id = "base_link"; 208 | odom_.pose.pose.position.x = 0.0; 209 | odom_.pose.pose.position.y = 0.0; 210 | odom_.pose.pose.orientation.w = 1.0; 211 | } 212 | 213 | void setPose(double x, double y, double yaw) 214 | { 215 | odom_.header.frame_id = "odom"; 216 | odom_.child_frame_id = "base_link"; 217 | odom_.pose.pose.position.x = x; 218 | odom_.pose.pose.position.y = y; 219 | odom_.pose.pose.orientation.z = sin(yaw / 2.0); 220 | odom_.pose.pose.orientation.w = cos(yaw / 2.0); 221 | 222 | // Wait for TF2 to propagate 223 | rclcpp::sleep_for(std::chrono::milliseconds(250)); 224 | } 225 | 226 | void setMaxVelocity(float velocity) 227 | { 228 | controller_->setSpeedLimit(velocity, false); 229 | } 230 | 231 | void setSimVelocity(double x, double th) 232 | { 233 | odom_.twist.twist.linear.x = x; 234 | odom_.twist.twist.angular.z = th; 235 | } 236 | 237 | geometry_msgs::msg::PoseStamped getRobotPose() 238 | { 239 | geometry_msgs::msg::PoseStamped pose; 240 | pose.header = odom_.header; 241 | pose.pose = odom_.pose.pose; 242 | return pose; 243 | } 244 | 245 | geometry_msgs::msg::Twist getRobotVelocity() 246 | { 247 | return odom_.twist.twist; 248 | } 249 | 250 | protected: 251 | void updateThread() 252 | { 253 | map_pub_->publish(map_); 254 | 255 | while (rclcpp::ok() && !shutdown_) 256 | { 257 | // Fake localization 258 | geometry_msgs::msg::TransformStamped transform; 259 | transform.header.stamp = this->now(); 260 | transform.header.frame_id = "map"; 261 | transform.child_frame_id = odom_.header.frame_id; 262 | transform.transform.translation.x = 0.0; 263 | transform.transform.translation.y = 0.0; 264 | transform.transform.translation.z = 0.0; 265 | transform.transform.rotation.x = 0.0; 266 | transform.transform.rotation.y = 0.0; 267 | transform.transform.rotation.z = 0.0; 268 | transform.transform.rotation.w = 1.0; 269 | broadcaster_->sendTransform(transform); 270 | 271 | // Publish updated odom as TF 272 | transform.header.frame_id = odom_.header.frame_id; 273 | transform.child_frame_id = odom_.child_frame_id; 274 | transform.transform.translation.x = odom_.pose.pose.position.x; 275 | transform.transform.translation.y = odom_.pose.pose.position.y; 276 | transform.transform.translation.z = 0.0; 277 | transform.transform.rotation = odom_.pose.pose.orientation; 278 | broadcaster_->sendTransform(transform); 279 | 280 | rclcpp::spin_some(this->get_node_base_interface()); 281 | if (costmap_ros_) 282 | rclcpp::spin_some(costmap_ros_->get_node_base_interface()); 283 | rclcpp::sleep_for(std::chrono::milliseconds(50)); 284 | } 285 | } 286 | 287 | pluginlib::ClassLoader loader_; 288 | std::shared_ptr controller_; 289 | std::shared_ptr buffer_; 290 | std::shared_ptr listener_; 291 | std::shared_ptr broadcaster_; 292 | std::shared_ptr costmap_ros_; 293 | std::shared_ptr> map_pub_; 294 | nav_msgs::msg::OccupancyGrid map_; 295 | nav_msgs::msg::Odometry odom_; 296 | std::thread* thread_; 297 | bool shutdown_; 298 | }; 299 | 300 | TEST(ControllerTests, test_basic_plan) 301 | { 302 | GoalCheckerFixture goal_checker; 303 | std::shared_ptr fixture(new ControllerFixture()); 304 | ASSERT_TRUE(fixture->setup(false /* do not initialize */)); 305 | std::shared_ptr controller = fixture->getController(); 306 | 307 | nav_msgs::msg::Path plan; 308 | geometry_msgs::msg::PoseStamped pose; 309 | plan.header.frame_id = "map"; 310 | pose.header.frame_id = plan.header.frame_id; 311 | pose.pose.orientation.w = 1.0; 312 | pose.pose.position.x = 1.0; 313 | pose.pose.position.y = 0.0; 314 | plan.poses.push_back(pose); 315 | pose.pose.position.x = 1.5; 316 | pose.pose.position.y = 0.0; 317 | plan.poses.push_back(pose); 318 | 319 | geometry_msgs::msg::PoseStamped robot_pose = fixture->getRobotPose(); 320 | geometry_msgs::msg::Twist robot_velocity = fixture->getRobotVelocity(); 321 | 322 | // Unitialized controller should not be able to plan 323 | controller->setPlan(plan); 324 | geometry_msgs::msg::TwistStamped command; 325 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, NULL); 326 | EXPECT_EQ(command.twist.linear.x, 0.0); 327 | EXPECT_EQ(command.twist.angular.z, 0.0); 328 | RCLCPP_WARN(LOGGER, "Calling initialize"); 329 | fixture->initialize(); 330 | 331 | // Calling multiple times should not be a problem 332 | RCLCPP_WARN(LOGGER, "Calling initialize"); 333 | fixture->initialize(); 334 | 335 | // Now we can set the plan 336 | RCLCPP_WARN(LOGGER, "Setting plan"); 337 | controller->setPlan(plan); 338 | 339 | // Set velocity to 0 340 | fixture->setSimVelocity(0.0, 0.0); 341 | robot_velocity = fixture->getRobotVelocity(); 342 | 343 | // Odom reports velocity = 0, but min_vel_x is greater than acc_lim * acc_dt 344 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 345 | EXPECT_EQ(command.twist.linear.x, 0.25); 346 | EXPECT_EQ(command.twist.angular.z, 0.0); 347 | 348 | // Set a new max velocity 349 | fixture->setMaxVelocity(0.5); 350 | robot_velocity = fixture->getRobotVelocity(); 351 | 352 | // Odom still reports 0, so max remains the same 353 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 354 | EXPECT_EQ(command.twist.linear.x, 0.25); 355 | EXPECT_EQ(command.twist.angular.z, 0.0); 356 | 357 | // Set current velocity above the velocity limit 358 | fixture->setSimVelocity(1.0, 0.0); 359 | robot_velocity = fixture->getRobotVelocity(); 360 | 361 | // Odom now reports 1.0, but max_vel_x topic is 0.5 - will deccelerate down to 0.5 362 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 363 | EXPECT_EQ(command.twist.linear.x, 0.875); 364 | EXPECT_EQ(command.twist.angular.z, 0.0); 365 | 366 | // Bump up our limit and speed 367 | fixture->setMaxVelocity(1.0); 368 | robot_velocity = fixture->getRobotVelocity(); 369 | 370 | // Expect max velocity 371 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 372 | EXPECT_EQ(command.twist.linear.x, 1.0); 373 | EXPECT_EQ(command.twist.angular.z, 0.0); 374 | 375 | // Report velocity over limits 376 | fixture->setSimVelocity(3.0, 0.0); 377 | robot_velocity = fixture->getRobotVelocity(); 378 | 379 | // Expect max velocity 380 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 381 | EXPECT_EQ(command.twist.linear.x, 1.0); 382 | EXPECT_EQ(command.twist.angular.z, 0.0); 383 | } 384 | 385 | TEST(ControllerTests, test_out_of_range) 386 | { 387 | GoalCheckerFixture goal_checker; 388 | std::shared_ptr fixture(new ControllerFixture()); 389 | ASSERT_TRUE(fixture->setup()); 390 | std::shared_ptr controller = fixture->getController(); 391 | 392 | nav_msgs::msg::Path plan; 393 | geometry_msgs::msg::PoseStamped pose; 394 | plan.header.frame_id = "map"; 395 | pose.header.frame_id = plan.header.frame_id; 396 | pose.pose.orientation.w = 1.0; 397 | pose.pose.position.x = 1.5; 398 | pose.pose.position.y = 0.0; 399 | plan.poses.push_back(pose); 400 | controller->setPlan(plan); 401 | 402 | geometry_msgs::msg::PoseStamped robot_pose; 403 | pose.header.frame_id = "map"; 404 | pose.pose.position.x = 0.0; 405 | pose.pose.position.y = 0.0; 406 | pose.pose.orientation.w = 1.0; 407 | 408 | geometry_msgs::msg::Twist robot_velocity; 409 | robot_velocity.linear.x = 0.0; 410 | robot_velocity.linear.y = 0.0; 411 | 412 | // First pose is beyond the lookahead - should fail 413 | geometry_msgs::msg::TwistStamped command; 414 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 415 | EXPECT_EQ(command.twist.linear.x, 0.0); 416 | EXPECT_EQ(command.twist.angular.z, 0.0); 417 | } 418 | 419 | TEST(ControllerTests, test_initial_rotate_in_place) 420 | { 421 | GoalCheckerFixture goal_checker; 422 | std::shared_ptr fixture(new ControllerFixture()); 423 | ASSERT_TRUE(fixture->setup()); 424 | std::shared_ptr controller = fixture->getController(); 425 | 426 | nav_msgs::msg::Path plan; 427 | geometry_msgs::msg::PoseStamped pose; 428 | plan.header.frame_id = "map"; 429 | pose.header.frame_id = plan.header.frame_id; 430 | pose.pose.orientation.w = 1.0; 431 | pose.pose.position.x = -0.5; 432 | pose.pose.position.y = 0.0; 433 | plan.poses.push_back(pose); 434 | pose.pose.position.x = -1.0; 435 | pose.pose.position.y = 0.0; 436 | plan.poses.push_back(pose); 437 | controller->setPlan(plan); 438 | 439 | // Set our velocity to 0 440 | fixture->setSimVelocity(0.0, 0.0); 441 | geometry_msgs::msg::PoseStamped robot_pose = fixture->getRobotPose(); 442 | geometry_msgs::msg::Twist robot_velocity = fixture->getRobotVelocity(); 443 | 444 | // Odom reports velocity = 0, but min_in_place_vel_theta is 0.6 445 | geometry_msgs::msg::TwistStamped command; 446 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 447 | EXPECT_EQ(command.twist.linear.x, 0.0); 448 | EXPECT_EQ(command.twist.angular.z, 0.6); 449 | 450 | // Set our velocity to 1.0 451 | fixture->setSimVelocity(0.0, 1.0); 452 | robot_velocity = fixture->getRobotVelocity(); 453 | 454 | // Expect limited rotation command 455 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 456 | EXPECT_EQ(command.twist.linear.x, 0.0); 457 | EXPECT_EQ(command.twist.angular.z, 1.25); 458 | 459 | // Report velocity over limits 460 | fixture->setSimVelocity(0.0, 4.0); 461 | robot_velocity = fixture->getRobotVelocity(); 462 | 463 | // Expect max rotation 464 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 465 | EXPECT_EQ(command.twist.linear.x, 0.0); 466 | EXPECT_EQ(command.twist.angular.z, 2.5); 467 | } 468 | 469 | TEST(ControllerTests, test_final_rotate_in_place) 470 | { 471 | GoalCheckerFixture goal_checker; 472 | std::shared_ptr fixture(new ControllerFixture()); 473 | ASSERT_TRUE(fixture->setup()); 474 | std::shared_ptr controller = fixture->getController(); 475 | 476 | nav_msgs::msg::Path plan; 477 | geometry_msgs::msg::PoseStamped pose; 478 | plan.header.frame_id = "map"; 479 | pose.header.frame_id = plan.header.frame_id; 480 | pose.pose.orientation.w = 1.0; 481 | pose.pose.position.x = 0.5; 482 | pose.pose.position.y = 0.0; 483 | plan.poses.push_back(pose); 484 | controller->setPlan(plan); 485 | 486 | // Set pose to start 487 | fixture->setPose(0.1, 0.0, 0.0); 488 | geometry_msgs::msg::PoseStamped robot_pose = fixture->getRobotPose(); 489 | geometry_msgs::msg::Twist robot_velocity = fixture->getRobotVelocity(); 490 | 491 | // Expect forward motion at min velocity since we are aligned with only goal pose 492 | geometry_msgs::msg::TwistStamped command; 493 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 494 | EXPECT_EQ(command.twist.linear.x, 0.25); 495 | EXPECT_EQ(command.twist.angular.z, 0.0); 496 | 497 | // Set our pose to the end goal, but with bad heading 498 | fixture->setPose(0.5, 0.0, 1.57); 499 | robot_pose = fixture->getRobotPose(); 500 | 501 | // Expect limited rotation command 502 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 503 | EXPECT_EQ(command.twist.linear.x, 0.0); 504 | EXPECT_EQ(command.twist.angular.z, -0.6); 505 | } 506 | 507 | TEST(ControllerTests, test_collision_check) 508 | { 509 | GoalCheckerFixture goal_checker; 510 | std::shared_ptr fixture(new ControllerFixture()); 511 | ASSERT_TRUE(fixture->setup()); 512 | std::shared_ptr controller = fixture->getController(); 513 | 514 | fixture->markMap(0.65, 0); 515 | rclcpp::sleep_for(std::chrono::milliseconds(250)); 516 | 517 | nav_msgs::msg::Path plan; 518 | geometry_msgs::msg::PoseStamped pose; 519 | plan.header.frame_id = "map"; 520 | pose.header.frame_id = plan.header.frame_id; 521 | pose.pose.orientation.w = 1.0; 522 | pose.pose.position.x = 0.5; 523 | pose.pose.position.y = 0.0; 524 | plan.poses.push_back(pose); 525 | controller->setPlan(plan); 526 | 527 | geometry_msgs::msg::PoseStamped robot_pose = fixture->getRobotPose(); 528 | geometry_msgs::msg::Twist robot_velocity = fixture->getRobotVelocity(); 529 | 530 | // Expect no command 531 | geometry_msgs::msg::TwistStamped command; 532 | command = controller->computeVelocityCommands(robot_pose, robot_velocity, &goal_checker); 533 | EXPECT_EQ(command.twist.linear.x, 0.0); 534 | EXPECT_EQ(command.twist.angular.z, 0.0); 535 | } 536 | 537 | TEST(ControllerTests, test_compute_distance_along_path) 538 | { 539 | std::vector poses; 540 | std::vector distances; 541 | 542 | // Simple set of poses 543 | for (int i = 0; i < 5; ++i) 544 | { 545 | geometry_msgs::msg::PoseStamped pose; 546 | pose.pose.position.x = (i - 2); 547 | pose.pose.position.y = 0; 548 | poses.push_back(pose); 549 | } 550 | 551 | // Check distances 552 | graceful_controller::computeDistanceAlongPath(poses, distances); 553 | EXPECT_EQ(distances[0], 2); 554 | EXPECT_EQ(distances[1], 1); 555 | EXPECT_EQ(distances[2], 0); 556 | EXPECT_EQ(distances[3], 1); 557 | EXPECT_EQ(distances[4], 2); 558 | 559 | // Make path wrap around 560 | distances.clear(); 561 | { 562 | geometry_msgs::msg::PoseStamped pose; 563 | pose.pose.position.x = 2; 564 | pose.pose.position.y = 1; 565 | poses.push_back(pose); 566 | pose.pose.position.x = 1; 567 | pose.pose.position.y = 1; 568 | poses.push_back(pose); 569 | pose.pose.position.x = 0; 570 | pose.pose.position.y = 1; 571 | poses.push_back(pose); 572 | } 573 | 574 | // Check distances 575 | graceful_controller::computeDistanceAlongPath(poses, distances); 576 | EXPECT_EQ(distances[0], 2); 577 | EXPECT_EQ(distances[1], 1); 578 | EXPECT_EQ(distances[2], 0); 579 | EXPECT_EQ(distances[3], 1); 580 | EXPECT_EQ(distances[4], 2); 581 | EXPECT_EQ(distances[5], 3); 582 | EXPECT_EQ(distances[6], 4); 583 | EXPECT_EQ(distances[7], 5); 584 | } 585 | 586 | int main(int argc, char** argv) 587 | { 588 | testing::InitGoogleTest(&argc, argv); 589 | rclcpp::init(argc, argv); 590 | bool success = RUN_ALL_TESTS(); 591 | rclcpp::shutdown(); 592 | return success; 593 | } 594 | -------------------------------------------------------------------------------- /graceful_controller_ros/test/graceful_controller_tests_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import sys 5 | 6 | from launch import LaunchDescription 7 | from launch import LaunchService 8 | from launch_ros.actions import Node 9 | from launch_testing.legacy import LaunchTestService 10 | 11 | 12 | def main(argv=sys.argv[1:]): 13 | config_file = os.path.join(os.getenv('CONFIG_DIR'), 'config.yaml') 14 | 15 | test_node = Node( 16 | executable=[os.getenv('TEST_EXECUTABLE')], 17 | name='graceful_controller_tests', 18 | parameters=[config_file], 19 | output='screen', 20 | ) 21 | 22 | ld = LaunchDescription() 23 | lts = LaunchTestService() 24 | lts.add_test_action(ld, test_node) 25 | ls = LaunchService(argv=argv) 26 | ls.include_launch_description(ld) 27 | return lts.run(ls) 28 | 29 | 30 | if __name__ == '__main__': 31 | sys.exit(main()) 32 | -------------------------------------------------------------------------------- /graceful_controller_ros/test/orientation_tools_tests.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2021-2022, Michael Ferguson 6 | * Copyright (c) 2009, Willow Garage, Inc. 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 Willow Garage, Inc. 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: Eitan Marder-Eppstein, Michael Ferguson 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | #include // getYaw 42 | #include "graceful_controller_ros/orientation_tools.hpp" 43 | 44 | using namespace graceful_controller; 45 | 46 | TEST(OrientationToolsTests, test_bad_penultimate_pose) 47 | { 48 | // Most global planners plan on a discrete grid, 49 | // but then add the start/end poses in continuous space. 50 | // This can lead to an odd orientations near the start/end of the path. 51 | nav_msgs::msg::Path path; 52 | for (size_t i = 0; i < 10; ++i) 53 | { 54 | geometry_msgs::msg::PoseStamped pose; 55 | pose.pose.position.x = i * 0.05; 56 | pose.pose.position.y = 0.0; 57 | pose.pose.orientation.w = 1.0; 58 | path.poses.push_back(pose); 59 | } 60 | 61 | // Set final pose to be off the grid and slightly BACK from last discrete pose 62 | path.poses.back().pose.position.x = 0.378; 63 | path.poses.back().pose.position.y = 0.02; 64 | // Set final pose to have a small rotation 65 | { 66 | double final_yaw = 0.15; 67 | path.poses.back().pose.orientation.z = sin(final_yaw / 2.0); 68 | path.poses.back().pose.orientation.w = cos(final_yaw / 2.0); 69 | } 70 | 71 | // Apply orientations 72 | path = addOrientations(path); 73 | 74 | // Last orientation should be unaltered 75 | EXPECT_EQ(0.0, path.poses.back().pose.orientation.x); 76 | EXPECT_EQ(0.0, path.poses.back().pose.orientation.y); 77 | EXPECT_FLOAT_EQ(0.15, tf2::getYaw(path.poses.back().pose.orientation)); 78 | 79 | // Early orientations should be forward 80 | for (size_t i = 0; i < 8; ++i) 81 | { 82 | EXPECT_EQ(0.0, path.poses[i].pose.orientation.x); 83 | EXPECT_EQ(0.0, path.poses[i].pose.orientation.y); 84 | EXPECT_FLOAT_EQ(0.0, tf2::getYaw(path.poses[i].pose.orientation)); 85 | } 86 | 87 | // Penultimate pose should be crazy 88 | EXPECT_EQ(0.0, path.poses[8].pose.orientation.x); 89 | EXPECT_EQ(0.0, path.poses[8].pose.orientation.y); 90 | EXPECT_FLOAT_EQ(2.4037776, tf2::getYaw(path.poses[8].pose.orientation)); 91 | 92 | // Now filter the penultimate pose away 93 | nav_msgs::msg::Path filtered_path; 94 | filtered_path = applyOrientationFilter(path, 0.785 /* 45 degrees */, 0.25); 95 | 96 | // Should have removed one pose (penultimate one) 97 | EXPECT_EQ(9, static_cast(filtered_path.poses.size())); 98 | 99 | // Last orientation should be unaltered 100 | EXPECT_EQ(0.0, filtered_path.poses.back().pose.orientation.x); 101 | EXPECT_EQ(0.0, filtered_path.poses.back().pose.orientation.y); 102 | EXPECT_FLOAT_EQ(0.15, tf2::getYaw(filtered_path.poses.back().pose.orientation)); 103 | 104 | // Early orientations should be forward 105 | for (size_t i = 0; i < 7; ++i) 106 | { 107 | EXPECT_EQ(0.0, filtered_path.poses[i].pose.orientation.x); 108 | EXPECT_EQ(0.0, filtered_path.poses[i].pose.orientation.y); 109 | EXPECT_FLOAT_EQ(0.0, tf2::getYaw(filtered_path.poses[i].pose.orientation)); 110 | } 111 | 112 | // Penultimate pose should be "better" 113 | EXPECT_EQ(0.0, filtered_path.poses[7].pose.orientation.x); 114 | EXPECT_EQ(0.0, filtered_path.poses[7].pose.orientation.y); 115 | EXPECT_FLOAT_EQ(0.62024951, tf2::getYaw(filtered_path.poses[7].pose.orientation)); 116 | } 117 | 118 | TEST(OrientationToolsTests, test_bad_initial_pose) 119 | { 120 | // Most global planners plan on a discrete grid, 121 | // but then add the start/end poses in continuous space. 122 | // This can lead to an odd orientations near the start/end of the path. 123 | nav_msgs::msg::Path path; 124 | for (size_t i = 0; i < 10; ++i) 125 | { 126 | geometry_msgs::msg::PoseStamped pose; 127 | pose.pose.position.x = i * 0.05; 128 | pose.pose.position.y = 0.0; 129 | pose.pose.orientation.w = 1.0; 130 | path.poses.push_back(pose); 131 | } 132 | 133 | // Set initial pose to be off the grid, leading to bad initial orientation 134 | path.poses.front().pose.position.x = 0.051; 135 | path.poses.front().pose.position.y = 0.01; 136 | // Set final pose to have a small rotation 137 | { 138 | double final_yaw = -0.12; 139 | path.poses.back().pose.orientation.z = sin(final_yaw / 2.0); 140 | path.poses.back().pose.orientation.w = cos(final_yaw / 2.0); 141 | } 142 | 143 | // Apply orientations 144 | path = addOrientations(path); 145 | 146 | // Last orientation should be unaltered 147 | EXPECT_EQ(0.0, path.poses.back().pose.orientation.x); 148 | EXPECT_EQ(0.0, path.poses.back().pose.orientation.y); 149 | EXPECT_FLOAT_EQ(-0.12, tf2::getYaw(path.poses.back().pose.orientation)); 150 | 151 | // Initial pose orientation should be crazy 152 | EXPECT_EQ(0.0, path.poses.front().pose.orientation.x); 153 | EXPECT_EQ(0.0, path.poses.front().pose.orientation.y); 154 | EXPECT_FLOAT_EQ(-1.670465, tf2::getYaw(path.poses.front().pose.orientation)); 155 | 156 | // Other orientations should be forward 157 | for (size_t i = 1; i < 9; ++i) 158 | { 159 | EXPECT_EQ(0.0, path.poses[i].pose.orientation.x); 160 | EXPECT_EQ(0.0, path.poses[i].pose.orientation.y); 161 | EXPECT_FLOAT_EQ(0.0, tf2::getYaw(path.poses[i].pose.orientation)); 162 | } 163 | 164 | // Now filter the initial pose away 165 | nav_msgs::msg::Path filtered_path; 166 | filtered_path = applyOrientationFilter(path, 0.785 /* 45 degrees */, 0.25); 167 | 168 | // Should have removed one pose 169 | EXPECT_EQ(9, static_cast(filtered_path.poses.size())); 170 | 171 | // Initial orientation should be better 172 | EXPECT_EQ(0.0, filtered_path.poses.front().pose.orientation.x); 173 | EXPECT_EQ(0.0, filtered_path.poses.front().pose.orientation.y); 174 | EXPECT_FLOAT_EQ(-0.2013171, tf2::getYaw(filtered_path.poses.front().pose.orientation)); 175 | 176 | // Other orientations should be forward 177 | for (size_t i = 1; i < 8; ++i) 178 | { 179 | EXPECT_EQ(0.0, filtered_path.poses[i].pose.orientation.x); 180 | EXPECT_EQ(0.0, filtered_path.poses[i].pose.orientation.y); 181 | EXPECT_FLOAT_EQ(0.0, tf2::getYaw(filtered_path.poses[i].pose.orientation)); 182 | } 183 | 184 | // Last orientation should be unaltered 185 | EXPECT_EQ(0.0, filtered_path.poses.back().pose.orientation.x); 186 | EXPECT_EQ(0.0, filtered_path.poses.back().pose.orientation.y); 187 | EXPECT_FLOAT_EQ(-0.12, tf2::getYaw(filtered_path.poses.back().pose.orientation)); 188 | } 189 | 190 | TEST(OrientationToolsTests, test_zigzag) 191 | { 192 | // This is an entirely unlikely path 193 | // Any global planner creating this path should be garbage collected 194 | nav_msgs::msg::Path path; 195 | for (size_t i = 0; i < 20; ++i) 196 | { 197 | geometry_msgs::msg::PoseStamped pose; 198 | pose.pose.position.x = i * 0.05; 199 | if (i % 2 == 0) 200 | { 201 | pose.pose.position.y = 0.15; 202 | } 203 | else 204 | { 205 | pose.pose.position.y = 0.0; 206 | } 207 | pose.pose.orientation.w = 1.0; 208 | path.poses.push_back(pose); 209 | } 210 | 211 | // Apply orientations 212 | path = addOrientations(path); 213 | 214 | // Now filter this awful path 215 | nav_msgs::msg::Path filtered_path; 216 | filtered_path = applyOrientationFilter(path, 0.785 /* 45 degrees */, 1.0); 217 | 218 | // Should have removed some poses 219 | EXPECT_EQ(6, static_cast(filtered_path.poses.size())); 220 | 221 | // Make sure the distance between remaining poses isn't too far 222 | for (size_t i = 1; i < filtered_path.poses.size(); ++i) 223 | { 224 | double dx = filtered_path.poses[i].pose.position.x - filtered_path.poses[i-1].pose.position.x; 225 | double dy = filtered_path.poses[i].pose.position.y - filtered_path.poses[i-1].pose.position.y; 226 | EXPECT_TRUE(std::hypot(dx, dy) < 0.25); 227 | } 228 | } 229 | 230 | TEST(OrientationToolsTests, test_yaw_gap_tolerance) 231 | { 232 | // This test verifies that poses do not exceed the yaw_gap_tolerance 233 | nav_msgs::msg::Path path; 234 | 235 | // Initial pose is at origin 236 | geometry_msgs::msg::PoseStamped pose; 237 | pose.pose.position.x = 0.0; 238 | pose.pose.position.y = 0.0; 239 | pose.pose.orientation.w = 1.0; 240 | path.poses.push_back(pose); 241 | 242 | // Next pose is back just a bit 243 | pose.pose.position.x = -0.05; 244 | pose.pose.position.y = -0.01; 245 | path.poses.push_back(pose); 246 | 247 | // Add remaining poses 248 | for (size_t i = 0; i < 8; ++i) 249 | { 250 | pose.pose.position.x = -0.1; 251 | pose.pose.position.y = -0.01 + i * 0.05; 252 | path.poses.push_back(pose); 253 | } 254 | 255 | // Update final pose to have a rotation 256 | { 257 | double final_yaw = 1.0; 258 | path.poses.back().pose.orientation.z = sin(final_yaw / 2.0); 259 | path.poses.back().pose.orientation.w = cos(final_yaw / 2.0); 260 | } 261 | 262 | // Apply orientations 263 | path = addOrientations(path); 264 | 265 | // Last orientation should be unaltered 266 | EXPECT_EQ(0.0, path.poses.back().pose.orientation.x); 267 | EXPECT_EQ(0.0, path.poses.back().pose.orientation.y); 268 | EXPECT_FLOAT_EQ(1.0, tf2::getYaw(path.poses.back().pose.orientation)); 269 | 270 | // Initial pose orientation should be way back 271 | EXPECT_EQ(0.0, path.poses.front().pose.orientation.x); 272 | EXPECT_EQ(0.0, path.poses.front().pose.orientation.y); 273 | EXPECT_FLOAT_EQ(-2.9441972, tf2::getYaw(path.poses.front().pose.orientation)); 274 | 275 | // Second pose orientation should be straight back 276 | EXPECT_EQ(0.0, path.poses[1].pose.orientation.x); 277 | EXPECT_EQ(0.0, path.poses[1].pose.orientation.y); 278 | EXPECT_FLOAT_EQ(3.1415927, tf2::getYaw(path.poses[1].pose.orientation)); 279 | 280 | // Other orientations should be up 281 | for (size_t i = 2; i < 9; ++i) 282 | { 283 | EXPECT_EQ(0.0, path.poses[i].pose.orientation.x); 284 | EXPECT_EQ(0.0, path.poses[i].pose.orientation.y); 285 | EXPECT_FLOAT_EQ(1.5707964, tf2::getYaw(path.poses[i].pose.orientation)); 286 | } 287 | 288 | // Now filter (without a max separation distance) 289 | nav_msgs::msg::Path filtered_path; 290 | filtered_path = applyOrientationFilter(path, 0.1, 10.0); 291 | // Removes lots of poses 292 | EXPECT_EQ(3, static_cast(filtered_path.poses.size())); 293 | 294 | // Now filter with decent max separation distance 295 | filtered_path = applyOrientationFilter(path, 0.1, 0.15); 296 | // Removes not so many poses 297 | EXPECT_EQ(7, static_cast(filtered_path.poses.size())); 298 | } 299 | 300 | int main(int argc, char** argv) 301 | { 302 | testing::InitGoogleTest(&argc, argv); 303 | return RUN_ALL_TESTS(); 304 | } 305 | --------------------------------------------------------------------------------