├── .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 |
--------------------------------------------------------------------------------