From c5fad62214bdb83b7683dbfb1139013634dd0606 Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Mon, 2 Aug 2021 14:30:52 +0200 Subject: [PATCH 001/156] Initial public release --- CMakeLists.txt | 110 ++ README.md | 218 +++ cfg/Pid.cfg | 64 + doc/figures/controller.ipe | 497 +++++++ doc/figures/controller.png | Bin 0 -> 33705 bytes doc/figures/flowchart.ipe | 338 +++++ doc/figures/flowchart.png | Bin 0 -> 75389 bytes doc/figures/mpc.ipe | 370 +++++ doc/figures/mpc.png | Bin 0 -> 29720 bytes doc/figures/rqt_path_tracking_pid.png | Bin 0 -> 100061 bytes doc/figures/tracking.ipe | 339 +++++ doc/figures/tracking.png | Bin 0 -> 20201 bytes doc/figures/tracking_base_link.ipe | 398 ++++++ doc/figures/tracking_base_link.png | Bin 0 -> 51732 bytes doc/figures/tricycle_model.ipe | 356 +++++ doc/figures/tricycle_model.png | Bin 0 -> 25078 bytes include/path_tracking_pid/controller.hpp | 368 +++++ .../path_tracking_pid_local_planner.hpp | 181 +++ launch/path_tracking_pid_mbf.launch | 10 + maps/grid.png | Bin 0 -> 1217 bytes maps/grid.yaml | 6 + msg/PidDebug.msg | 12 + msg/PidFeedback.msg | 2 + package.xml | 44 + param/controllers.yaml | 18 + param/path_tracking_pid.yaml | 38 + path_tracking_pid_plugin.xml | 12 + src/controller.cpp | 1230 +++++++++++++++++ src/path_tracking_pid_local_planner.cpp | 635 +++++++++ test/__init__.py | 0 test/local_costmap_params.yaml | 41 + test/paths.py | 67 + test/test.rviz | 238 ++++ test/test_path_tracking_pid.py | 130 ++ test/test_path_tracking_pid.test | 67 + test/test_path_tracking_pid_accel.py | 88 ++ test/test_path_tracking_pid_preempt.py | 91 ++ test/test_path_tracking_pid_turn_skip.py | 87 ++ trajectories/back_and_forth.yaml | 228 +++ trajectories/coverage.yaml | 228 +++ trajectories/coverage_actiongoal.yaml | 241 ++++ trajectories/coverage_map.yaml | 228 +++ trajectories/double_points.yaml | 245 ++++ trajectories/empty.yaml | 7 + trajectories/empty_frame_ids.yaml | 92 ++ trajectories/test_path.yaml | 92 ++ trajectories/zigzag.yaml | 143 ++ 47 files changed, 7559 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100755 cfg/Pid.cfg create mode 100644 doc/figures/controller.ipe create mode 100644 doc/figures/controller.png create mode 100644 doc/figures/flowchart.ipe create mode 100644 doc/figures/flowchart.png create mode 100644 doc/figures/mpc.ipe create mode 100644 doc/figures/mpc.png create mode 100644 doc/figures/rqt_path_tracking_pid.png create mode 100644 doc/figures/tracking.ipe create mode 100644 doc/figures/tracking.png create mode 100644 doc/figures/tracking_base_link.ipe create mode 100644 doc/figures/tracking_base_link.png create mode 100644 doc/figures/tricycle_model.ipe create mode 100644 doc/figures/tricycle_model.png create mode 100644 include/path_tracking_pid/controller.hpp create mode 100644 include/path_tracking_pid/path_tracking_pid_local_planner.hpp create mode 100644 launch/path_tracking_pid_mbf.launch create mode 100644 maps/grid.png create mode 100644 maps/grid.yaml create mode 100644 msg/PidDebug.msg create mode 100644 msg/PidFeedback.msg create mode 100644 package.xml create mode 100644 param/controllers.yaml create mode 100644 param/path_tracking_pid.yaml create mode 100644 path_tracking_pid_plugin.xml create mode 100644 src/controller.cpp create mode 100644 src/path_tracking_pid_local_planner.cpp create mode 100644 test/__init__.py create mode 100644 test/local_costmap_params.yaml create mode 100644 test/paths.py create mode 100644 test/test.rviz create mode 100755 test/test_path_tracking_pid.py create mode 100644 test/test_path_tracking_pid.test create mode 100755 test/test_path_tracking_pid_accel.py create mode 100755 test/test_path_tracking_pid_preempt.py create mode 100755 test/test_path_tracking_pid_turn_skip.py create mode 100644 trajectories/back_and_forth.yaml create mode 100644 trajectories/coverage.yaml create mode 100644 trajectories/coverage_actiongoal.yaml create mode 100644 trajectories/coverage_map.yaml create mode 100644 trajectories/double_points.yaml create mode 100644 trajectories/empty.yaml create mode 100644 trajectories/empty_frame_ids.yaml create mode 100644 trajectories/test_path.yaml create mode 100644 trajectories/zigzag.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..d6258a64 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,110 @@ +cmake_minimum_required(VERSION 3.0.2) +add_compile_options(-std=c++17) +project(path_tracking_pid) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED + COMPONENTS + actionlib + actionlib_msgs + dynamic_reconfigure + geometry_msgs + mbf_costmap_core + mbf_msgs + message_generation + nav_core + nav_msgs + pluginlib + roscpp + roslint + rospy + rostest + std_msgs + std_srvs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs +) + +add_message_files( + FILES + PidDebug.msg + PidFeedback.msg +) + +generate_dynamic_reconfigure_options( + cfg/Pid.cfg +) + +generate_messages( + DEPENDENCIES + actionlib_msgs + geometry_msgs + nav_msgs + std_msgs +) + +catkin_package( + CATKIN_DEPENDS + actionlib + actionlib_msgs + dynamic_reconfigure + geometry_msgs + mbf_costmap_core + mbf_msgs + message_runtime + nav_msgs + pluginlib + roscpp + std_msgs + std_srvs + tf2_geometry_msgs + tf2_ros + visualization_msgs +) + +add_library(${PROJECT_NAME} src/controller.cpp src/${PROJECT_NAME}_local_planner.cpp) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake +) + +# Configure roslint for nodes + +# Roslint cpp +set(ROSLINT_CPP_OPTS "--filter=-legal/copyright") +roslint_cpp() + +install( + TARGETS + ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +# Install files +install( + FILES + README.md + ${PROJECT_NAME}_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Install directories +install( + DIRECTORY + doc + launch + param + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + add_rostest(test/test_path_tracking_pid.test ARGS rviz:=false reconfigure:=false) +endif() diff --git a/README.md b/README.md new file mode 100644 index 00000000..787774ca --- /dev/null +++ b/README.md @@ -0,0 +1,218 @@ +# path_tracking_pid + +## Overview + +Path Tracking PID offers a tuneable PID control loop decouling steerting and forward velocity. The forward velocity is generated in an open loop fashion by using target velocities and accelerations. + +One of the tracking options uses a carrot of length l in front of the robot to determine the steering action based on the lateral error between the current Global Point (GP) and the Control point (CP): + +![Tracking carrot](doc/figures/tracking.png) + +If a smooth path is provided, the controller has the option to track the path with the base_link directly instead of lagging behind a carrot. In this case a Projected Global Point (PGP) is computed as well which is tracked by the CP. In this mode, the yaw error can also be used as control input. + +![Tracking Base Link](doc/figures/tracking_base_link.png) + + +The PID contains two closed loops: Lateral and angular loops, and an open loop: Longitudinal. For non-holonomic robots, the lateral and angular loops can be combine to compute steering actions. + +## Model Predictive Control + +As an optional feature, we use a Model Predictive Control (MPC) concept to regulate the forward velocity. The user can specify a maximum desired error bound to track the path. Using the model of the robot plus the controller settings, a predicted path is calculated and if the error bound is violated, the forward velocity of the robot is decreased until the predicted path lies within acceptable bounds. + +![MPC](doc/figures/mpc.png) + +## Tricycle model + +The tricycle model is supported as well. + +![Tricycle Model](doc/figures/tricycle_model.png) + +It is assumed the front wheel is steered and driven. The steered wheel link (SL) can be asymmetrically placed with respect to the BL. Limitations of steering and actuation are also taken into account. +## Anti collision + +When used as a plugin, Path Tracking PID uses the costmap (and the robot footprint from the costmap) for anti-collision. No evasive maneuvers will be preformed (sticking to the path is key). However when obstacles are detected in close proximity to the robot or in the direct line of direction of the robot, the forward velocity will be decreased. This results in safer velocities around obstacles. If an obstacle cannot be avoided or is too close to the robot, a standstill will occur and the navigation is cancelled. + +The maximum velocity of the robot will be scale linear with the costmap values around or in path of the robot. One can use the inflation layers (or even social layers) to construct these gradients in the costmap. + +**Keywords:** tracking, pid, local_planner, trajectory, model predictive control + +### License + +TBD + +**Author: Cesar Lopez, cesar.lopez@nobleo.nl.** + +**Maintainer: Cesar Lopez, cesar.lopez@nobleo.nl.** + +**Affiliation: [Nobleo Projects](https://www.nobleo.nl)** + + +The path_tracking_pid package has been tested under [ROS] Melodic and Ubuntu 18.04. + +[![Build Status](https://bitbucket.org/nobleo/path_tracking_pid/branch/develop)](https://bitbucket.org/nobleo/path_tracking_pid/branch/develop) + + +## Installation + +### Building from Source + +#### Dependencies + +- [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics), + +#### Building + +To build from source, clone the latest version from this repository into your catkin workspace and compile the package using + + cd catkin_workspace/src + git clone https://bitbucket.org/nobleo/path_tracking_pid.git + cd ../ + catkin_make + +### Unit Tests + +Run the unit tests with `catkin run_tests path_tracking_pid` + +## Usage + +The path_tracking_pid is a plugin for [move_base_flex](http://wiki.ros.org/move_base_flex). + +To run move_base_flex with path_tracking_pid plugin: + + roslaunch path_tracking_pid path_tracking_pid_mbf.launch + +## Static parameters + +* **`base_link_frame`** (string, default: `base_link`) Name of the base link frame. + +* **`holonomic_robot`** (bool, default: `false`) True for an holonomic robot. --> Unmaintained, expect bugs + +* **`estimate_pose_angle`** (bool, default: `false`) Whether to take the pose angles from the path directly or to estimate them form consecutive path poses. + +* **`use_tricycle_model`** (bool, default: `false`) True for using tricycle model instead of differential drive. + +* **`steered_wheel_frame`** (string, default: `steer`) Name of the steered wheel frame. + +* **`use_mpc`** (bool, default: `false`) True for using MPC to regulate x velocity. + +## Configuring in RQT + +Tracking_pid parameters are all available through (rqt_)dynamic_reconfigure. The main parameters are: + +* **`l`** (double, default: `0.5`) Following distance from robot's rotational point to trajectory. +* **`track_base_link`** (bool, default: `false`) Whether to track the path using the base_link instead of the control point ahead. A smooth path is needed. + +Proportional, Integral and Derivative actions for the two closed loops: Lateral and angular loops. + +* **`Kp_lat`** (double, default: `1.0`) Proportional action gain for lateral loop. +* **`Ki_lat`** (double, default: `0.0`) Integral action gain for lateral loop. +* **`Kd_lat`** (double, default: `0.3`) Derivative action gain for lateral loop. +* **`Kp_ang`** (double, default: `1.0`) Proportional action gain for angular loop. +* **`Ki_ang`** (double, default: `0.0`) Integral action gain for angular loop. +* **`Kd_ang`** (double, default: `0.3`) Derivative action gain for angular loop. + +Each loop can be enabled/disabled separetly. + +* **`feedback_lat`** (boolean, default: `true`) Enable feedback lateral loop. +* **`feedback_ang`** (boolean, default: `false`) Enable feedback angular loop. + +Moreover, feedforward using trajectory velocity can be enabled/disabled. + +* **`feedforward_lat`** (boolean, default: `true`) Enable velocity feedforward for lateral loop. +* **`feedforward_ang`** (boolean, default: `false`) Enable velocity feedforward for angular loop. + +Target velocities and accelerations for generating the open loop forward velocity: + +* **`target_x_vel`** (double, default: `2.0`) Nominal target forward velocity. +* **`target_end_x_vel`** (double, default: `0.0`) Target forward velocity at the end of the path. +* **`target_x_acc`** (double, default: `2.0`) Desired acceleration at the beginning of the path. +* **`target_x_decc`** (double, default: `2.0`) Desired decceleration at the end of the path. + +Constraints on the generated velocities: + +* **`abs_minimum_x_vel`** (double, default: `0.025`) Minimum velocity of the vehicle, used to reach the very end of the path. +* **`max_error_x_vel`** (double, default: `1.0`) Maximum allowed x velocity error. +* **`max_yaw_vel`** (double, default: `2.0`) Maximum allowed yaw velocity +* **`max_yaw_acc`** (double, default: `2.0`) Maximum allowed yaw acceleration +* **`min_turning_radius`** (double, default: `0.0`) Minimum turning radius of the vehicle. + +Constraints on the steered wheel for the tricycle model: + +* **`max_steering_angle`** (double, default: `3.1416`) Maximum steering angle for tricycle model. + +* **`max_steering_x_vel`** (double, default: `3.0`) Maximum steering x velocity for tricycle model. + +* **`max_steering_x_acc`** (double, default: `2.0`) Maximum steering x acceleration for tricycle model. + +* **`max_steering_yaw_vel`** (double, default: `0.5`) Maximum steering yaw velocity for tricycle model. + +* **`max_steering_yaw_acc`** (double, default: `0.5`) Maximum steering yaw acceleration for tricycle model. + +Anti-collision parameters: + +* **`collision_look_ahead_length_offset`** (double, default: `1.0`) Offset in length to project rectangle collision along path. +* **`collision_look_ahead_resolution`** (double, default: `1.0`) Spatial resolution to project rectangle collision along path. + +Debug topic enable: + + * **`controller_debug_enabled`** (boolean, default: `false`) Enable debug topic. + +Parameters to configure MPC behavior: + +* **`mpc_max_error_lat`** (double, default: `0.5`) MPC maximum allowed lateral error. +* **`mpc_min_x_vel`** (double, default: `0.5`) MPC minimum absolute forward velocity. +* **`mpc_simulation_sample_time`** (double, default: `0.05`) MPC simulation sample time MPC maximum allowed iterations forward in time. +* **`mpc_max_fwd_iterations`** (int, default: `200`) Prediction iterations. Total simulation time will be then mpc_max_fwd_iterations*mpc_simulation_sample_time. +* **`mpc_max_vel_optimization_iterations`** (int, default: `5`) MPC maximum allowed velocity optimization iterations. + +![RQT reconfigure Tracking PID](doc/figures/rqt_path_tracking_pid.png) + +## Launch files + +* [launch/path_tracking_pid_mbf.launch](launch/path_tracking_pid_mbf.launch): Runs move_base_flex with path_tracking_pid plugin + +* [test/test_path_tracking_pid.launch](test/test_path_tracking_pid.launch): If all is well, a red square moves along a toothed path that covers a square. The square should be followed by the base_link frame. + +## Plugin +### path_tracking_pid/TrackingPidLocalPlanner +For use in move_base_flex. See launch/path_tracking_pid_mbf.launch to see an example. + +#### Subscribed Topics + +* **`path`** ([nav_msgs/Path]) + The path to follow. + +* **`odom`** ([nav_msgs/Odometry]) + Robot odometry. + +* **`/tf`** ([tf2_msgs/TFMessage]) + The position of the robot wrt. the frame in which the map is received. + +* **`vel_max`** ([std_msgs/Float64]) + Dynamic maximum velocity. To be used for example when maximum power demands are reached. + Higher level software could also use the reconfigure interface for this and set new speed values, but when required in a feedback-loop, this streaming topic is preferred. + +#### Published Topics + +* **`cmd_vel`** ([geometry_msgs/Twist]) + The Twist the robot should realize to track the desired path. + +* **`feedback`** ([path_tracking_pid/PidFeedback]) + Estimated duration remaining and progress towards final goal. + +* **`visualization_marker`** ([visualization_msgs/Marker]) + A Marker indicating the current control goal of the towards. + +* **`collision_footprint_marker`** ([visualization_msgs/Marker]) + A Marker indicating the footprint(s) along the path that are used for ahead collision detection. + +* **`debug`** ([path_tracking_pid/PidDebug]) + Intermediate values of the PID controller. Topic is silent by default, can be enabled via dynamic_reconfigure. + + +## Bugs & Feature Requests + +Please report bugs and request features using the [Issue Tracker](https://bitbucket.org/nobleo/path_tracking_pid/). + +[ROS]: http://www.ros.org +[rviz]: http://wiki.ros.org/rviz diff --git a/cfg/Pid.cfg b/cfg/Pid.cfg new file mode 100755 index 00000000..5b0a4e0a --- /dev/null +++ b/cfg/Pid.cfg @@ -0,0 +1,64 @@ +#!/usr/bin/env python +PACKAGE = "path_tracking_pid" + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t + +gen = ParameterGenerator() + +gen.add("l", double_t, 0, "Distance between axle and control point", 0.5, -10, 10) +gen.add("target_x_vel", double_t, 0, "Target forward velocity", 2.0, -10, 10) +gen.add("target_end_x_vel", double_t, 0, "Target forward velocity", 0.0, -10, 10) +gen.add("target_x_acc", double_t, 0, "Target forward acceleration", 2.0, 1e-9, 10) +gen.add("target_x_decc", double_t, 0, "Target forward decceleration", 2.0, 1e-9, 10) +gen.add("abs_minimum_x_vel", double_t, 0, "Absolute minimum velocity for reaching target", 0.025, 0.0, 0.5) +gen.add("max_error_x_vel", double_t, 0, "Maximum allowed error in forward velocity", 1.0, 1e-9, 10) +gen.add("max_x_vel", double_t, 0, "Maximum x velocity, used for scaling purposes", 5.0, 0, 10) +gen.add("max_yaw_vel", double_t, 0, "Maximum yaw velocity", 2.0, 0, 10) +gen.add("max_yaw_acc", double_t, 0, "Maximum yaw acceleration", 2.0, 0, 10) +gen.add("min_turning_radius", double_t, 0, "Minimum turning radius", 0.0, 0.0, 100) +gen.add("track_base_link", bool_t, 0, "Should the controller track the path using the base_link frame?", False) + +gen.add("init_vel_method", int_t, 0, "Initial velocity method", 1, 0, 3, edit_method=gen.enum([ + gen.const("Zero", int_t, 0, "Always start from zero"), + gen.const("InternalState", int_t, 1, "Last internal state is new initial state"), + gen.const("Odom", int_t, 2, "Start from current odometry value") + ], "Initial velocity method enum")) + +gen.add("Kp_lat", double_t, 0, "Kp Lateral", 1, 0, 10) +gen.add("Ki_lat", double_t, 0, "Ki Lateral", 0, 0, 2) +gen.add("Kd_lat", double_t, 0, "Kd Lateral", 0.3, 0, 10) + +gen.add("Kp_ang", double_t, 0, "Kp Angular", 1, 0, 10) +gen.add("Ki_ang", double_t, 0, "Ki Angular", 0, 0, 2) +gen.add("Kd_ang", double_t, 0, "Kd Angular", 0.3, 0, 10) + +gen.add("feedback_lat", bool_t, 0, "Enable lateral feedback?", True) +gen.add("feedback_ang", bool_t, 0, "Enable angular feedback?", False) + +gen.add("feedforward_lat", bool_t, 0, "Enable lateral feedforward?", False) +gen.add("feedforward_ang", bool_t, 0, "Enable angular feedforward?", False) + +gen.add("controller_debug_enabled", bool_t, 0, "Debug controller intermediate gains", False) + +gen.add("use_mpc", bool_t, 0, "Limit forward velocity based on predictions of the lateral error", False) +grp_mpc = gen.add_group("mpc_group", type="hide") +grp_mpc.add("mpc_simulation_sample_time", double_t, 0, "MPC simulation sample time", 0.05, 1e-9, 10) +grp_mpc.add("mpc_max_error_lat", double_t, 0, "MPC maximum allowed lateral error", 0.5, 1e-9, 10) +grp_mpc.add("mpc_max_fwd_iterations", int_t, 0, "MPC maximum allowed iterations forward in time", 200, 0, 1000000) +grp_mpc.add("mpc_min_x_vel", double_t, 0, "MPC minimum absolute forward velocity", 0.5, 1e-9, 10) +grp_mpc.add("mpc_max_vel_optimization_iterations", int_t, 0, "MPC maximum allowed velocity optimization iterations", 5, 1, 1000) + +grp_tricycle = gen.add_group("Tricycle") +grp_tricycle.add("max_steering_angle", double_t, 0, "Maximum steering angle for tricycle model", 3.1416, 0, 3.1416) +grp_tricycle.add("max_steering_x_vel", double_t, 0, "Maximum steering x velocity for tricycle model", 3.0, 0, 10) +grp_tricycle.add("max_steering_x_acc", double_t, 0, "Maximum steering x acceleration for tricycle model", 2.0, 0, 10) +grp_tricycle.add("max_steering_yaw_vel", double_t, 0, "Maximum steering yaw velocity for tricycle model", 0.5, 0, 10) +grp_tricycle.add("max_steering_yaw_acc", double_t, 0, "Maximum steering yaw acceleration for tricycle model", 0.5, 0, 10) + +gen.add("anti_collision", bool_t, 0, "Stop on lethal obstacles", False) +collision_group = gen.add_group("collision_group", type="hide") +collision_group.add("obstacle_speed_reduction", bool_t, 0, "Slow down on near obstacles", True) +collision_group.add("collision_look_ahead_length_offset", double_t, 0, "Offset in length to project rectangle collision along path", 1.0, 0, 5) +collision_group.add("collision_look_ahead_resolution", double_t, 0, "Spatial resolution to project rectangle collision along path", 1.0, 0, 10) + +exit(gen.generate(PACKAGE, "path_tracking_pid", "Pid")) diff --git a/doc/figures/controller.ipe b/doc/figures/controller.ipe new file mode 100644 index 00000000..138457cd --- /dev/null +++ b/doc/figures/controller.ipe @@ -0,0 +1,497 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +256 704 m +256 656 l +320 656 l +320 704 l +h + +PID + +208 680 m +256 680 l + +e_\theta + +208 680 m +256 680 l + +v_x + +256 704 m +256 656 l +320 656 l +320 704 l +h + +PID + +208 680 m +256 680 l + + +208 680 m +256 680 l + +\omega +Controller + +8 0 0 8 360 680 e + ++ ++ + +8 0 0 8 360 680 e + ++ ++ +\dot{\tilde{y}} + +400 636 m +400 684 l +416 684 l +416 636 l +h + + +416 660 m +440 660 l + +twist + +224 624 m +224 684 l +208 684 l +208 624 l +h + + +176 680 m +208 680 l + +GP + +176 680 m +208 680 l + +CP + +216 684 m +216 708 l +360 708 l +360 688 l + + +216 636 m +216 612 l +360 612 l +360 632 l + + +8 0 0 8 360 680 e + ++ ++ + +368 680 m +396 680 l +396 664 l + + +368 628 m +396 628 l +396 648 l + + +368 680 m +400 680 l + + +256 704 m +256 656 l +320 656 l +320 704 l +h + +$\dot{x}$ generator + +340 732 m +416 732 l +416 680 l +436 680 l + +e_y +\dot{\tilde{\theta}} + +256 704 m +256 656 l +320 656 l +320 704 l +h + +PID + +208 680 m +256 680 l + +e_\theta + +208 680 m +256 680 l + + +256 704 m +256 656 l +320 656 l +320 704 l +h + +PID + +208 680 m +256 680 l + + +208 680 m +256 680 l + +\omega + +8 0 0 8 360 680 e + ++ ++ + +8 0 0 8 360 680 e + ++ ++ +\dot{\tilde{y}} + +436 468 m +436 536 l +452 536 l +452 468 l +h + + +416 660 m +440 660 l + +twist + +224 624 m +224 684 l +208 684 l +208 624 l +h + + +176 680 m +208 680 l + +GP + +176 680 m +208 680 l + +CP + +216 684 m +216 708 l +360 708 l +360 688 l + + +216 636 m +216 612 l +360 612 l +360 632 l + + +368 500 m +436 500 l + + +256 704 m +256 656 l +320 656 l +320 704 l +h + + +336 552 m +412 552 l +412 528 l +432 528 l + +e_y +\dot{\tilde{\theta}} +v_x +$\dot{x}$ generator + +368 448 m +416 448 l +416 476 l +436 476 l + +v_y +non-holonomic +holonomic + + diff --git a/doc/figures/controller.png b/doc/figures/controller.png new file mode 100644 index 0000000000000000000000000000000000000000..43df506c478c99ae7df571229f22115e0369679e GIT binary patch literal 33705 zcmdSBc{r8*`!>2HMJ1vrN@*aOXJx8Ik*Q=VV?{EPA#+qKGDU<8p)!xD%u`e($vlKm zGE-!p-*Y|B{=LWE`~Bmck9Tk1<9j?kPu9Ke`}6r+=XGA^bzZmMMKuN5Z7kae1On}O zMfpnv!X{z--;#P0{!8@a13LU;%XMW1dBO(ye+gwNVFbbxhR(%{AVp zX$;5q9h_N<`?HkKuOa-bH>cujl81B1bBnO#H#UdsDNrq|F)=ZZ^cJ#!5`LrfJyrmij$>*!lW~vbuUx z!kPO_{D%6MFDvF%IE}X#ISfC|ih6ML=uv!icu2?;E_YD&POCYu_wfPB)~1vT@qs}> zH0&?W1^EXBWxZ83G>m)m=1q3Cjgr!~_y%!7!AgETtvG4-b89zM6ctm)bqSA2#vG|J zsj0^q8N)g|JEh&{^(*d_8CBc~7t^?Sv9rLo?{he}Lg3z8i&H;1FT{!5de#;rd~>I} z+M~I#wv1j&3i;5ZEmWr+7#JCaJr<{a{`_fN^Qb;vwxW7|=gyt+GM@I1j#l@&#(s@= zz=~gdA;*WN;ZAvWz*^DzL}Ys{&HI>yrn}_E^;qmHEvI`u$$CH z@Vy=3^@cn2f#*RNluU!JOWSbzNZF)J&J z^?B-Y|IE_*`nrk^1)=SX(3yG}ul3dbN=o9VsUPoen3#+;y>EOjH_DeDcl%H8fddEX z9rq9}9tf6c1*Kl%GNJxuBlNdw*3I-DXGFdop|Dwn;5AL;|yTp{ru(2ubCxNQ&UpNXiJLt)BrX0{H5SL51GEYI?KPmYH!`Tb?;bI zRFv21^j1-U*UJa5TFWq>W|YG=X=-Vu$(mfbaz*5J39c(GT^1M^=$~v9Ym+Io;j6cU!M%BuRJ3L~WLQ{pOAP?8xJ*t|}^DB6#%> zJLXFgq7j<)U5$+wRaNV&s;ZR4SC?i|{QdpkzMUOzh#y}lBJ90uaxXe9Elu6)Q>udu zZOx$l+}vDE;64q{H@JE36DPE#j{e*j4$cb-QcYsL)H0iA+gIM4mv@2VjPa2pFD<$n z=WdNfs~kIaOjlQzkB_fyqV?m)2lTIR4OAbR38$%-*4NzS{CRZFi&~B@OE@{LuP*T$m21i# zj%T4@fBLHQ`gzAal&6mzWo*sVNKtFuUL1-etxTfF?u^B#AK~XSD%aP#?>{hp8~{L~ zXA9QZVAzO!^hoXB>zxf6V)n32-L2)S#h9V9QABup504+T|757!^zz)eoAdRXH#uDn z?Addc+8oD!F6KBF*DfNP?42)z_T1KuxyCha9U7c?(zeW`@qyMoPIXh>6qFy*#mpKH z5f#s!JC`J_`JbzmXcM-Ae zn>KA4%UxPqS@5Mh$mgQ`{gBqhix=0l?^P7tV@+pi3t^WVD`3|+=v#a(pS^bTKi|AZ zSy_3^A}k}rQe9o$K)k|b`moaicaqn}Iw>Sx)=RrW%f^O}Jy9pea4zOtes<~=%kjHu z!Ey&aB_}2}HnS4k4@D*9tHnvBWoO4lMC4;tPgw)?XS5Iq~0G5Jr&PbL>D6w@vmcC*WZ7&`1ZEO-O>GVT=e^JH?0evMn-+Q zHldM`H!Un!4j$w$eN|X^Sx(NlG2u*`t)%C&Pnoh&VgM51U#Z>u8d z;X~>l4{++AopMSae{?)(?N+*O-N3XPpe9`_Ln%d`VR3QV_#=(4#SHn3Ae&JN3%`jG zw@pt?<zS!Ej%wy@Vy*OL#r%x`fuF^kck81 zNli<;RiuCN=>GH+3Gc}Tn9`0TO^KW|MhmRlg^75z-rYz@e^6~MxzlC;rJf{Mu&TW2ztMKe-eT=x8 zni@V2U}DfGX4H^z`TFWc?5!=S+-$)MK&p_CkdcuQWLl(yeCuxH!kznsK2%rxM0+Nj z>nrz^#1Y8SE5J8>{P00y|D&R!qWkyngD!on20&bhm&q#BpR&~deL?m{|GT?_f`V1b zz(x;G&$L}r2-L~mvJxcbsjSyplV;!=&!0aJTE6vba4?P4IKxO!?fiMaiV819Ma4UJ z$`cb!Z~`Lv48Z5Cu3b|fV6((|jb;d?ArLyLjvOKoybVHB2!wZhiW~&ORpz6N1j4KB z|Nd?5W&&a2o(bsZ?K46Y1VYK7sQ*v=`eTz~0zDTPi(s_+>8dO)}M`UgKo&epcJveMG0e47ag zqp$d)jvhOP4E;PiTTR(kNJxl@iRrE5$A@4_;9?5$@&X3bgwrk8MqAoHel!nI(fztj z=myRcB3)Qq{AMvd!8`eVu`vgId$H3vfTI8>dz8o$nU-dL<_txPR_nKwg+Ebz6sJYl zqr6=KGhoFkI_ft;5>z4yhoVDN-bwNE=L|hn0sl5F-b`qk{WGotCP=$;XE|VD7Kz1=g!op#0D@b{j z_qPJMO4tmv&KeoT)7vl4j@rk#ySTLGF`yWuZtCc;O%p(CJtD)8LNZzFjdglI7+OaI?I5^QO7^mC@kbQC|JRDeOR=)t7;3 zwOEN;vm@$ydV0uEa=tt3ql5vJ3C-0FaneX1QNh8%VPP$(nI5r8pStx^{){Y60f^V! z-@hnrR4!ZqnjJjlc%1zxuyx|w8x$Yi<(}?E29e!Id3XYsBmDyd_lsJzKH<_xjbP?C zR8du(o1c#Z(5Gpnq&@(>G+DXlvR?3Q0&^Z%R@$%msj=7 znICwZds;w*3j4urb6tIX@k*JI{6-AK>&C`us^G|f|Nc$y`-OszL@z5_aqr$eQOov= zmhKC`pT);F49^S<3;>I%;(x#Qr5h~ooL*W5%u;RMd@<)jti+F@p>t)~Z->(Xe<${d z%2;&@RF1c2=`j-h0|Ka&Z~yM$;^(&o%_{XnR)~#_Eh%va_)eCj@0!cgahXA8Ue;<> zoa-nr@HYUh8yg*^VMcf&vqm#uur~J3O<*#>R$%?3BaM4t4;?F(IMQ)nQ+iX9u)&bohCB z*QN)yI&F2OG`WW>z9<2RO-4>smtTAFo(&mJXG*gE~Py^J?Uz(f0tIV^%}kv zq!578?eTW&@5N49bNb=9`5;#DOS1c}k{&;9iWaqen*HKz#?dGdQPDcyA9fP=I*_ik z)YZA1!$JxR3;q2+x1?OK?9=)p2{bWgBmN zPHEs^BA~3N7egNk9V0qgcR*TC*z}8p`@EIDeh-MhoSYnPI&~1x;p*x-GgQwQ_#EJI z|0*}|r_!=AxL{dzdEpPg%d@bsf&Tt?KkW4N^>NHI(s{YK&g z@H{TLZ{NOqUS>byGMWI8*kW27OR=E4xAplt+S?;WPv{n(>ND5V*XLnpSHE>KxF(}z z`r^m^>9jKOeeaY;(kp(Hwf$l0#|E(2%9-9D88O2>p0FJF`Ez-GQqTO2*ZR_MSDqyf z+h9aSdin`&?h5yXuZ@i%x}kA#*~!V;^=uh^@-9yjP$_9^Yg<`ml1TFA=4s@DEU^En zPu<`-d3iHSOXrcsGiqvloSeS8;bCD%I5|5I*_GP$w^rE+N5)pgZNLfuz&CE(sM-#w zgwzrg7&yY6nS0`S!FuwBIpiKZyqsNi-@ct(eok{@A0b_R*u?(!Z9Xtm zaa9X>d3km9$8X*Udp;1=J~rFlH!DlKb?a8*C(v3Fvsew6(XpdPRa8_EQzu>jn(W&b z1&N{u4-aytmDkbSY|)k;j$;-Wr~*D{ZEam~XH6*!98yS3%&_Ek7C#ov<|n&7 zJw0(@ZZ57O=Rdu`2>nu*T!>PdD>y7z?#XX&YQBF*s`vwlW+!QO2@^CGTqH;l5lrlf zsi}&uNG!J+v-HG&n6ir7hzJQSFU@eyiHVClW5Z&^tkV^Av_?B~jgw+5EiF6p7%aM2 zd3m$WrB_t$3{jHlakbJvB<1!Y+rBso(ihZ zC_~kNpjca;3#c0$8XDp_b}Y-Nq8zdK_Rm9F0kPmKxZC#jb`_nb<~7NveA_-giD6U* z&KA^v?on+ocK)NryI#a z>pPK$czAiEvNw63SL8@&Dfe7?%11$nmX58b{GTJ%0=9;2a1F}emvTi z!Q7%n(A-(8d4Mlc(zrTspRg&JGuGpbFQ8ln{v~gxNZlNowgq|~v=}Ikr{CZX|1NnX zXAn~NXAA$2@%(6R4ULw2_cq}o2?s-I5{iTb1s_L5m==*xF0w}M3y zUBb)C))0$NPpJI5)+^;b1R)||ltCU0{Tm8naqF&DpfeErYrO0R6sCK5V44txr)FnO zUJ-Zs7;_N3l@^zmzdjX|14{Dq^UKO^Km)6?1SdPj&0VKyeVDFxb0g1L1%+T1(a*?m zEiE(9fFKuMD?a=foDHe3r^MNosOaJ%!EOSvp^6rgF21!|8*g zX=*mNv_$giRXd(NoCF1G>S{iO2I43DJ&Dy3;u0`*hi1D_|0%}PZ%oY1&r_SDMk{rl zd76E8GuQFsflztq0kifqL#jBtG>jb7;?NETK&1DjtIe`U5u!6@XB}l_Wr+{d(nkMO zY3TRH9Po2b6Xpk zFmQ3XLfMV1gT8;Y`1mgQpxr4xMC!zp6iYKRYJR`q;0okmNF`tdJ>S2hT2D_&S%9V~ zE?&UFtD|`+O4lC@1#D*Ei;JB6xpU~B=wqq({TO5Qxz+ zF)_%jYDD7oiOyWb>2z~=ihWX2#}yP2dHFNkX-DrLipJ6S0yoP6tjck7w?^yxRZl{o+5xDcnZ<{;(z zH_FTm9%j_oq2Au1#O)n4fN+EJqb&*-F68_a@T`>mfcnct%CM;C zp4(}mT%wZF=f_7Bl$4cSrFt9UWog(^bb<647xP^`veP##I(qESAK9B<_Y*%MAz8{E zH^#$2ar2NddWA?V0*ep2luB0T9gxej+1o}?Po6q;3R*_1o=iNy2MfIgUF`<}zR2;R zq1%wj@t{wmqSUwJgvcP)q1XxOvA)u9b2}AW)6&twN%}QBTm#*gL~6Cy1HT7N+oLB#w3 z_8-Ir1cZS{TujWgw$bD&yOWa>ItnMEP=KP==>fXv+UWfzv8fG;>Yq7NiGG%0smrbd z2iC^gxGRHh-257Q1t&{O?_+g!wqaRUYwMG2A*kb$w`{BeVq+(M|IW_HkaQSQ0rw9Yb6uWI3l6>jiBn#lWH*4K16AFY&6^!3It2v; zrgAFQPuc&X)(EE@Kk@$6V zS?4FdwID%2dh}df6qJ_!-r5S)p=hz1lIO0;^)GlrN=nK;q~qUlJ}3~}-QB_Q#;hG3 zXZ`nZZ@%vZk_rIu_WPO;vFLHf&Ta~!4+2Yf;M7qGx`}~-BZcp32c{tomAcK1WgC!2J?^M8^GFTDAf=u^svbOg^ypqxBN{o7{;Bl77v4fa z@-m&Zj)Jsj&mKq$oX3t0g4U5wSh*B}?YIjsA0Lq54sIRk4LJe-{>bk-#rpOd6_sr} zcJ#ct*--Uh`<~sqfhFh!r>eii_Od^<h0J5Lc+q}p#bUPu3*Q#QZ9BEFE-+yh9N?uW)2NM~pZK_}7C(E*r|Is1c-i16_5W$qo& zu^Srx0QaDex7xFke`1iDpe5a&>`F>b-e%;IQGP)teq(l2gZy#ZKM?R}8eUQ$%%N7J zI~_mO-w+rWBDb^sKfC~eVVoB(05qf)0CDUb95U*Ke?BGFzs`K|;*!lt(ff7S1;D?Jwe^90`?$E4`ue2D zD%(z+I1&D6Yg+mRD8gv0O-)T<|JLTaZ2?X1sXQn+Ia3DfA|p#U&p?D*WSdO?-4PKI zB4PjQAq4G;ii-4fivgx>7o(V>Ret>V0oIqDyDz?h{E)m`t5EQV&u>(tio;Tod%g&^ zjiup(GVv8AwG3{~O;2|3*`tF#FSZ5U`qic3_`&w}mB1b-A!JG0X{k4Eipn;kB_=0HJ%% z+9Ectr?K%Bs#?HiRpd#E=yTguIf#lVxt=_EGBTDW)=->%P(wq5CFS$Y{i0`2O}?za zm)8Wb4v&r&xlCVY+DT`w)YR03N7vET&MuF_u?{l8sr}m0685Y^^vv5It-i+}1_vJ$ zF>55Zb7y9vvXi!;yTw5Qz}%mY^TE!75Sslcf-lI;o0*vGW@YUT zVbe_jDMBP<0=ZCy0?@zZTkGlSvb3BP6;|YEs{COGz>kx1eQotk2T)A@v(YJ$ zb2E!d^-6=xfJ0VXT+Fg>ANz~3#6iQG4A(T}lRyZotBZ?@(Dv&5_D#LuoZwGfk1W03 zo{aYOtwFX%2a09j^?uRJ(!nnml+NyEC-;;fQ$t!oXgw7)-V&s9frg!hg#|S6Mecj1 zhyJPfI>>;~L{lN2Az%*(TqU$P<+6nOu`Mkv?NF0I_8}V`{OhQ8?SWtk8dX^LIv0uq zH62CdlP43XkAd|dcGkwmEsB!i5fL0DJBiqHWAXNY%dK>5B4d2%(>Npk+o^WtA)O)) z0@{a(C1rife6L(v-(``O^!b zG+Wl;8Z%Q%24{xohz17m706kNVj7sx3EX&eK$4mrSsA<*`-&?kXWwPsy&EU`@|7#1 z3x55(ckc!hk8<1DjeE+<%-lTlhvrjlEwmIhbU2Y6@Yt%i1G4{-%spBrl)abr8c~0q z`(8IY+F~i3p7q$w++00)jsfxkU=RIR5!@KgH(B)l`>3ny)0Z!A^YaaJub(Y*Pv|)4 zwc6BK0!sKY+oxR*LQY(q;YTknL@)5>?AebHD{wSW6AaUS3ov^r90gK+VE68~I9QcN zTj41|hW1*X^;1ed3CjbjH*nPV@8uK}d@bG&OixTszIpu`d0W9)kT=Ti5U?%C?Viu4 zKB4o+!ul7Ke$i|VefI2GzvWg@N_GPr>ePLw5_(CL=BR^O<7*(sC6A}afa*iJDa+Q| z$I9AQ*~@jwe@@dV`|Bx z(kqwcPij^#zG{@^e(#GX;o<5Vm!hMiA&4QQY=1W}(ICf`x;hvXph8ZYrc+OnZV?l+ zMS@0|mLu^es;ta2s2fK~WPXsM340=j7{f<7IXHTe{Xak9;^mE?W=AcAW<40cX>rIo z6Em|yJTzjh`ojkvgW|W%-R3KED4R1{(Wqa z?dQ1Blct{=8ZM})?3U^}{SSOZK`a?nanH zamPAhVbSIq0=DwAv$G=!{xCrER8XRa^72odE>u~ENml3tVat#tl_Pk-(G%G&VGTe6 z#r`RG(B(-=N}@{fl9rM>(jlI%tj#zHG{SvRN zyTvU09zRxBQ+xFM`8AwMP)d-yv(E(dN?oQw?>E+0Y}3ERd##_m{(-vk#|m=d&w&B7 zF=Q=TQ<2`4+?f)FpPn}~G}O^qKy_HP{d-T(2NJaYrq}#e-#|1VD&pe(QJ3=0eBT?7 zq;;#mB%wjcmfn>d6log45IPoq8~osM)h&ejf=$tfU8`3b3Ke;k&^Hv(rg~ulZVL+^ z2MPUuIW4U@@GMdY-9ech2To}gUO{t@DhKIZ1VTCvqJu>2l{cnZeX`a^4xy%3_?Ykx zo3(x0Ha`8r)70nH)NEe!8(~jccO4c#Y#PvUf;c2i4ezAIpUeq-I5!{HeE-#SFy2P0*slmeG?nIe@->uVfd@#Wa>FY z)@^%xKu$CY9WYURf0s##O)W(2v1#W<7zw(%qI+fT(jSJ1f(rt?0sn<+1ACB7p(YeU z_(WP4HFRw(rDpq7J7|C8@L>=T(8(0lnA>BmT(HeSMu>@y24p^~8{8(FQ5*{i`Qio5 zlTfd*0MN0yCztx%6Ehr7o-6_j;<@rVvFFQ&!RpkDoUVwmkuKXLWJd^sZ?K}+Yzs?E zNl8g#Z6z~vFgN%MKmtk;7!XmrtnATa!UDOwx%tmqU%RLSzY??;bXq`__w<|;SUY~4>;dD%0_$$m8#g|-mb&KMn(9Au;J~wL7lBK4yY}sa ztX(%G5fWW9fK`~p!$4<6|EvYQObv|{qiK=?FV}6HIF$8$t$(2dp|@{lKlXjw7E$#l&djsH&*U|NdU5Z_xNqe(7o2^+SRN zy9ljUKuM-rwo~Oy%SLO>1JSQtJJZ1vaM{vT?k>UGg$LVg8D3}EKJv-I}{r8{G#aWW(xy~~#!$6EbHi3D%5 zR*h^oXu3<N?&S)AXaMiue;ax1{6>xDQ|%;3No6ZD&WW7p2`8D^0*^@pHX+8zG?{ zgc0|_S^A-;XMb428}#1DKbI~w%WkY{R6Jy;gvn*sz{tqny?Y^IoiF>CR;q-4XhUV? z-Lx|F?{)N#KTL(_i=0A77tC_dU!H7;=~m~QC56$X{5^qO{(O2%T5+-Kj49Ci;~>5n z)f)B&L?`XmVI2x|3PdQe%zM5F`e?X@hdzFMZHjoVC`KN+i0t7O5rH2!Sv7`3YSbmm z?PZ=A!Ta2gk&({la$AWHLqnTGk14Zs>Y;KExREGWQ~=P>6F@&{=I>t<6BCv~g~y0})bHhX zKdNNc7hi&P(N55!Q&RsPi$)d}zI7Y zmmO?UPyLTUeIh=D{uUMW4S|zmP|^;Djq}@aXR`rT_@n!CC>$rAJ?}?rLnB$myi?)s z@;gCB!mB!nf$>tVmquHhwr}75pbyZA#kU5Of>XI5B|^8coRH$<%LWz>GU2NEKkx2l z{{?tKcP!m_=``hjbZY{j+oD4}jD|vL$1(=r}FUbLtC3)%e6@~rvC z02P=-A+bJpU|I?@-TTwDH#&N8W z$1d0NA=9v--C)sre9RjC8gLAdc1WleaK)O<<4C}(sc`nJ=f7aBvJ-UHbNKhu9$iCe4F2~6(C$MrdXtuXM3J5?uQp?md`Q^(2SPr0@ z34jnG|H4?ehg%!=${lnq*S5i>cKI5zHtHq(MXEC$N>7{>;Hb3In5LGsX?=9woi#TD z0|S&OJAGfKrMYgbd+zjgSY5mY?GuNVEf{WQPSxnQU>j~an`lWIsf2f>Kx^oKFidLp zWmX4VKolbZA@sFG?QY$=QtW7s{DO6k;4@gpX@yn?M+c<0_nnSHHKw)=0OwxxM`k?!Jh^MRgPu9Of;6CYTQfFK(mP+;zV*rLGjO-rES}`{pqWaMO6utc!}fab!%(bc__EMfBlUNWA86%XpA19N=ZtB z3q#4+cWxe~2*gt~=D{D!v78T6b*r3kT*zY?FszwRT$GbL!oxEGrYbUj#T_(;oMxET z7pLzlrGXVgy3<8J5Eg;Qf1{}YeFA|E!!0Ybj<;@2BcA<*--^-h^wp?W-v#^S%o5@m zD5!cxYli40bgQTb>;_pEfB5nRYIg}L09V%~fCh-`-vCcLm@zn!+`eNYl|wBY(6 zLEu{0lQAv$#M+K(x)k2 z560b7Ps2VtkGBW)Zr5>Y#%Hm4IGLg*$w^F)z8WXvZh{U-<~M6W7KlRUON*84Ui|34 zM?`xQp`NTFfL@(b)`2@OO3a#t*PtyXEsg7X8oQaKDd+}VP-xSnJ7&ek_JD0t*}_fL zgeq1hJ|32z_;_0r6A>`Y;u93!rYJyU`YOCGC@JBj^A)5=T&PSyo;t?CK_0rjr@#4&33ML%5=#0~V+}6*!}#;|ytq zEbh)aA0S*FL|wigW*8(;Q1YN4+n&N}Adgm7r8nxBwkY^hmYK0pJcns!c(}l8V|{AM zjz|qh+_X!6em=s46yojejZ?6T2zdrHq9aQ$B{NgSmST@OBNtR7={u{J&z? zt-hNOgY(i3q^_yCiNu7(ixap;gi`k4KBtV#I(RZ_V-!W(X=uz*Mc;4RG&yoGhLekn zTmk=3kAW4Dz7(SMtNeTp5*8O=*<9{SR7Bh+LAINNfm9iw+1{QnIKPwdHY7ZptbZa? zqR`zB^7i!Ur{NhaKAflj`naL-7P!q>^oSh|Wx3Jn@^2B&@^ZLqYGEPAahnV>gQ%#e zlhYhnI>t`=m6-@DegOe!Fyn`j2#F{mBclWXO+mRuz<^nVugS|k;q6<;6t!pAVDy%a z@cnRBpx;9_SB!7+E`}!-OPFK&i|yf#$DT46#BfAa0IA_uf4=|{6AorF_a^TxMIK8+ z?7x6HZ)F>3I?rtmJ1-~q9$h6gdO;M<9HMyW1N^<@oQTbZVEU63XU16h#ubhz4`|M@ zCmraO35VY|H1NZ}O8<9l#W}W-(e?0C6~ZAm6mCadNK2?^(p_UuJ|ygtirE@|3Q7_R zC=4w@!rE8&`E#_4XYu{}lrYUm+|zJ<0x}0p0{TpRWhjf{l`AL3#MPM@X z%tW?|;>%5n{Y`mRi zjBYPzYg95rLPPC;eR|y7dhcPjFSed8U+r3=}aT#oK^a_o1>XuNyYjEV% z*H6MkidHR%{Yxek#-50IWY-({Vg#|NnVGszpN54xcG#-Sq} z)U>s-l0vt1%XZ4Eu;}*9Y6l5ug^#V?YMR1T(6Z$@qd09 z9F)dzW4h)bQuq!gN>$3*4^MynGC}*nz~DP1PWz&se|z^ptmVZ;WU8}dV>X=h!XYYw zG}P4nb`t1KA&$}71qnR>deV{-3@QV8M_`zT@b)tLmpx_fk@WT;d)N^ngY5)DK>r<% z>cr$^24ZT;+9rZG0WDc1?tkW%SoF|R$~LO#t*=*gsl^NqORfZgFmxztuiTB$Cr>m# zYU%SiBgk6E5v|YVd{NEg&Zt@TBPbevQ4!Q1jH?7jNjlqHz1ok)0X&iqA|u1Yk-lI* z*06POaKJqlqR#>|p~ZQ0{jh}jHs8Bi_sGpGKx;!w9aI)SP?u4R--ad%j{fuKV(2!W zduV>bvV!t<7TynEAD>GKY92BhkcM!WU@`W~%@u63f`pc8qL<#r?Low!SSWs4W0?olkPJ_djG@4-5nmcNDFIV8mF*I2?g1dr~k ztCgrBEMxDhD=S+Ru++T=V_Q>uq6hgzAAYpLtJA7h&!NOl=)aNpeki$3GpGPYgYYRz6~Z zQ|r${pBE-av-*aRF3R2JPLv&Mx(-bn(+8>)H7ZtUS34mlz`9U3qB#1C%{&15|LqND zi<6d`s%j8D1fxqfMUZ{KW#gsYnHU+3t*s5TwK+<)<>c;nr8f)CYRz$RaG0V2#>OV& zIMUd6XCYT$c_b5yEml@mk3e^=zP)Cb^8xdy zkCWDOc_=UZA~m(pe(*ES)VAO44BCI_@*p!|moA{MS2G}*d3x6^oKsv+%p@?7-13Ty z%*f#OJWJ6^5$ZR2E^se8M$Th%coH1O@rj8qR_AF2?fOsuV|pE#Y=<`r#WfZ@p`d%{ zVcj4iSK{PJ(O2;RaZJmq+11k}aIr^4-^Ed8Al`$J2iAN4{)2-=|zkZm+5Q${M2axSN?- z77VhbH7q3Lkkn@VOP5}QazTrgf`ke)`H@LYUEOH8;fYDYVhy|nXxBmV*ZxMb-XUC2m=%ilkkjA;1M>a?P$hi5@aU9cfEyqr4oEB=5 zgLCTp_h)HWv{63V!dQsBV&h6~|GqUh#)MCQ{}iwZ=q;iuy}Z0UJw2`ZhM!_A=Qe(Y z)rp_n@LKeqa6=uK?jn~9jRZLnBU(;1-S`+01$-2 zE+QfV!1B8#g|20fsHnvR<@Vv?B=z&>Y4#}k`1kA3l5-DIWBu-jO}XYz%@&vdTU&tIMJdr@>1C z{QX&kOsMDb?{fYmbH$XDju1A&7{$x#vu8jNK@@cDdH%0f8MzyUanRz3bB++WcO5yW zxnY)#q949nm!tQjHQC#8V*J+QbK@UEtb?lDKs}=l~Ko z6|+BUYVv$G-fOfM|hDsrMO9)S;cj+()nqn#KAH6K5z z)u#G<|MYhEId(V07RpEjUqhq&=rPNc?z0DO4gLD{BqBoFL!PniW>UdlDX;Y&FgVmE zD85ATc7=2tbQQ_f9u%A7De&+l86%?_s zBI9!iGCdw`Auoj`otH|w;6Z>}rh#d&S*dGrp^ zKf$f0Jd%8%RoVdiLu*Gz$H$L@D3fveRJAxxAH(Y>4!y=j(R^Y)cyI$fsq~DD?yjz# z`z1n79XR0;--2=;z`vV?1&SIo3ybcbnqJCA%qt%HiGYQYw1U{d8Mo=qS4B9gs`849 z>u71gPa+JTq^hbl2r%~hx#>r$tNUSOJ}V$1lK8c0+NBgtR-6e0Kqt9+ic{Rwc)IZG z=Em1hYc=k(^BWYCE^oR`6ry-5QC?n-OpoM%LQ$n-(?y!2jHd(Sq*n#R#TUTb{^LK@ zfhEn5dAuSdZ|fl^D)Q?Iydyq#bcjnz#<(3|W`?*Hm9;hDFg{=R7Ywg*93TMb7rMOT zbP}dcc@^;@0uaE>wqEP2e#82N?Jz^@(*Hm)0)7+QNSP0P>g}Pbl)uL%o^RzLnQSuS z`uyzk%YG8uD|%DTP4ryoDuIz;ygR3G3@tL;Fi`%Vv2zSd`$U(Do;Z=7ma#dNDU{}P zBDj{OsNgtO0Rq~EvV6QEV}3pY{#w%2!#Y>d`B!ejgai&KSd)N(qAH2Y{p1 zb^?3`oOLV@DIO3#R8NV;%G-_nNq=Wbj6kr&t;;LG?g!=zd(g(>03A9+C}G37b%YFd zpMG}Dip_fjwncJt8d3azx0Ms-$ZBlUwf~2AoGM`*r{&)XT>4!gr{ww#Uc%IZ{X1*k zz4LzmLqKoOX=#Y8@|7s}pn7xK`u@{t!?DN6^%u`HOf$)T=G=^g8*c6RC#bh50{O@TwMiAK1@xN)Le92h| z#vK1Oh4+$^9XSWv!tdTa^WL43;LSAHSBu7C8VWiDV`K9uG880WIc1$dH1|m$fRT~R zcoM=}`3*G=c(%cU;a6>GP6L2GctFK#AovsAkkQdmcyKUYOlV|SYjEPYb*qa{vdLaM zgbU=S1Qh~GRLCqz0-~b4$B$nv_CMvmu`Z2Ae#jB%ohpyk6$*;pbS>`W?dDuil2C9& zWxx0e|N4KP&jza23r0cxtaqeAiV4MgprYfUNn_o6E4<>-8I{){@7|pT&*F?f2rdGOXh8hv4>$N=o{m)MNA+>x_;aP#;;Jx!0*km29C#e-Cv5SPN#rz~M1;Ku;vf z$$=`s|AZSB#qkB8>>{F0+8Ax&Bz^*F)38Gt77!9TOo9On(isHohkkyr07ytmf|nb? zInx|Rk)Y(k4zf!MA)B&}9aC3^F7wX|dgyJ2Mo01HrBHGMc@4M+vl9ZLe#3}F4|~Tc z>$W6iX5vE#4Mm$`pFJDbJ*{PcBbu$7m!6miM>qU`j6{e49Hs2O?_C$Ct`+@U@+ik_ zC+vIL+Dvop=;EQHLbqWF=>yeA&W(XruLQ=1&GCK_j5o@}%bGVQLg`Xsb)M`JJ$(3H z!!TMp7P8hfn!PL1%@jtD^TGdHw>d&97@l?%BpOQNwx6ENS!qfU_B>kacK$9 z3@yM%w;Q>7b=3ne48xmKP#rEhf<7Ey=<{tDw(ZX6#SEpJ=g$u`;^N|CPD#=_H;@c4 znUPvfD{!2fdmI`|EZ(Srx8LBdNIJaB8}q-V4<*lTP%a9$I6iN7WPGeAo_qc{`+2cQ zdDd{g&dUtypDc82@;*j%=ldH(9<@KOnVIOs)phYoQA(mf@l%^(i|D#P!82?1&gCuE z>B0`C$qnN+TonL^Dc&6r{*C7(9 zl4MsQO#8dHQL%q85sFgb2>4jlh3Ntpmqlm?SRr~M7FY4Y1qJH4KYa(;NzZ7pmW6O` z=jSuUrtOSSP2}L1R+1Yr zAn%TvWKSyIq5>-_2ocPK-lBLJPiuR-wx%YGu6dJ1j3`;KGVlfLHZk^=WD3}Wx8kS^ zp?e8HL6Z;;l%(=<#O^|AJYGXlgY`~S;p$KmqTMG%%6TBZ*WSUQt*vb-nfr0A2^>r1 zVRVMyyE3dPdiuD^{`jw0zWDxC5^Pi)`c7&4CqgM^iU2yf72S9+6l(<~-ylk2$P0XD zu?uU1NdOdj##UB=d3aS2`7H@yM{bvI|8x8dGc)t4XX{W_KY#iJ53YT;gS~y>%?2K( zaTp1(cH}OUreMI)*lc1JWN zXDj-&cM)L_L8Vfa{feL7tX0DI{g3i>$apdJHYKz%^+U7*GSc13dkyBSwh4sQJ{c)` ze?9g2bBgWHF?S7V?=T6yaoCk5yLy2}#OG7%$L-0Yl8_LCd%fwKT=R3Cf0(US@?`}K ze&54@avo}O4DAw+A31VgDM@H=Ab2C*1~)!J=E5G>au^2S!jPkWT>A@eqOfb|LRtE1 z7yab?j-%>O^h!!3=TEV-Q?QG8t$Xy+LQlK9CUD z!tltQrUVmStl}6}f03K}xFEN@+)&YXd^^-@xI=Kd3_&i@Pe$5@rs)o2_W|pimoHnO z5(`D~V3rh9dKhU$d|f-Zw7eW06SFbfasi$QD0G+et-c|1t*))@B7#lx@bRtx@I6RR zL|Vf*zQOOGLqpYNhwjtf#HcFfOZo3tBExN?q4D$g$3b{`@i>eF%jhiRAg@|j_*-w( z@mDmC&f=0j4;}zmK)2aRU6cD143=FAkNn})BQ)u1#ZxL8;g2-x0vB9wwNq&<%%k47hrY7m0*% z%13OZY;kcBzzoGQq}Jp__$k;e%Ma$+(glIO}jgD2EGcgDekj-(Io6;yEa{{X@+%*;k_ zOlkA&>fjnh*upr1`|+T8b8HRDr%uorUD~(rR;nkWyBjp zkg(Av5fu?ZXhWl|{CV2@c&Fm6iS@%euC7J-`CQBXt6pnD zHmV5=31OS?6{z)G#rxybq+O=G>rH2TnCLf?C+i*Q{N(@6%)Dp1Khh(+vHCJ4rN-bI z+BolGJp+$Fyd>yRO8U;^PGGx_NWKdH#Y<5U!q2g_|`5454ll?^op$h5&UV7HBP+HmR z0)scEsIg#^jYuc0tw&*bJo*k@LJVd;)Uibe3CmEEAzoGK`QLZ5|2*P_9b1ec(8JOM z<|N;7IPKs6s=^UgYZ3=b2)yIaK$H~K7Dc}bO(amA^DBc@JAP`AZp>T)8dxIC@9pAl; zcpPHZNbv_2@`>!vOiBV-I13>b3k{D0-mrjaZh7Wo=l@+6IIz)O08@u_M6W1~H?I;1 zE--tNJpoNxiegXA(ds>2dYW9n)AM6{354Q{moInTF9SBQvaq-{3X{Lbn-9!6d#k%@ zoD^1}{695z=3zOnZNL8&r80C^L=u%`ZY36_(y+pvqCv=_G*K2(q|!igx0IHUF)7NF zxvWZw%+X^Z@hD1z423c!6xpBOTF3jo$Fui7_Obo(49Z>i@4l|{I?wNPb+g3t&n~+{ zX^qUT0Q4-{d;3Y?UH-jv1OcZ;KkX389H>}`OUUCW9v1n#pS-oJ_^+`CG>(YbF2<7v z2amB05QV;>Z>TME&j#wm+2-WuuW5Uel9<>%!S2`MC^Oqerbe11-b7^*jVAMCgVBlf z-Mfi@X+dEU9v;4zK*T5MKO)_FHTK(98WI|6$a;JCvZbY_y4o?MPjm&JAd=xk-wlXB zAxyd@>3k3~V;8uilp#X6YosMgG@^34Me0Z-mx>;75*M0NP{065&~lqIXCFB=2hzs` z-QnTwk0hw{_mhzTvd0e{BKv8Bgcm!xV@IF2i%{5BSMVFoJL6YMVb$tU!rW;7mi!4J zVS8GdB^(mE=z*p|x@KlyY31$abZwV`-O0)jPq2bSEfWML=q@lF9Rx=w0Bg|!9XQb6 zpbrNOumDlx%SjO>d}6v=sVr#akkAm&#&llneN{1Yq`YgQW2cjAw)+WAXMGu!1+DI# zy=F7X|AKhq<3VVa?LKPhkWZnQtfow!%y_leV)Y6>3Y|~i_Jw+XcM*gue!jlH=x=3P zTUc5mXYbrabcq(7u* zN$Y5M9C%l0c8G%Y3TC=N=Ak|G{Jg#ImCoudL?qZ|41i3f(22od&_3By7-it@$cfg} z^Yix)gL@_Z=|f8&ZSBM-Ye$S41%f?c?AS=;?!4bciwxUVR=M0&(o^r2KDl;`W+Kru zig$AreBy5}f`EgtO?EUSnC&yH#*@cSFt{V&p#Ip7Pz^Ccstq1s^}SWYQ%4lwa-xF2 zhp&>3GWCBo5*1||ff((}>;3dyNu!O5`o?y9MH zO_Z49vRb)vY=Yf1C#M@bqckP83YJv&FZSE|bH;SNlgsF~tYd%!=LiiSEI-18!gJ^7 z9=ca*8)$e#AhLc~Z!o&FS>vKgBt$=I7GHqAhRq(?)Sze?@_D(fyfnU&~$wM#Fz(^!5kv>Y>Q? zj)jT|OYS&uo>KblKP$-XA8;pi@fQ}A}pthk# zH${oSpmh>d=U;j5Mb%%}eYIy6-ojs_W=k*&##7tVs?y;t3YPrlM=x&Qm72=8{2)Ov z^W$y6z44@uivdxbqy5K8$K~0lP=l)~D=`4H8g41@Iy~Atd+uk1OCEtlR=d$;`ulIV zunY{banWEQ_TBsUTX*abg(&R~tLbroLt8$m=}|4vy|5o74wj+lw856<`zYI(s3{e- z{f1xo`A)_a=B?_pqP|nH2Z1-%kDFe<9xP)|h7Z~|IGX1{yUP%~TH9>Eo!YfOo`g0K zEYOdVmSs}bXZVj2gFBr3bo13MXU%{B)YNj2y2#4JUXmGmsH0K_&&YSzCqXiQ&6+hV z_t~>|qF@EcdU1CjE;}(i4^V*}g+v-0WO(E^FV4)WM}1k|z+^v9SW-9qzq9~9{~XmE zGLf_hH@8FUj4qjJcxP8RV8wd2#NQ+ZBLOb%D>VbnLr>)7&?j$g{lV^-fQP|lS9gQi z5bzvn=o#^yilDB#IxaZ>+j~|~5>RCV!w^yk%TiKPgE!G;&%nx^>FG70_LnFnOzHXB zB!rLpHCJk|YTYTG;Ubzfh+c$k6chLE8?K5O5aetiV=Nq*K}hiO$GHg~fAI8K^5ISy zLZclAJ}O?E%`_d5v8B&`eA|Vz?pYwI`$14;sEY0dUiRc)Q0|Uu$SlDO65;tj$ol@CHW!~!5X0^k;XcXD33>39H zX`k_7j+>A#SFf8AZ>PUCe8c$MS!`%jjiGPc10DVj$Uoa}(uHv(c~n&FnsUH<&bv>o ztvK6aC%*wbk{XoL3E>62*wJEMa{*f>!W#^0Zu90PO8T1y)zMEdQ2p*9YdUxrT<+Yt zi5M9~bHcB=SvMT|JB-0*D?S#ZUl;)|e?PQ0Jw53H=<=@xL3&}SV|+_+c@2!zETS?s z{;N+cC+@TE-z=lM+cE8AR#u~psK&N;aJZUxE|dqYCnKuy>tp&S5%eu=6 zm-_pYx-1Lx&?m}f9NipryJn&&gC+9>Q!Wx0`neyUDi*NLI3Q`n?v<4(si>$2FJt_D z3^bCYLll?~Es$xb^u(lyJFSr9K$NQrwNh_)^1nfvx;EXvefwm$#GsHs5|=>#9St=i zi#Uoo{%q1cr}bVsF`mc1z&y@2g{VH-NYYXH#0Yg=?O*Oydz*4!rKA1OWuJk1DGUiV zRf_0d9zEc`*G>Hwbkj^mec$PO7G*E~Ce}rDQx#Q3{^mu9q>J zuerIIb@piK_{8h&Ek$oh*O?KLQ@6JDp5M8k`dI3i?!}t`0bQGnjf|LxdVF>{;%BlN z`w^evncQ6Vs^NyKV((qNK{8~-4g^py06lattw9Mz?Q8q8aX#5F)CI~JZ@F%^oNWLH ziAQI3XrV=6TG|u(Gx}ceop&RrAH-b=8V7fO@WcrVg8YDXCeNDHM-Q!Ge~AQj&PX(? z2z6;$)z&P%Hhs`^Ls=OT7KOQMZLEQpFC8B_=^_atIQt2=U&3d|8KcrPOn>a+as!z& z2V#}qt4+`hD3c+@TjS#DCeCqp&*J!lRQZL_`O6a&g8jX{)igBzM2bzCN>cG7ibhO~AV27~zUk70RuW*MiukzFNI;+` zI~eJ*o&)9yEZ6NtVP|gB1ndO2xm6E>_eaM?w*^dq6as$eFG;yOGvt{V#`6zTV&(V!da~`&x`beKZBS7oSyt*9EB!s?Q z#dShgd+PZuS&|rexAMb0;8Zjc`%exF9V~}meTB`Va9o=ImPxdT* z;J`EI54a<;lZRzx#$(1jB4HKt^SxCGKT8dx=>NMBgL~A?Hpz4D+($^n^V76lSS=ff z;0y`LZCvt;5_l4H3TA*I78X&sb!luk#R~>0XwRN;D{6JFE3-+1FI4; z^0v4vv!w#8w|tnWx?|d`nKQ?Z8yB*2<*mQ|dQe{eG&a{;j&UWd9)MGJC)aJREUa{QRU;US7noLEX8T6+4x zeDL_T4<9n`HvzwxYTm!$y>1)^0-&{}s9>gU-uR}G|J3|G+UU)^V>ip^4Ded#$LfdX z!TGHcha3=R-o^$!YQ&J}P5Qx?dK$D`bPEk_gBFcHW@buJzTRyTN>SMr(^YU3)d6Nbc3z*w@Rb@H+isl=X-nW zy_H&(jb7Fzp>d;uV&K}DsHzc2+f@B~A;bqfZu!?@aFQ##J`^r9`t#&I0I(&dFEiKG))}T(Ny~G*6nXm6!H~+LS%iUH&qHv@KEt zpdfag6Mj|A-Hme9$xT?AKeV+4R*{DA-|H!<06KN4Yr;xz-CFzQ-MYpR2HKOXR&t`SIRNMbJGQcw zX@b7?NBA2zT4(x^`>$WW#vLo-6AnZ6FK3J}2g0C>f!v8d73YN=L5Uj&-oI~pzRE&Y z21I+|{Q01xDlvhVho)qAOo?^UpNBtd*Xsy5-2yIxU?AzLsHpCt^q%gEnKX?0=GiMt z^eKXGzWAw&uFseGf`~X98oH@3UPCQ_Qg!0QL#GkHkT#+;7B|_n$MqU5`mGe65`hQHwMa)FE$1w zDSUyE8Z6@=UtMvl$(7u({5eH4&ty-kN;~)ES*Zr23D5}Cj<0XSK+VycFy(zE!J9`= z=wC!5xfnlZwD0Gj>*bEt$taA??yii>!nMdn9n|I0#~U zgJjNC_Df#g&!2x=g&v(;W80f>Z3JSV9cQXB)x0Fyu;I0)93U!CNnfSahFZt==~z4vR}17*ef!X1Oj-Cq+ibsKEEy(932oB=uQyI6q+0j zNz|}euh{fT%-wrfQ&ar;RxZG-L~FR?VQ?6d;gJV-Z@7Gvzm!c^*=A4p{)#h9F5bv4mR|hI`b0q@Cp}#3 z!_I|t0Z>giLM|z?AE_089qVWaH=s_Iu@KU;MNH`Z|NV(|ToWbni1qs$YqyC0-ad<9 zD6A#SM*)F>hUW$(bp0El_WAP>oVSyc6Nxl$8TTn`cp+-a%B({?lDmrJhAj577t;iQ zpXfunbV<;k7L+|vO%T3qhM99ZfvVWa4Er3>l0t=TUUSXMxG^LGry~;hKmT00Syk`TJ10ZU#8rH99HaE5 z1kGwD=F!)2&~vzfkkQf6CO}8g(CfXJJVR>a`0ciJ^EiEm@_3M0jx7NKC6yg{p%qC8>)&0~jy%fC*80+&;t3BY3 zXpW|uPnkNEs;qot8uY?2Ka)pXk{}*(fu#=!sNc6w>BcL&HETrWIszp9Jy-d@OMTNz#e3nX$&8h`-w_Xq z7q6KPJg&jC_$$2xGy+xT8x4T1otzNF`qGj}>YAF&kUUXnKdeEbtKd5*Xq~?af+xGl zlfQO!5O5`{x>+AYiu7q@`SANPN&a3p2Z!!SH+}ykhN~*FloG>I{r;v#r zS=|%_siomI`t447g>=Y{{rkPxm*@8}LxhApQ3~mPXRs~+64SqkSflCD(IeTVa%p}l z_SKKLCdZqZY3cDBE%f%zAp^?$^x~Ud#vpgVnKyV%7yy@}Zd~6{JP1Bc;_T+Oou-e| zhe6@h>(@xT?sgo#cyTQkYq;duYka%$!e!WY=0q{+v3e)EkDvuaGJ{>}$@q4N6#weY zNfHWsK-JAnS>&=(Lg`|#vD@{V+D(v-ByyR8Oun>C()L`m@SNAK#JRp`kxJz}A`?Ik zJoA9@{?IYkJYAzSbh}FfOB@{dxN(H$HLzF)>ZDBTC;S2ZvLKNG>hR$g2%*sh zbLw#LZJALa2(FHE=dNbrQOck%$KOF&W^(ut$aPufoD??aGM$HY?M&{yw)|U^;fNg@ z)AWMHhEovA>)6F?Zc%*idh~oVQ~eKy^Y~-a_DH==coqn20>HlLEV<`TA{B%vo)M$k z^5u6cm}?R&XUMfC_wF3RkQ90*Vw51XmIEiK*&oGh@I8ey2uof9B0) zW@MmHG$+qKCcCn75|yo0q4JsZ&E@L(81)-06S)a9kAjhtgQ@A+cy`~_%8@qGdNYjx z5bY}<*g0SC?DyvoM3&do*eDIri}3`{^0pEUGa37aipWYUBxs=DR-o$r5Jk!nH&!vm(F_Z+ZTEKM42-_c_{wc znT2b=)OYK@DU>#wTI3@NEtV`^T(8)t@gK(h|3c1x^-&fxymM(2kW7NbsVxCrnu)G~ zk9bv~y3+qfQE1PL9glzk=}zN0L1@<9$~9DLDt#n6-hVz~wrcR`Idt^s+m9drN^kZ> zVVZ^1FgC~i1lb8NAV`dWYP~k$KtV$DjT1ipWKW^Y7di)B2>sfnn(_cKRMKb3pCa9_ zmT=vNi3y&~7!&kon@@z9a48CWfY3OEmDLh6czo)nljdC}q;&0CYIl${QWPc7B_B)F zH(WIfX_S0Et|?fnV<;gy?@e{#eQ;VAm-j3u_SyEsljp>#Bqtbi=1?7gvUzQEGC-*4 zWI(1(OwJJ4jvmXduaeLRr6m55IrF_->T&U+)hPSHy1N%U?8eNXxZcIt**e3SvcQ>0 zbO;y8c~Ii1OYlQ`8|CE$%g@bcYoUg`PxGl9&&(|U8n1k*`Jv^W2r*Xl&@yLu z5fFV>SM|4(Bg@RBhF8i0#<9b57YH%iACVaXPePYX&hr>&*sz1;h{PYlW8EDZ5FA^<7~84#GWxw#UL-om%K-v)Xwx|`no z>Q#Sr_2k{V~qZBXPs1E`k5O0L)zli`CF?$S1h^k72 z4ZFsPlNYX*?CH14Ck2T@a>D-uST=8x6JK(BdU`PlT-+iAjIf$)xn&b*e`Yv1i0L+Y zd7o%N3HzG7z(6?v4_u7G%e+>Re`;CcXTpU9-0cyLRE;SjBzG=%jtuzw4?R0JTYC18 zRjVe?9V$fCP1;s)b@nLF{H0oFCtJ)JHN(ki(1gIffN=?6&{C!Xesdfe;;6#Rk3OTZ zzMh1_YiH;1VNIxwI&kdR#%31#&m0zv2}T{nK973^Z7s zAgtyILp+INCZ2|E_RciO8ah=yEcuL24sVR_;c^8)t`kR(lKy zt>`^d9r#C{@2#^qx45Ftj!iq|!24da-<)?MBR(~inOVFN2|`}+@R$!iIqtO9kLh6e z<;^A(9L#Vo41x5`4%GdjrXPHAayXy_u7l=J)0 zh51J2ng3pVU=pRD;3WEg;!EL^*4>`3xEov$?x3Yao%_=2aaBQ|^Xp%}U@UZ%D>Y~- z7^#UK1~1B#%sHbDyB-Ax>!dLV_g@nHDLjWeAnSOtIB0Voy{YLlY)HDrY8uot0rxTo ze%_yvp@yVKT|L%!mY~!t*=cgX#n(s5FO$i0-e?Xb;qcn>x9rn_s za8^@lKNTNGlwb4pi4fnzU|ZKZj~r7yy^Q9QoucpS*W6(!`QCPJ>KB1oz&3y&kAlxj zxyRMjRsYG|ycIz~2-1(@r|?Uq^+hr_zt;qw8t8k!b|vTa#A5 ztY|tL8WODe*|TxXL6CzrgoH00JmP<#N-=Cg2^PJf=FcxT@7(dkGzax@#4&|iy`BWx zeW%H(KDt~f!!x~=t1Luv_Cgbun;4gdr!y|~Ni6kS0kJ~B=B6n?VGPHZ{dk`1LE^!% z1Ew{*1_rj!`Ot#rsQv!s%BcnQYto!gI1lShO=Nf_s{mS2Q9%LIXE==()wNp70Y2^g zSv?NC$dz_n5lxxINpNb;sxD>Iw>Y|H<>aU(X2BN(58im`&n)^0>>M0YvJD#mghoRa zao^qN1727E)f6(Z#WHcI;`;YsH&j|YE0uE(CC!@_HClb3`&RIeffb{Iks%fx z?=J5W>vv_tnKLWg-TS#Lc?1Mr%Rri!x8pP(_fU8c-Yb)cf%;-Rq?t$kNdjT`(!8Bh zkX`$jQf_Ox5!{prFlW_!${5f>hxR_s)xAgEQ2QaU1WrK-4QUu;^*s8C2Zt)g4bwk6BYSy6&Zcr zMi_=IKAyfBg>YJ$$MOhae>Yxi9d7}c8k<|Ymu@gw?YpKO;xOLTXe*Nvv%o|{1$x@u zz>_FFQ6EmYL|gu@ih&Tf6tsJ(1BFI8$N*#h50;h8_E3Q;25ISb6Dg}URJQJjK7-@kGd%5_am!*6cU10J*MYij|r#B!#l z-I<`_mo8q+ch2!AL6)&OeJnwV-OoH5!qr%#MqO(e9k!T!j0>lZJH38C!W7{!2cYSWOfje{N}>J`{x%{gL{)W0qS&qQ|Ac&hw|J`VYFA!ouq}ZY1tEXma-S zOp+k+rgZk+xZlcyDplzT-m+}aJ=v#6Ep7vsCO3(mN)wa1F=6w;Ux0_`)wt}-aCuSP z$Gx=Zh=x`WGMZ7LVjbBYU?Nw|ZYO}BiwxBCTA8f*q3i)xLg=JFIv~H$WWu(p?5q-E|t;rox3i|3qh&sq6$ z#?q7X2AwHV5I#zjQ+^}&>wf1;As#rw#8`Ke$}x0rE*w@N8H z1Vo6pVPS$$v&Fw5cHO0ZXF%f_d^lWDE^r!se)CAAzB6Ad2zrP=+vQP{0GyzlVd2C4 zA;K~-8nWX$h>7N0N?V}w3 z_bcvYlcM-1z(RtKu^H@U|6QU*(BV9Mcx8W-ZaPxK#(8ibb7sz5FFAJN#P;?hp>oQ2 zgsp2=fbB8;_?{LmZA-@pxm@KMQ#{1XETt$saQgI~^UJ#UDDhX5WRb&FyYy|13Fm&- z{i^jBwr^P1J;J&Nve&W=^SM<1t(m*At?EC&kV>(`>V6Z`dhzWA;1t%!Fk28IvZWtKl(eG?3ncREB{+9JA5l>3Y8X4(tEj9kKea$Ra<{q zH-Cf>!Cg_p;&*(Kz>bLDORit{9*qbDP@p*=P(ygyL;Sk)f(36`c&J7|6TL@MB|+n_ z%~r6!E2;{6B4GL@!&%HkUbrwWk6@s_$;#4UtgVCeV^_u8cc*FQWynhKJ{9J$2+r-#&d5c6|V^G2`p^m^7=1kT xsVElKeVA^ww%1)Xp?oUG8z>C3t93`G>W;B5Tk>aw^SXjyKY51TDO<1D{{?;#?f3uy literal 0 HcmV?d00001 diff --git a/doc/figures/flowchart.ipe b/doc/figures/flowchart.ipe new file mode 100644 index 00000000..d3f5c307 --- /dev/null +++ b/doc/figures/flowchart.ipe @@ -0,0 +1,338 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +Global Planner +Local Planner +controller + +48 672 +144 672 +144 640 +48 640 u + + +192 672 +288 672 +288 640 +192 640 u + + +320 672 +416 672 +416 640 +320 640 u + + +284 656 m +324 656 l + + +368.145 705.333 m +368.234 670.666 l + +snd test/perf logger + +256 592 +352 592 +352 560 +256 560 u + + +432.163 698.34 m +425.746 690.014 +377 698.167 c + + +316.473 656 m +353.224 663.063 +346.575 689.685 c + + +239.941 705.333 m +239.932 670.667 l + + +512 752 m +512 640 l +592 640 l +592 752 l +h + +Robot + +476 656 m +512 655.758 l + +topic: global\_trajectory +msg: tracking\_pid::trajectory +topic: local\_trajectory +msg: tracking\_pid::traj\_point +topic: move\_base/cmd\_vel +msg: geometry\_msgs::Twist +topic: odometry\_map +msg: nav\_msgs::Odometry +/tf (robot pose) + +461.091 735.831 m +369.472 735.831 +359.917 735.831 +359.182 735.586 +353.792 732.891 +352.077 727.011 +352.052 720.001 c + + + diff --git a/doc/figures/flowchart.png b/doc/figures/flowchart.png new file mode 100644 index 0000000000000000000000000000000000000000..c7ec2a83d2548faf8ef5d33edf0402e6208fd20b GIT binary patch literal 75389 zcmeFZhf`GP_66F2jS3=)AR|aHpaKI(7A2|(k~1nvMRF3Fj6_FJ5kbj8asy2iBtrux zg5(?;8U)E%XmWV_FmvzRSM~mZSM{T6YQ|Cfob#O>)?RDvex;@&M?=L#g+ifd?#kcR zK%pp!C=}V^5lZ+!ozJu6;a_AJ4Y`}B+?KP`@QXucGRiV2RDLk^&LcGZ`sh>n`xq4J z_*di~vO2qrCn(fD_U>&NEmy<2p`(LJ<81r;M$X<;{ZuCFvL~_DJn}a$k|K}vcbvyP zQ%t(T@nGjc`03=1Do%@&7V0=}T-f<`74_S%x=8y~e7$AO5@r+ef|%C9^`^nR$uWD@ z!QOt;mN+p?3#lx(mE?U4ZsFjEY}b2#c;9TVb$?Sz3Hf=X*%-k?<16~l?@K>49LcdE z6q|l~dGg@*_hV;za;)|e_9yqxEg?^f{2XbauG}5o-}W{*iW=F6k2E5;`}=oLJ{u=o zDNs`X{<`$!|NZ*^&g1{$;XnED|Lc56tlt_7mTJnY-uv+le!^-BpES_HyFKAw!+h|7 z_eU42_eG$j2x2Ch_%+j%_*3o#38crnXHE~6eVXpePZ1k^c|i&OfHK~lq%>6Sq>NPv zr*h)b&5g_XX!00-e$2gs51W%6p+P>;=UA~-FR!4>f_^97!{yUr)%F(Gtw2UT+vob> z+`7eNC6qKweJK4bog8cF+^d)I6Ox`=^fQh<_sybDUbu6WAAUHl;_MGYg{u$x=O)jX z6N+snYMJeXiY&Ve!sEvqBTIa`MQmau zSZT$VeB5@H>zcp3pl<#>-q>93G}{Dkec|mzBfi;Mbo)}vyb0OtljIZaIq77w(%6RIUY%{XEkk(F^TtB5L@Zdvy0Zw(uq#+ z-s|64X_Z>gq=1JGu@$iyc*4vw<7E^?O^RFYHS#V)o|yNaC$?9QsfB2bi8S)+Z;BSI zQ;HV&PT8R%LDK5XHMqfio7R#QBHX7zR!6ChcfIhFuB0Gzl3{GWa-R8^G)(e{Rzx?xNgjih0?F$+jF1R z`n=bNTtsq^uR%&Nt0-lxiNF|vMo3H3Ss?6;BpKAYnP*~se z;YROTU-{A(YMbEfwRu7;ghY&h$&X)DJ%6dpxb>9U8+Ya#*S#bY;^P?AHS#p^+MX9J z(WU=OYJaEm+5YbOM5wA{KBqWC(=pmU(p-P%S+9+s#U=WM@y=YmJ716AXrfdIpkX%6 zEuY~cjH%+M98ViwAc_MiYEIpc$Y_5-GYQa568Pq9(u-ZM1MPaOc<>+f}YrDcN((JY$ivq1H93&y$#{Frmq-VLZaE4Qf zb^mBF<;4W9qhSxUrWL~nG$wJp*P3HRgRiX(?rksHy%n4I1G7*zIj}r_n$OVMYj4x6 zL>R8kUST0_gLUc^j4OBpp>y=_&?z~VSm@1k%c>*wZn0U6#_o%B|3Q~wPvzO39Gl=2 zPR+Ea%ki#Xo3jNnnE~Nncs;SPRDO2<)UPsQ$|~KrbVNvPe(b;_LT9UqE(G4xpsd^tPdJfILzdn ziRUWP!fB+LG)9d+XJv#7Z|)Xe<*@VC=0ZWE=Ua zcODzMjWY;3&1iL&*co*~anOWlk6R4v$mS9b*3fQ=mms`#%6KS(T)O2>5R=KGnJ}dQ z3KfksWq4$6X{xv(&-J0=l9&!zuArBULVb4CyVkAoM^9bqJ26jQ62v~2P4i`IL`~%Rc)rte@O9pu=#r&<41;7j1-wCl!zMF zy%k=WY!#K-S$eKVE$ziQo?7KmGUyOE-I*SY)5&?z;-ZQljS;c2yz}OdN37mE_N*=N zkZR^Vc}8|zS{V;5yE9Eb-9Syt_uqyoWp+=2k4net&mBX&dwV^dnDboUqfc_m@#4{x zH0ZzndHu-{C&WST{oRSxPE}C|pJv<~pL+6N7DMGlEG5du1s2D+QRrJdLQ-2(7non( z7!T7_8vhZFsfy)Lj1Zb1DEcHZDwG=+fn*ld&DMAco6}+NJmqxkc=HTuVSS}LCR@;V zY(KqoobLF3*0D{-bZd3mR^{>PKvT-8i5Ok^p@Mt7mU_hUTFUFZcr}<_kX9kpX?6R_u3pU zKCBUVn#bCBe5Z6KGfRmy(@o72*x*mYY)#eTF#e-|(LsjVeJ-%T$a}XN!u`9xC#gI4 z5boyRxB2~US@L94w6RA^AH^isoN#d$6c@8PGboh9gB_@j59Ic6&{fd_Ca)3Dr;MKU z%%ipRc3SpnYAm{a9^TwL_;{HfyYm<05fL(r9;M zEO^aMyJK?ljAFk6#F#0o#~i<|hAWe><&$AOnb(tjj84$&KTiGtAYyP-!}ssEOg&Pu zSS9E(XdlpgGNAS?%Sy?b(}ZPZO3l z8YD_&=Kz$2qD!OqD6g)BnyY^_#Ea;$C_%WgoSxw2z)|KlX@6pWv(sG+= z2_K5%S0SKExH-j%9^CTFXSb78Cgl-$JK6H#-xp;#+uJsfg`eSj0Baj zobDp?-&uM3=4QeoK(kJq=H*y#eJLh|eS4_hF}j9sST)Kybcke6u7Ql8F;9g!%0~X3(4}qFp!cErE8Y66FU7WyIwecl9PZz>dh`7$AVSd# zcivomewbDSpq=n7|6}#R4Hev0Iz`Cppu$%!@ah6-uhP(RwA=d$B> zb~|yQe3rMf(4sTW7;6=3=mG85Ikz zS~unkJC$%6*g4woRoe?zb|=DYd08|VQSwqoo}_RDBwIjNYq|A$FOOLny$nUu=V`!M zhnF}*eEBBeMFR*YAurE7L;Rr8yYHQy+|l?ifO5@BNN`G}bk#J>B2iFM7jSc1!v(qq zPT@H#ZqwMvxph@zj{psJwXrpB*Atd7Hi;}b&KLET(@8DjaSl8?!V;UmLPangA6$sj z3J!{n)eQ^CXaXNiurY*eFRP8$`VeIp;OE(^j|t*#=6Sc?o@XRgVdzSEW?r@5#I!Yj zUSZ93BHFA8>E{!z35yDE`pYos(*s3WET=~Sx!PmB^h#`t7U!o@f}J_!MUbAR`-H-7 zOT7xFp}A&Xud`h-ilPxraghP+sAV;~5=B0IS67DmS3}hw5j<9Uqda=WO+3hvB)4Qm zofWl>LxQJOYTx=t7^gAOeU?X;?*1b*G0grpNmNFoNW`+MenA)iu@cq#oBF6 z`+V*Zo%ftElPyjhW%%ud3TNh4$?a_JhGq}Db|#yw{4_ru=xxQ^{THORm7PbFAKR>a zq2?=GVf^#nh{Brus%IFtZj1Q(AZ8(NP|JvMfLs06XRQpi&&SD5E^IGWmk`*byeheu zJI(Si(V zSV4xFRM-)6@!dCfyDZ(j;uS!;6AEkftzMyS;u87An*c`pH)n9Cx!Tv0h!}TM+riR& zv6UvlW{7xeyJC4K%*=~)&)dvInaNvFbs(@fA7;5rr&1!x&Q0fA;=Q+7SDY)h)@M9V z>-#|NgZm6lq}gYJOuB%xZZ}-lFuIdE7Yfb_Lg`yn;huQ7{DMV2c!li9rJ^Jcf9kWX8S4uND3{6&|hRMdP`? z{3^#JXuK6}kc8GLn=o6KY8WE}W(93fcf=7inxRYcDJ@P037!Ihr8NG5Rq2$|eC12C zwe}ND(WcvL^R`D(S6mTfgrJ?~(Vijzb#ow10{u?S zL_4-hte3v8#7%~_SfCZ30EtqZkAdf@#EFI#V~<_PKh(cC`aRHwk6U=)?d_E!8XC3} zuS#g~`|@Fz)TjA{=k8|g%!$SuvxTioo@dd;2F^ipa~f4u(p>acSoO@9gOT0Qdpxc8 z#K5hEg^I=E#$hP&xfeyzOSZsBtVbp=4O@AdRNb^hINs`Ps>Qdi|ta^(Xp9Kx4 zRl6(f9M&Jowcz73Oo0rKsT-1_E4qUC+?s*0NW*t1~TW- z(c(Mnjbz4xc-^69inuTZVL@j3fzx z@b?ZQHNK`h8_T)h?AGS`oxOq7sOFX_K*;Ut?4y+uq%O@gh^Dohc`};aUwQJxi72%` zdY>CmmTgcf6cSc7p5rZt0idn5?S2qoH9vcSorUvbl20Hxs|>Dc9GPl>j1qugjmyy= z`1t5~Y(+qEy1##BI!uGUAJ~`#^>CM3#aEPcM;R@unJN}uFd9Bp-0)of&bW3Xb5am1 z)>VxRjQ5MJqxH@UptIv$M~@Oh9H4I^Q%ag{o?&r7l%L$N=lTadfr8s>%)k2T6c=yR z-IWo=26tIqn@$b2mbPD=YUeLuTik9_JI04!eSb#8I*R41GQNFlZQja#eZj`IV!rq# z$!)Q6<*nH6YNvADTbB9jTO}|Nn11#{SJfC^G5MhW-E6AVUjBgmh+Ot936|WdCmxw1 z_pTv3+~<3L`X?oOXeE$q6W>m$7WVRE>$Mx@i$V^;*-U6+KQ63H+4wpZ0k5bDtj`Ky z>GJoh&d>v6=r|M(CNjETk&Ld5A~W+Dh5-uefnA6$;dO=?_>Rj(mo#`S#v6NrbdxD^ z$fd8l?rl!ma2+4|$m%|oG|{bVW=7@JL0%yl5f`i+@4NP(6^XaX*r3_ zxc$#1s46Yk4aa}QUpOj#y`xlr zcYEE%+vGG_he_1d`gwr7(uS+62(gCSCww7kDx!br$ya}xiPTV)f=NoMg9f9xT|mx2iuCpJ&579gl{t?^{GsY;N-IDtwA4&hB1 zP^>Wd)7uz5TYe!n_wnnTvdwUnr%m2NILy@aq+0AdM6oYY5O) z=f1wuP|WOQMm@}9x#8TWaCWG#*mh`O;vRCGh#El+%d!uC+4cUJXtQMzSHOmX3Y4lN(5@nt@0hJ5H zm9!9&ayYdlKjr2Mh<{y>8-2bMd#u~u5V70e+Xb-=)gOSMQLOBUvPEaP%c3n`#|62! zLI_h0$Mw<#*gcVrv}r(r_P{%!t|&p{PVub8uXZT@`?nd~M|cX;2~a@f+h|a9OHk++-;B?SqbSlcq9=Rbs#}s`bXAxGp)Wo|fdy z4X+A!|5Mr~+=f07H7Ke9xP6NRm%HfWsCrZ6&E~a{+d(f5#$RFcn9HAl8O{NV^4W=z z@F>Pt6lCcYRnH;fmp0n}39JLpZTV3z?d(FAU8$ z=hZSIvG{qWH!pz`2=s$+xQ&DZmH3h~926mNSy^=G7X&CSk)OCW?Cp(k`k(B_?i`%4 zU*a7#AcdX1$xtC54$jw?k{H2fWQ)wX*IgE@J&aK(joF6F35B9HkaZYjFuigFAx>ae z$NbMRde&f-viE)7AZa0M4^wBn+i?^-hB!-Z@jEIRRt1~VdC~%T^W|kA3jkfNKP z4@!v*N{8$|k8Y8rHU@gAta8zVs04`uLKMI8oExHoLExZ#v<6`MP&%0IpkQA<7?Ie0 zMbQ@>$*MqFGPpZ|w04L^MU|_mcEeT0qIZR0s1jy0aD0cvP#M&TCLqvlew?)1y$1|f zI5$csgbFjf%vIl84zy(4t}93e6@!EIjzq_~K7O;!bGQ8aR=I-Pl@$<2}GiZjb;m=>|R=2vfOnBV7k?QRuM@*tlphnsMs^#=S=j?|Qc zoKbWe$6)gf@&D3s&92tuHhU-o#X=XoCi4HyLTLQm3{vZ!nNL!UB z$fwUZmBFwb1+pqqUQrhb(8?_qf9B!W9{j&?xbkOL+UNeD^oEPb_A*>k0^nXNhePJU^8$Yod<5u9PxEW&3V7hjU@Ouc(bZw(l{ zGVEjO;SvoK-ungLR_6)5h34&F5PJ52zN=&Nu8bDLr1j?;2N>{Xz+*P)yZ(GJk&*JY z;Ij{$@o-nJ0pZvmS8Lt|Fb2c94?(*u>(MKHniPBLe&r&kRGpwWjp@`c^(`okRfMz9 zr$kILTOQf$zY%Py$86&Uov+O+%Irh#4q&R?Ni%7X+-bmLm_j^EKr}ciE-?HeNdTr! zgUaO`3zFr>-V%Ajk3&HA2>#P2Q0)CW+4mkTa*&jcqNGI*WE><`>Qkr}K?pRsYpOn8 zsJofVSv^+qVhbo>M1CGJatpk?@f*e(0vfirN+DspG_lz4tv4;j*F?;yoJJp3l*}R&99ds!S^RX)E zWe&L#13fk+L#C~}aFEeZLkJG4*}X0>J4Nm7ZWCmF%e_4tEoj%4DC6vvcIMjCM2Xz% zh(9UhS^44K-Q)!svVVXDAJ$h?dm)ofkIYBOaVjZbLhGZI#>ecdhiIyNoU=GJ=32yoY_r9y^cU^ z4o#4M;}f?TSFdqAFG)z;*7Pxk2~w<)8+6I|0YxJ2^&3_{&c zGWfH9W<=YE=@fu0?R+->o=~65D>@GAL~9@^a=Z8(hoLOvxr0E9#-TalJA;h;Kfyvg*ANU zPJ1)IY2U|3Vk<2Y>-xGe_bVToPHjWOmHu71yGBTZN;QFS7ymY!4k-Ub&bGwmP=R;k z@`mV&G5qcBJANC^`nO5^CdC zinI3Rl6?|e*mw|NRnaj;CAf6js^gvs#D3MhGDC3UB4>Bq3ed?n3N^&zw^{o+F^9TX>jS{Fw{Jn;+p@oDZ?{=4JQj+;U$1UK>-U%8-re z8dJ8QJ*8a2v?_t!a?LE!Hw;tg)LSbZdyB!gb6^NeY`ISw-aC7RtQd{4yAw3WeB^H$ z4TGUj-(qbF0Pijld=KoxLJLI;9A+(X)Bk(pB@LmM3DkMyC!#Z;4vU2^#t2C}KYGdV z44KF<6M06}#PHg*!Vsc+doAk`rj4Um`>{%=+=_+kx-oy)?NeocdtYy*99EK`waC}M z$k{%Id#L>q_4b!o48^kj2_H!0;4kBLKT0 ze_&&}@V}rV%>VDriRQ_5UkDZ-RS*d5=y8L43rps`5+ue@Zkv5~Oj-66_eQdoEAbh2 zoZ1pfnb<68y0Z?l%xd`EU$zIpN~jztxqgf+?hgQNHZxr~<4gdZoq>E)d#B)lsXPsE zgGT2UIT`9S@hy*+BWyZSUvaP^gAt>wWQ#~04YOQEnjbKPabmzlVQf@8Hz(E$9E@+;kgn7rHCMLxZ83gk*>s&BEUqV>e?sg1ebZ@}YT?tuk?M!-Cji*u_&W=293y0x#c2wm;%=gE6ZB4I{3?18LCy*+$ zI0kl|BF@Iuy{uxdc*mF4PLG(%uKj1eA3YB^U!wC~*C%R;P3ENA+@`bh zxqY*QnP)!H{SeRrExaj(0ib3penZKL|IgV$#(iC0jp%mIzlb3qJV-QK>t;fvtC7(q z^%r9P`&h|7@HZTd9KZMzG zKW@=1IW#x4T&n$EZW38O2)SE+j}StN#@{W}JuoxUH5;cG$qQidkMN&_o}x>R`<7Re zdbJ^XU*_G2#~~9upk9$Em(f?32l83?N=L{EFXgLG0!S&6Q6SaByw7JnFpL<{R4Gv* zMwZWTv-$o2*w|zLgV1g>n0^mr6lE5*b>xVa4cAO!xDV7&S!`%jn~eq4d7Tr9R=v4m zE6?9Vj<**O-QlC4G;+}SKxx0#a^13sP37}RKKVj`Uwwry+rNF<_d6Ehx_y_l=U18T zC>BpFc)%Cz)C+-=2sYx=TojMOqjTQX6SLm})cpb%#cX$MZ%6*$4Z<&)in?YB1; zKH@7{uh0Iu?JtdFAU^flBp^40<6eZr5DB#EwLG%FBng!OeW@ATO} zU#((L(sU+c3HGHpJP=y}B2xbyVJco2>?KF-gM_p}Y@#H?`wRi^V;w9*V(nTCs4;n7R#ak9_nFY{IlP8J(iJ{52>$y{Z5h3I#;Styq1?MVL6TcF`7*4 z=Tx(xYH1?Lj}WNJ*Izl#tv6|d+RdYFYJ0r&B(hNM57b( zna)lMM>Gmq596)t)YF;${=Ug3m{sDs*&YaOqY5jRiM^4W^r=u(JZ)Zxha4dd>QK;Q z9pePe-Xg00e7N%u&h);*I$^7xLSKW$?NgGnREeDTQfpd3MK=d?p%?R@dW{k;hM|VK zL-1_=wAY=WQ(+MrWR*|^dJwvQ(6Lk5bg~m~$#OJmj(9CD*Z*P}w-M!VN0T36`M79-aMl>m8Dlm3Q?>5B zI>NHu6zlhr;m`)KoS2EaP)QYwv`SJk`cJx}(pQwDB6JtIv9#2^1tyJgHj+>=bA<{h zXvMFN?vY$vt+xL^18*CcrKV_aM1|?-3!~EI>^c8xon%6V7Fvz(3T1qGTOxb4E!FB3 zxqyPQE1379fovK`j9$kdNmymZ75XEX$R;t_zJXr8bMZ51n${mx)+y@6p@5d)t}q<) z1PIy$*RZZ@)SOYk{?vMY+~dNTpOtN?Zb8hV#nU$t+6?oRM)bfRg;40G2x{j8e$Mej zr<0n5|9^q)xUZ-fsB81+jg(+X2af1TAf1bSFXDs2G82txOk$roOR=O|>Fj=o$Pi+G zaviE|__qmchF0)%a{xqz9a<7>g_Rv|?$AAO%UZqPZtkt1Im5%O;C;%KaKYehdP#Oq zAV)Mo2c>L$*`i9rc4m>i>%uKxKlxEl&*)^3@QV*q&SW*HkFQcn8r z6!Ldx=?1%c;TUt>r87%DxDz?GGniz}=cG{ePZUmO&&JjAp#PIO#3TA&tvr$xZ?EJSA#rGDVw84 z%V%=|3%MDe$uR2&-qUqh{as{+HP4UeydwrKV;EN$=Dj63=EFljbexUsb8|pn9a`$S8E)Ue2DgcT<+g?M}KEC(pdY{Gm; zU}cZgcX(xUEwv`L;KE-C2n|-P{3z3Yg1)}9)Kfu<3Kd;LUHQ%e@X-9<9OEJ4ZG4tO zB(DuvJZ2RHLb*0#g{>kqIaB$I&SN!?Vd+B=+W}2TMggbL#>v0PEs4L~NPns+C~0R} zYFLJv)5odP`U4kAkiKtNSrhA*hS(cCciX4>rX*?y_5pNYh`}8yvdL6q;D096ye!Gq zuzdaC3%5BP@09(IJa7$8&eg>N${C%wqzmOIa}0<1rl%wEZ-pZ>>nk=Y2 zqIJs;kWw#kKSKtMN<6J30j&8hZ8N!3Bv zw{QI5&g*N{;{2$EV=3oThVENpJX{H;-3gfL9jt2k-mPn$sMF-E5xNxY&g?32hLpFb zQ^tESsWBKP(ln-(+FMHF#kT;5|ES{QNw*3j_5v?!=W#z{{jDEO<~ND`ofQmAN~K{! zZHWfo^?AO5doQPLe|~wfhM$V@X-b=Ympi_db4J|t<2(p<#3<&JyoPh#m&FRGMmnZQbdn2>(+kfzSoK zlV>s=-G4(HP5j0@tIROdFC9*zvNV5l7A=CHqvu+{co#`v=uw)|Tpg}YsYb8JGrX!D z_|5hHcZ}rjs%FU@+nMj5T$dkvRD$o{T*vFi%vAd1w?EY_uPRgBqg!mLaWVyz}YX4d$znaXRK(05_oI1|x zxvH)gKRiYsR(t79;gGpzq5W6CdH~QFW4Oe~sbTq*TbD_ap<1>#;#y?TF<}o>6JE4< zd-6H)a*w1je|ainlu-NShY;0O5sQv*IvQE)>;eIjH%JOP3X(z5IIB`7YuCw?yPE7&n;8F1-gtipB6UIx;NJ#mp`v7HXS_1h^Ac3 z80+36u*Wo#zTF7(Y~9*hP2&r~caF|8Wb)Gp+r|~pXNazV6MQfR6{3%z#kWkLpM^C4 zN9(wa+j8XBv7<*x&zOtLp{?n^spmbK$;=PXl-J+O!WJYfxm zUGl0+IlC0_Ye0>MD9zAdwLZ<5R06Qmef=es)#j6FbAhop7fZhGbRNn_gVqR!ED>cc zP8r{>&J8;SO)-m(i=9cg4#!>^+*W~!suNjL7{s(n_||?!`^bk;8uo?(H`9)&Wq+$V zNr#9+@>N{IU{noRTrNSO%5W}nAPvj~<&jHVIgnZr%5lEk8Jw9DS~I#}M2NtZ#JcxR zt&ppgu1Xx6f01d=k6+AoPt8;sHbb|Ou^*9`oMB3~huP(y2U8=rOFn!OP1HJ5=C$h% zx>9`}m?HXP4TBi`{lHMbC1~=4{$S}rD8cYM!p=2wMCu-uKsicxlbjogz)XvDVvXmy(fdL2FoyJD~Ok1R@7%Mjq>`| z0ym;I_JfcDQnmJoa=iNtr)IJBea)(8Ziq3t0@TSXBx@_s(eY)VXXYWad+hVuoU-Y) zEC^pBk!yLRBi~qVrT#*2RQ29gBfz@l_NZX!fQ5)5k)~>TEBTJ;k2PsN6Lyar00CaZ z-PET+Y(H@8b?`1-Lg;J2xrGumw$;1NkQgU|&VZU2(8o-Gq>YR}wVBG=1FDq}n751T zw2TFnp?rXxKnu1k`L_vPTMdRrGu>Hcd7|aL-K2p05udq~{6{2->WLUDeME>ymee+| zCL_vPWJx_WC9KfMDt?vESg zuDu|cR05%fT#pAP2vJbC3wpmo5XK;Wbg`F_T*azlgy&o>ukm&wk;|M;(etQh@^(Pr`dhgn?{ z@$b4_ZGro8!knO$IF{j{SIsfQpRsC-ctv8VQ7;rN>;h=nY!T~M-a0IDBqI!TEH!&) zwn0UYSL9{o7(^2PhvvLMZuc8lAcD}t@(@vrj=x?XLETp+6A>0sOO$7{9Qp?QShX#$ z-+`6^r(_+R;vN>~HW6Ly$BgP|hel-sL`TFapVh@EBhJ+thyQbzl7tuda?58qH znnpf9pWm>X!FqtdFbeYRdowy@0p=64#W6&W`d9cm!{EANo;ZWd`+*=!bT`Asg&AU( z5M@LOM3Ud4DxBWZDRVH1wPajYzYv5tE3^RWv&scU-li2rMJuf1iBWY>1wY}<_O@pE zL>Z;aA5?qccZsn65&m!f3J5GsL`!*B;h;Idar7uyZnB4Ri)Ez~Ve4^@<~&>lw#`1)Ca`iTrG`J$P1XEDdZaYxnoN|3!Bupjhw zz27ouTckie2?g&=6qHGk$p`6Fp6=CT)6-BXi`JcCNy_D4s(u*)4%rDCe&c61!=Jnl z%uc#3FDjK0{ld2NC#5DR?jFxG4Ua)-N^#L4m)!hy3ImHeePgarVNaDF3L(KeG(k+% zc^=3t;s(xk@ET2zA{5QAx-cdAe?~&^>A7i6GZv)fn?m3LDPf(m15 zrv%8{4xxTLtj|d@5i~i)U{UPRk4Qc{Sf#%^LMQz$2@F|eL*VtFmskY*j5#7}O^{!) z2Ko(;@2!9>s{c7agOTPGR2*fpuIyjDdywoRz$ej#7oezKJdoI5uih7iU5NzzCFnvm z;;{I2Xpn9dwtb@FB|HX<`wJ~J-LFwxDFCDq+Bv%p)%9RYCYB=c>U2fM;y84ng@3ci zio%W+J9=Q&qfn6|hY_nJ=lL003TYY8&|V-47)zjBFFW=o^2QIOUb>eE^MQzZbIWH1 zp1j-yH69M_N{pjX7OR~?SuTAoh{+BZ6nz9DzYw$DzzFa!w&^)c6rW+?hn*x0fR_O_ z3exp6P%Dsq)*-;-XAwoPH};ZpOy8{X^5}P)rTOQyl1~-c)m0@oQ#tjDvytM6?CXJ} zMmpYY;tXs;$rAc*S-c^mk4K6x@vxzDzsbK*|5^!7b2)tVR$|waEp@fV5pat{fyJ|( z`nYseh6@4cBIOrU409yev9~mAl5zHzLT`_iO~)93BkhyJxB@hQBSrAK;{4fW(CN!p z$>=M=YTvi`6KpIq|2@dOdNlDS9+sdcvOl#9U{HJO6iJ__5!OYo<=#`2pPH5FkUrOo z)Z7a(Z^eJhLnB#_f_Fc}^J1rswDDe%H@~hJkj4ZQEf+8vzT1Ijmj+D8TDh3xl$xKN z^trxL`;^_|mT!#y!(efDK=>ow9fn9HPzL5{U!)#+b4`ffxmJ`)?%lI{-bf-bl zO@kh>7L&y9gzR~F;Lo|yJ64SNQP1)BEEE6s%+&`9h#m0YTR6wMH#}7If1+zhyMJ5y z{t?sR+Zi6|mQ<@R#FM?dHYX=~eNMD*l6BMytLuSY2InaEN zU7{w$eDYpC0)Y((0%o=13L<0oN(y3BxiK%~mE}W@fDARQf4&65&2{pFi#2ToG8?m< zP@XyJ(k{yw4ID8+;Ss9e3Z(4`py-7W`5m#p;3=nuPpOKx!_HoS)_cx0tDLu+XUyV0 zlTM@PQiVc+I4s7;A7>Ts3n!N^LW;51Vi+&;f7(9S780qb(LZ@B<^(!{QQkIAzH9u2 z1X@4BHts+U*;=5YErB91ZBLeUKCy!=SUu})gaqe{pp{q+ncwv90)H!EE?)`=!RcxL zFttTzlKJ7+_y89*!Sw5Rv--8``M-J)sL}_Vo zT_7I2W-zZIceDZcdETyhsyzifPad#QtPHe4!Hfi&-{Dt%oEfDu%4ZOn1KE5_E^1(_ zxcGr8HHBHxl>0Y67Q^x+=oEHUQZeGsN7h#oQCF1b5a9?0QAxq4Bht)G%T=K>Ihlr; zOm*s??a}S(rI6Vs9&}b`@n8n1=x9oDOeMOVmOLkcNJxehuuObdms~yNIb!YhqNTky zg1S*_6L6fR34-m>qWx$RQTtsLMGOpG6&c$da4Rv$Vlo7o$fDN>i2RNmpCzC~FL+OX zS410@*c#|+2xaj1!2&#{!1zgpqr^Tcl@CEraw9z7wO$FEwVV|MHCgX-65Dw@4Xzuc z^7dWH;qjBMo z4Rk+dw!L`odV)8qgAp+cz!r5)*d3dN!LnjMMckEJ+$qSe&Oo7sv0m&jP-Y?=tj6ET z-g$zAJpqnSVT3zmNl*^|PIS4e3F20>MfwL*TUhjBHt&p5kvBr1ZO-aq7cP`0*)$MH z;Bl&|q@^L)r(XMntQ{NNZBL80Ek&E3i*yoKg~fYNORd zMi4n&N{5)Lvk9R1DyV$IFpX9q^WU5+nY4P+>hqa~HDvIBeYd8y#0>p1hupm{b#iUb4%1rGcaJ{9W z&u2BX*;lkh3blD)=sr@3J%{Dgv=hZPbw;5e&r9$>S1(K`XI@bc77NnDcDXU7D3ne5 zDvhI)K%BQ|^+5N1@yNH(KtD>5^t67h^I`o>d)Owc*T$pm{4WiXT1w4l1G z8ma+ncHOf&toh;*6RWmgq!Vj1XPG7|VPSYJ+8NBa zwm5v)v~*)UEcE4G+#i+TO$vMeAZWAPdETz<>9qvSN(w81jIdKioW3pqb zv)lGiBnNC}Dtq>g;EOKcp*J_<$)aB)gCi8^6Yftv;2Q;y#@{418E4F<0%u$u7b|9` z5nOgk<=W>oA;d7zoGcsI1U&0vF*&`Y#Ifl#C0cD{Pt$X%jY1tNF6Kj{`&8$xunE<8 z@&2@ME%&=m7`2Oqtoz!u@(hR4p8U9jW_Y?(YyHC!_l#|(9cyEyuRWJ!NFr{Y!JX&5 z53!Vv;B=_H1OB|JPbl9ju2+^%AO@A(i&6JJvjj8oA}E2V8vxr;A9DJt4}7e)RPPZ- z+`=ug0zjJ;Vzp~t-R|YIx*yMld0l{oLlc6P_`q!Eu6sC_HbRO#qY){5tMNJj*$*tP{Ea47rwW!g9cVTaJ-U$H1WNy8{QAx_R$D?-F-N;3%Ccj5@ zdph3Gw@1x#Bh`fD-hE0aj95)9@ju>H`tp^0H$Qzh*1STF#VL%te`Uu3R&;{I_AOw~4tTarOXLnRZl=9sfX|dINSxn_(!n@CZk~ydP3nB6xRVVXo9boTvxS9!1(N%fa3ofX>;$)ru{(|Po;s;U zT6=xF?%cqvwzTT{{$_Z>Sn^0nd^Cg5o7=wh=#**`SVeNk3F;kn~T| z8m1#!wNq!8Sa$yi-x#0tpnSsgPn%Tal&c`OQm{(YI-`)D$ZT9R)e-z?N-O#HQN1($ z2+#WZrsz*5Rh67yUFVeuR5r^2$;vqfz~5>Tl9ypM@na6D>#N4}b>KTp*M?k-hQ-qE zTF4^bLm>!T7_qD*MDQdez3^;Lqa@4xRo_Fj9hGpjQee<_#=5hi&xJ1IArQ?35G71U#viW2tuwE+!rURbqXVf18HfnismO5<&6ohwI@i7M~hqwZ#V zK7;*hXRm6nUHt~b_MUEusvp84m`4q*_UaVUIa&U=_S+td4O`;pgT`5(7+xcN)F>nO zdzayoWJ>HEbO&NQxZxjX>)=FzW2G#AwQ9?LL0N(M=eRuEb;k^4 z&ZMqLOR_YgvZX}b$WFAPg`wk09=-K-QTnr*+~Niog;;(3K3UTIF+JD@@+C*?SEJ1Z zhPiH6e!j{jzo9fPR-dQt^1Wq(4!`7e-+!nKuvNw-DjdRIRTw{LZUMEcZO+fNjx%uJ zB&8(a@Gjm7%3bX@&(=mYB+b!m8Co>uxqi#cX2jw<1>7V&I6o#!%U#T0OPVBaw(Cp_ z;_l}jC?{HVoH+V|L#hr7!l(04B@?>VeG#`IP8Xft^@^+2N<*Pd{`Y{+bKtJzYExA7{iQxb9Tc#J>^64sFsUjhV99f+(m2O6es5=$yd$>+dMRkb z%~FH!ZV*d`J=*DWo6n%=;M3>GoZAc%x2;s&68~8r_SLo=j8bo`^#l@+r-hT%n9h09 z3dg@J*?qE*!cO0{t+r$&!OhTj%NBP{>H=rRE<3+fPj-Gnkk=U5#Q^eaAbi}v!upPD zwi_kT1vZj70e9QL+pUgULoiSkaX0YjNY~7AcmbJJ@gAR?leLa>z-s^b=cX7hQ?bnK zgmBBf!u9wctry<8x!dZxohQqYCmfU77k9%K%hQK-|Ka{4B-tlo=E(bSTgm>qwoY)4 z+%U3;zJ)t}Yy5%Hd6M&+qWQ`yGwHO0RVFZopFlqVx$L-$b`+Ox1^IO*t@~ui4n-_X zmHk2yTg%N>9Z^P>_UX);Ia!5KRI8jr0Ub{=Cd0id5pY5wudtG6c;7NSCm)3jrO0=C zcql%ZDScRO-#fbxX>9F&swi2L?O8I0LTJQdw>WK8J@wM?KIm?oY0LpF0NPCGMJeln^>qUg1D;1I%3 zu;)KG&j!plLcD&&Ig)mzU{h?6$%B$B*fH1YcZO_k%QEPw{|{Sd9uM{U{{Nw-MMw3MOug;?99c+oDB~s7hE@ZLQ&V;^WG#Z1yIMZ%pz{*;&tDmNH zYh=7m%u7|N-}Kq}uF;Fmza*p{_|23>WQ35RM}_3x^W8hMhHZEgE(z0A`8XZ$!DKqQPd_SmLp8m{fl!b{sdHHCNT0Lrlq`cgJ^? z$&RFY@hc_wuQT`@2u@Y0Uo9TGgWPeZ!jGh_a#@(-9(8M#I)0n*dN_5;SyP9c(P{&n zh7T)6!u+E`>9JSomb?L*551$s^EqmL9roPhf_mfQ2dkD897i}4=}A*Nza%Yf%3gqS ztfz`&Q>&MUVBhVGmxF8O0>?H_yFt*@g9+cg%UoXRZW~DRc*t#fEF(8)WdqKd7w*U} zpF39;DA>G1bn;qWjoSxU7zcG4^n{nKk^>wfpEVr{Q63t-rS@&$C2x>-%9_2HV6WJC zMw^%`$~j3G7aR|qN(k-nIRUR=%DLJ_FSax3REa0>0@}Uqapmnl!PEAq$Dk(D_YC|I zT|-b36tfuw$<5r)PK*T-gIyn)#|RNxu-D|6$}0}{@nlKH;Yk?_al+n80tT1wYO&0N zyi=en)!&?a@7;C$mXc-{cDJqlMKr>X6-Jrux8R01oU0heoCj!Qn3l zTRw(V2Sf8E6P6WLVY&<7Yov!*H%m($e^4CM?C8+?z46h8#{?ezK{-lD&Gm!XgHAnm z#h=C>=NQ>+-+n$h;Zk0|szh1Mc&yHiV`8!b&Rhm$J-0&}xI&=^+TDLdQto?(>K*U4 zf{L8CW1IIYdk4Bdb26cLGnP`D&Qgr2?lw_(;ZPv^Td>tmF zH?e>l4oL$kOsCY@7CEoOne`2iX!)F3BNs=#m8$NNOl1!^A4u*>mAJyfp5+@9m^%{N zAs5$^6hf8PP99eFZuHk#T*)-MFm0&4Ku^Y0=J*eUHJp9vj&*Uin-e7G zF>{u2-;MB}$$3|1pYd0UYQVjACr#g74y#G*BVGB?ogFT+v7B2n9Tn*^bJ9jV9Zn&w z&HinYT3tt%x%rj610PMTE42UNLD+5sdb4@2>v-2EHy%t8s)R`P9j>o$H~BU8z!^-W zPiaYa9Y;WV+RYxN+aEekTnE#)RF)>6VpCPKlcLa}J9wo_bea+?ZYH_^6$~L|#!~Mk zhdeYJX>llSY-;{KGxu}cvvW!3&aSuvWgE9fqAF7P_pZIKH9|PndEEp9~{4s3~gMm=PhO8M8&pgxnt9{4T$2lM3 zwmwB_eo$blIcAY|;d`cFn`XJ;J>lS6$Q~_{ThV$!a7*_;{9itx)Y0&|{mSE*kV5Wd=Ym~j+y>X&2d$O!V4XJRXfBH%Dcf%6DQ2QnU<@_t z)uNIM-%5mQ7>PL>A6w0F>bE4ncZ32Q=k4dgvMuw4U+V#Pj^zP~qZxte!kb=g9MkT! zJp9xBK+DK2QET)kAGaYL{PcO@i)Dc$z9y{0ye+qk zyY`zHwezU0j(xg{&W{i3Dz4|ni?ILQB`Xl;95VSbJ1qane0rk3)g8rUn#$Pu92ZI2 zY>VmvyL~(vzGtESP6<3nNeqsE1gF?9xEZmQLEf^;%!4UY``Yy`<(kNch7G7han8{g za_RSs2fp;>23lL)(d+XkQJKT#56-#OMyGR|56t~~#vV;N+UXFn(y=`4huz}1eEKen z z53s7}EQk&(lJYc7Vz-@*FzD{JVt?~p*d*D2=Ptg9o!rJKiW8>Y@v2zT$r?s|Tbkj3 ze8($__}LyprdsaCAimiX>T_OSzaedPPn(Mz5yv$~<(7PSUnTqoxOxJBSk?+BsK;j9 zN9T+QZ!N6E)){PfpXFs7(?pMX#Jiov*qYzP6ZJs}oNF9(IAiM2V}q?TgU<~((BFNceCNc;SnZLJoL^C`o-jKoI8yj;Lb2~fl>&*WRn>KO2Jw4FxD}BtKZ#=*0WL)X zH00H&79BfIa^?y=GIZF;!C7%WT=|Xw4oi07#hSmm8(6z5dY6C!tfd*l`J_$!;`h`> z#y7XVzR00K%(X7G3gp{cD`z}1u77<@$EGERI5@_&>%4HFJD=jhAlXKrB`uNNxK(!S z%3qCJ^sVteB0CDF#Gt`ta*oP56EZP*KCkJ)uUIxtrv+dlINxTfW5d=mPacWw;ks_A$N36GNy}{n(v9We!x_ya`|CEr%o@FWDA{7t&IMXD_sU zf8k}V;v8rfGjJkf0SyriKjM=z{?wB`7OA^Ab?DB-! zFL~oNhrI!eUHT*AYT?TBEyWJ~u`YgTSLRx;-JAb%zQZ!Ybg^ENyB~}J`$uMj_?~p7 ze11?IC>(W`fBdQc7Oyxol@wu3<7w31oOgh%G8dt{*p(RerOhZ8iq0HoHU`V=kjlOj zTH%Axq{;j6mE&{RzZZyY47c9_rJ%=25Xmg;{zvZsLVsqS=?#35tvdiI(YqYpgUrg> zNl*w6>vO9{Z`iu$GV+TQN0?SHVI3}I?|#sl>;BK=VL?;lbpcKJUEuQ0x+{53|+CfHCUA|~;-m9s2`pQ`v4UIcvq&{4*r>zvnu2aUD98lS3g1gBP|B zr?3AM#5_Y2|Gqx~MhYh^%g1HYZ$y+%HNtli6){buxe$q7ylbYdYDYJ5G z-@>o`#lJrUXr)HyYXsliD{t6oq}|_kV{F+C>D1~8-Fk6{U}KR01WEBXmUhr0b!0y& zHn(5o+@}Qg)*s-hu7}TpC!?7^Z%UQ4B z%^^nY-SG7vB_7~Kw`1V)N-L`CppiL_o7ef|(q%Hd^1cV${*yD{v3BAweQWe{0$fwP zKjDEmMYuzA8x|S_8oDHAaJD}S-*>mQUzm;UA{F>1GUG^Im?I%&r#)!wUf?Z!IzN$q zXa^%!ST|YOn3@LOgZ&{$?DvhMTIc!kwa2GSOc(flrDv9d6K_jX`;s>ekd5datLqy{Sg9e4&E9ARJpHfd=j{pvs7 z(T~_hCh4HU^9?K&9JvVK#RY=nnS5oM>Phmro=q;yL(4+m+>KWn$Q1}dW3I|`qPGF$ zUD`?wajAdJ&uR7K*Ry>=iA$(CZ*o{p(~?-h9*&?#BA?W$HXQ3aPTqJIJb^wWwYd`p zDi1n!k8M|Q%#fad8S=mi$UxStpSR&pC1{z2OG>i*bisD~9eYVeYw!w87hJd}{d`BgGlJrwv zYy~p)(JUtq&Fl*+u9uT{i{e&5T$taQK zsm%(^4UEsP@Ie(kfgWf9XmVVS)F6@L4$Myw=LjKO?EhZ6`$6C6Akw8I@`3vP3p}r% z%wkDvRF@Q0wtbjKQnctAqnV-w=vS4s*0C$kMq~r(1`4D>*>EAfn)$qUS$-rKpQpi_ zCw2d`od@WKKM!C~P^Ju*8@ny%I8^R<5pi z6q}pFP0SlTP`HA$a?q!6FK-0-&XwBwG7ur!T2%Ux2VS1b4jr-Fb+n4Elxr{^j}|k& z+h@;|!({fQw<{n!3KB|*S%(>K5c(Eci$N`Z4+mzP>StxU!nYYv)10U71>HKF1Y&MU z;jS{$G6<)|5yTgn=jt(+h%=-BmMJ;VPqW7sNR>!VNOk z-x^OSshH|xq?>5U&;|-o0(36L2#j+Q3TaKt7KY+CyV3Mho6E~lIiZ_KPia`xQa zF*SO#!ZEYa`Y(?VRh)>ShMB6fpdySy?vFL-ym2N1s$hIuSx*CEu}Iwb>>cT(HiX0( z7I-wDem&fegxj6|zEaKirPbVC!9AG^NiR~IopeCjy0mD}lIu@Vo0*ML2}xK;N=yi} zBfg}%{q60V?4&We%&u=BdxlX5_K|pGf?(ALl&sDJ?%Bj)QtI3_#jcL9gcp^5Qv$hk z-@qWhB9=T#WK>`f>B88@EDh=#zx}gtiD*uDuJ?Aq`Ile)xk+_)2aBO()8}0)O10q) zu}jpEXTZIxS#DnknJ&LIxPPj))O&u%7wswO8-fpjA4Up|*yIN*5-q*&ZlncYV(%rq z$1|eKf6aWbyZmuW`>1IH!9kpntoL4x;cYNI5AH^IGe{Yk0#O2=denD=m$WdK4&v?` z{Fo3Gwr*&1U_J%@o^7rbiK+MSwWF$P^3guObb?Npq@x#v5ON!^<1r-Dv3$L;k8XnU zHo$6~C|euBX|Bw-s(?P;wp)u_gAFz=tCa!RYVZFl+hli}dcIipww4p)(M8yqVcq8T zXvOVrAB@Mm(j>@>6ITP6cE5vGas2Ejk>#tf;T;Xj(aSwQif7_WXdMJGGUY$`qkn2E zF2)Akht(h%KVG?gN1k1el*b8BjKV&*AAWCSjtI>%JuLWpH`G)n#ZcBrd6~n%yG=sN z0#vt%4{b*`!@NsB&MCR=$p3%WJCP8y$_%ubPW%ctkehb63C63VxR4Z3rn{C$SG=eQ zmO;R$Gvz?!E(!UJ@ZenroaeOjiU}Vpj!Pj>E`gcn6Pj~5f$Q==n52KIjzYA8gDI%? zDKw{Jkp36X^AydH!;V}Mr%+wlt$L7ZvCwpUEISv~HB2H6*?WfkXmi57$FuXN2#P%{_)Udy&c)#mt5;|zG zItxyIg`@u$oNvWw1yomFA+0G2OrH_%4GC@6hM-+u4y}9SQKgNbwwqpA1j8E85exm) zqOSA6)^327Vm*qKfboId4FlifhqAAyC30|CLcPX9Fjv*j5L0SG(DQGQ`V++1u*(q3?nZH;jsg7?DJ=!hHKjazTJ zGByipKqY_64UUSP*!3kl{YR-bCqSkI3 zz;oLwy*&0w=xMVDIJlzjxkL!+C0xogKc5(H3#Sl8-5WOmm^Cj~35r4!5JdrwDw&!N zNE(2pOWJ$LF~u!`SvYE52|7q@MPO!&?4W zym>2V-`0d|AF!%TMx+%a+-pc?U+ngM1-d(Qs?oX8i0sZ@&H+r_36wd#>ctT=mD?xI$v>Sq{w(j%B39$h}o05KQ>)&Vst zLOyIjDZyf74(?r9=@X$IRVbS#@@yVoeA>hW8_s(U&vh)|onI^MU+U(jAg~?1SG2en zUWU5BlV3KSfI53&w~rO*XsvUz;6djz<+};GJg(`lfPDc%O3XBh?fdE#H*%S`Rgl;U zqtC8-!}-6rTcL$4p@B;AKX5fskBpn~x6aTt6+J<+;laFh)rjbuYi^!;K5X~;t&81l zzBSBEsa=mQqwN7U29^lJOMq7T7$&IuYCaX1MOs2vkeXTV>R8gz9pt88AO^C1-w)Xo z2o?Pl`E?79)9z$RgLR;uunYbhXpuj_ub9h6X7xV@8H>p2rNzW^0GY`TBvm5qlfBl< z){jo#E9YM!)R0B{xXQ7A-YW+XITnRnd?`)R!YEuNx(#6WIisi%+cA!S^2D1g9Vu>* zwdE?ezG0SIi+Y~g^~$3F8|U0d1NgEBR-wfJfVe$M>uc21?dQX<=7G*shAoK}W_3ffJ9TpO z%5K52u+~D*L`iu``MjajDkB*kj706%*&>sr(|l?MK_Lf5p{`NN9)t|thw%(XT!+J@ zA`dt{_OAMT<(ZB!IZK}$EIsTEvcS^L>mQ#d#ppd3KR)dW5cq#@o&G@X79gi3ezyDg z{AJNK1B)pzoXmDab6_4mr0As&$=%rXHT60JE#RzYo^-&2p+ zB+a6fhNYbbJL6*#brbAq=wz38!&)O5p(ec9fKOnX54bPsjhOAY@SxRY`t3;uev~9R zy83SFQ$&|@hXJ9Ku#R1Raa%kBy_D;~35Zm|v>A`h5!Ti|IqvbsM^T2#d5DKmV-7p39qNC6U&9eSeo}TgV1yRlpay+3m_-gzVsi6 zSJ?CDY`;ptd4N_PhM&WOmPKHV<#oM7I@%Rc6igvA^wiAO9eVl`Rg_q6NKq&^1w&F| zB6NY~J#HWu|M#675}55(J@(}d-jOIzh_EB3Rm&z0=HGqcZv&2M7oLKUJlw0oAfA?aeE zk_)^;H5_ipusoSP=oJ~ZV+x6`73DM+S70OSOXKT?F;1U0a182E1uPQPt}qIQ*c(PY z>Vpjc_ccMnZgI3sn>f@3A9RgD@;@{&G;$Qf9%HYjf67nqt0YZX??c-mBHY^HDiLc# zlZTw)WJpYd94RL?A9^hu02I*FJXDI8<24c3i}sES9A5fXi13HACyE?hcSgF zK%0>4)+>M=L<80W^!*DU#MAksT$|gPHf!2sYB&#*4HtFJQ}e`;JewQa;YgK2Ti`r% z0PvzluHP&3Y3%JF+H&kfaAY*U1OAoKhM1X6Q;d= zG~bC-OOX8wgODE{Ze-{#ZH{AOk1fI*vm@}Y!wxY_`8 zcP&j*7RuyU@EMYe6QA;d@ePBbayhe_t%R%fu#iq%#|Q1zCF{BwegbJ{!-cV|vm%$y z2C1Sw>J+_VaA?A-37U(}{H0!yUE~U*A=&rg6Qvu*7E^!g$EL`b`}n_Y@I`lZNTMwF z1)p*{SX|A}SpN6fgW0($O+7FYA2YhN=N*cKsr7AJkdWXalkSVeQ3D?YDoT)6ZN4V= z7wJG%pHF#}gD87>1mV*TYfM~36FqTz#glM#VhOLKTQ$ofes9YTZ@2(nM z820t^VYvt^Uf)7o+eJ^q3%Sz2q)q9jbR#AjLbKt`_c1$sQh|M){SBHZ=c*bK7nBxR zH{-^I6_UnRP$?$e?`f9H6+P#X^&(4B+c%#lD(LJw2SXw&r?UwZW;YuPZ$s;}=;q%! zO=TNIl+<*7dQlBcT?}a;BhQZQ+GjYWImY$-nJY+tEW_+@Ti5E(eFGLLyQ&xxPe(sS zCv4=PMtF7Ew#<%SuwM7Zer&(2C@N>*o5rBNulc2w={(zd@vasrW3oBv&Lsa0Q0aE# zUV+@%@7~TjE0)%b$l%7C1~pmVoCM<*+zp~t2MzY1UPO+$n8CJhlDQDVGL=h(_OvBL zR>&+6>@%OTO;UNs4yJi!;Dy6zLBjqsGaz-1K_d9&@cl0W{c}#WjClDkDC2Vv+2ukj zFo$m~$e^(bWOeYW*!-1CQ_np1Ay?xUJWl^NvIl0ObC*Y@g^4WBDCsIEvHtphWpeL* zlJTnjdf1M4!2B=|S_X@x$$9(5mFs=5v~`A0gikaXIkP2V%u*Kyv2nFQ2k5x@8KV03 zF-C?(N4a*bYWsEQ@1B0K%7(y@H8^7abvFXerh+W;j{nMR4o5b7Xd(5UwU0t$aH@y-cV94G(6QLL3TjACvgL2M3bky<1a&qnm`s=*m(i*jH1iwsrV)Ca< zY7G^(HIPZ>-bbqLblYb0ZYzY5gqzwVQMm0{o+2se8lli>Fone`*C1`mWoD;OTNzF} zjm8FR%yB4Gj{y_7I9L1SM#kAq{gsAqgfAy`PNQ?0rdb+s_s>yH=G1P4FYxFUse9&h zaoFAsqZHtMh?`D4ZA}Q)_LHHn@-Za zzDDBr*xdd;7XmT7yq^V}QKc}`}VK(y? z=<()PJsF_BolW)-AY1Dj4y*xD&!ul*FQelVHJXmKxOE-XMD}{X=~Zp9d4uu{yDgtu zv3z0hmP5UYhAXMkWQ*@L+9zSxo?zXEF+UU_I>>mg`@s`S3Aq9$I9H9`@@h6c4>o+h zYLGK}#SA`|B|S6&82qwYunK5d;?7mIW30(`VP+L>AJ#N2fm|hB9A( ztRL00myWdGINzg_*`Fwplh)aCF>dX4EAA5z7V(cD820|p+k8fTeOA{!{Q3-oC-$)+ zCg51Us}g>J$hMEssS|vp#kYDkc}}0^U9qv+ElrJ*=ZEJn<-}XX@)SPpDA@#Ai>VZ- zg5P)vUA%>2!_e_z%`gzr_Tz+&=U=j}Y{8}|Q){%ps)}0MT$=7<6gvzJXP@NfibWa$d)>2&0swm(e+}|B?Y8 zazDzL?Z7A;#?z0H)!JM%ebd}FtZIdE$9DKA%J2O$x}4@et*@hkN;xaN8p`fBh}SLR z9-bzS57x;wQe-pOkY#y~^u^jZ9=5Euz|vqs5LO}VtlDt2@Un}R&}TTjIe2WtlIJox z)kTQz2Cf3)+7__nCoYLdGm=^9NrM4NF?BymDf1v7YTh}2yY05|k@{bXhR7Zqn9 zzfXBEzurhd^iEw8!OMXctN6rgQ-#HDuZSc)V*APSR9Zo7&5$t=3}Qm18S(Rgh73*& z+tq3$*3I}>?rP0~8+`6Qp0QwG@;~mk3dfN(W!!Y@Edu3AJ13ZuACPWST8Ul9CHb05 z?6LBJWZ`2SDH^$o9D$3{-KqXluQpjx9CgEM%Nny@>4{F5F(UM~t5{>g?@b}C>RoCzD;sHXpMy*LMAYQG-+zh#U=-1^oqKQTFgamw4<_krg0a^tt5z)|aaPK{ zcLcYf6V=Ls2=b+VI;5sdV_7(B!QP56*e0ssLt{)?b82IENl20JjE7<4~$^kl3E5t{TK6K99NCgPa1yL zjcZod5j%g>E(S_6|96Nd5I+QZ3{9^rP5;v!im&OI;rA>~=@E z0|c@e9Ohf(H!NniztHE*n;wjK*kn|Y04=*uwEi*Ozeu+e`=2AmTNq`)S-wq*YL@7+ z#bE2bHhL*@m!VWVd)A`oT$BwJ!3FW=r~aw69!I zn0vhAJg@Jw=1GJy37-nL&)ppbX=ZCP#S;|9IIz$r8xksCXI8^Wu&d7)qoxgeM-HMP zQ0=Xc2pMb;_R_n;!pfYPtiO6sYm6g3UU5{gpg0y!q=b}088`&q+$Kuh$anwqKfm&NqC^J;{jzFhZA@Tp%>2wo{$6=j#FtmI zdkFjy!<`?MI9A`wCVtllIoSGr&uPz+IK=dl>(3wYiiH}(wO7=upRv;6gsVL(#KDLf z-y>-CSAg&n4uu90`J{}iZ#_hxq#=3iH}Rh*0&g97(dh5ixEmYAKCM@#%E0N->7cRw zw2(+_AZf9&xv8WmJmWM*jKo0A^sd}~<2b!Bq3nemr^DS($q0%xyy3g#wjmV_>!s|6e$P& zivzvQUXgpoRj$Eij|Tg7wwcB{>cpix3srPSAgkNr)??HJ#_Uqwi<_iNY^%oZHs0=X zB|c3h9A53aQf(d*5r5|r4}He|0HuEEn+p()*w-Ha9D6U?Q{CV>DK~`4LWo| zgYHb;d8loRt&0$ZXz5hS10j_Yri_PZR-Qf+`vq_y7p!&#OT z(f~5LP36`nRzLK+3+CA3aFxuGu3O3+vq**hQVol7?w#X;eOyc5A;U`VNC&rV+HC@b&;$iqtvk1#2S3x$W~D2)XqjfeQj!AZ7| zbwg`7F&{50tQkYhH~cc*Z$({~=iBN^iTt}YmSX9T*Rzp3NX~@h5pS*6fmWe46*`>a z*ehYz&QYh@QxxP!+C{EF+2mU7LfZH>f!P)}nI6_QCfc2E*%W(x2bBUU^7(wdlvs2Q z%?V{0lT2AQ-@y=!23*gQh`7l*iTKFYZM?J7dk;myaYg=bdCq+8ri-%vI?ijIw*`&p ztGXoGw98d;1|!eI>fvyy>IV|M16R?-xBqx!q$(fv`rlVz8k+8(P_v8Yl;Chl&E;l= zFN^kB;`;0QG9`BatOlJIyy;Ofh_-zo1mN?f8 z$ES(J87L6*!?jU%RQA^kvU$Leld!CatvI|aC8X1yXQAHHqb$@CmZusX)1Ag#h+7~R zT*J9j5-;bGtSp%0Zjt(7O!H84CuK01#)0eKDddvD*&`Sihc6LYjv__u(!aA0Yf(to zNS?(}PjUo5X5VxnNRVM~FpZZl4R-(W_=yH7u+<^P+rQmMxl4|G>s-rOcpv`mSAUxv z-EoH`4uFplP@u=s%G*YGbzvdVkH;@U~E56gW_@s3` z>$g`Bid7);Zkx@eNw0S)UM)MHWKIt8b1Z4pR>ugtw6_|+T!(qk;>N{Euie*_1kHn@ z-LHY$f+z&WaB@BYMar=?YRQz<5eoEAJLl^f^x*(nXvWuKB(ewV6&9juO+&OB$l2j` zf*mL3$syHlo)|x8PNTDfgKn(Kei|%3nauj*Hoe@1^#*iVazlHOOVBf-YbLv%LnQb) z>!(bGrrVViIo6?u71uFZ4C>4+WX@_2`xl!8>?8B!AD;Ap-r0Pi;DNP`9h$>sHCZg=?YeW;q4c=6vhG^`1`W)s3*V7i}KoyH^cr#D>>3xx?T2Y z?9{4X^r>6+jUK^cJ6J%DAfiX<$0N=~y7bf7~}X>dC$w%X_#4nnfahVERj*&8?g zzAHhqoqWN4G<<_9UF;IRu&*NZYJ$zdEITqx zE>uJ_e8VTK&#z|5bMjl*H+^pv7p{eNNq>{%hU25ZT9&h!%{Xssj2`VU!oTFlraLEO zK6F~P7otYC?#mpGyd@S}PXPxMN|kZNCu%P}l%UMKH9Fer>Zi3Rf8n{NqBORj{qMFV zgG9WwET}~+JdE3Wj&TWX&wMfXn{1Y+{$u&#cFx^0&T%l)Es-gue9uq6MW?x5>g&gT zBYU<}9VC)`pCvBbPmgtcXtIK&O8njw5|tLb?g=rsHR-yb-z!_Sdv6fsr}PRVI6N@y zo`&x|=$7<`n@2)t{F4|dk8D%!e!lZGbyl|rmtGx1j1=B4VRie;`C+oPo0|8$9%K}Nu{<-Ok>XKh(K$Id~^de%6 zqod@=uD&Xp9)N5%%k1tqiGvRfCX5TwB7axzpVc2O34K^JqFt~%v# zkuH|OQOEY7JVwPoUNsP4TK`P%=5^Ra=zD8o{N_0IINwg&ZW*9(NY<FM-D8mea2=8AYbzi%Scs!0AQ$l-eVnRw9yL^zUsk zPb*-l5b9b*uvG*q(w1VEV|MVkn-CPesbccwEe@4{8TYK+J~cn)ayCM@tU#1;=-cWB zodT;~DamuXI2k%;QbYjhMso2H9cLkx!yueOT(pQ%c6|>JjoA({S-7=bd`+8uEREvK zuL)dyepxZTNeUeZxl%qFX)h>nc3qV3Z(mYZ%;+L|G>B5Wpf5SOC*@zQgl5|J+}l6D z?OwVa&~F5`$e0Tzg9|muie1`_1-gZwR00 z${I|$8ShfB-?Ru#r0z+f4^e^Xfd|F)46!sD)Dw;?k^NLrMHSi+H-R%rE|3RVOreZ9 z!YhiLKcq{)+q|m|XP3-$*X2sf<=#v+16-Jz?sb(RDnmjgrL!I;x}^bM){>auJuNmZ z0tsjg417w-^LQ2x(JVF3ypgkMicef_wquG(w@LJWTj|}QYVOy~uvuT^Y1|IwsC5MN zN@>KQ6tZ!iDD);$RZ;HH(kH8G%j^`h%+*>Jzi-mh$c+3mo&F=n1H z0-P_Fo0C>!vq#WJvxX@!PCwkXbFFOaVPw+vCG>raKm$UELh_) z>hi^CS~vdvDudQKFA-QcCS#zTOxoGeQV1%8rn$$Nvw9|v{4TtgNWFZQ=uZrNuiuny z;iuy@4fC!()0)$f@-X6);wr0A%0X;otI^U)0GMvA=+k-UNlh+0++i>#385e@(66oO z+za}NZc$&2un5ZGI~z3E?Ffw@rypt+$^xa=+MBoKfhh=H%h|AdLA^UMv}YE4`PjRJM3$Y*Mi`T{Q5XB*mK(k9Erh z#v=QxV`_T>)2tMav%2Ntq?Rle`yFMA-nV3Z>-b{wGE3*DyJ@o73Un(!zPMW8$=3uy zOk8us>qqJ?=vHg@SNZ~((G&O`|AfN z*(Nlj4NnT~c{ZybYX6!2bEjOuolQKaMh@adB|DZ3*a);+ME4blUA=Z&_4VyDOzd{J z6Fm@p^dIq@h3)4_04a-uvO)?J9N>GiZYJ3&h4Val)9p{XXI)v=a!+qgwzUl5&gGwu zwd{N#Y?(qOV&^1+-NXfXbGjf+M~aZ=&%1~Vjfk|W<-b&G)|PuX=#5b|o_4r#(L(DUUvo=eX5W1Kmdn>r8;pf*&((n#9D!VYrErk$b5_scZ`L1cr1{x6a z<;pb*9y%LDWE5+s5@o?;&S|XsTgE*9uqemyRF)?QVscV%2x6m&{xt(LkTUvV0vH=b z9Tvo7YO($S2bZ`GPp;QDGMn+aC$YqKJ>9-oj?}i9ZhMIr^LxXJxinueJJ9lQXE@LW z>lZ9!yjKVp8R2+9A_#B)c7Cpx)$wtt%hY8bX)4sTqVHJ zC)6Aj`KuyVr-hbMb4|~k7}|;>?U289XaAsi+^H9Vlwp8S=YR0{85@o(0yuvu^GyXs z{ORv-TCUx2JG{&I&cxXMj#PS5jcWyPi)_WTNR1e*I%{^=n4wva%>)y@oO18jm{$&M zfRj%yxBnRC{tz5C3avZ-bFIt{HZJ>ZP*zUV^!HbCnIrmax$xqYhxA^>ZbeDD!*E!B zn`549HEuJl94_KQyix|8hl%kL*tzRbNN}yTUgn7a^&RR33lU?3*}I0KbHnZ>EiTX< zJ}~0XQX=oaFEn}39s}y_J2OQo@3vIHHXEO~RNu73!eMtCk&HyyIBjury#WQQek(cm z^rm&zRRQd$QmWehEeGMF#1(}y9c*PQ9@u(okfQii*;>ibjh7VN$*1B049m6gF9k=d ziDsm%^{$$epRJkbNPU?l8C+Ny$vL+K^_uf_CWS8u6`=QDu+0s$_757nClJ7Eg=+C( zm(c@m8zkmL`YtXVzUW-LUXTSs{B5rI8GIQJ_6)*{=&cIzupc$#`3$qq6CkV+hdtr` z)6*J?KBzIW9Bw3D>!ycM$`yOU(qlyzh0i>}f)5vvVOH$<(-9T`k|KzsyEGAD6PK*s#{JDny6-ZYVY^~6Idh8|f`7j)dR^wsX z?v*qkaW0V5mGR}Y#Kpv$PWqp6Xf{v$HhnP|5(x;d**%-_MTL5sU2b~*08XBR{a8yG zZeWj=D8)qqp&*g&l~86`j$8f#t9eNXHH@eu6ad<#Z@8IZq>|0mu2sk`tA%7Vgd5DM zT(ntBZN6E-%N5zYu#1vz;2m@e=2)5_HgRNlww^YKddBh`8c!e%N5}^c+q1R8J(XVN z@)5Gc?liyCKxn?{v~Zt643ipz-SV-l7c|Wv=zTclFjg%kE+}b9pY*zdZ?W=~$`;(H z(;g6*aT$#P%;Q~nrBp25)W2Ku$ahl6mcnO|hFnSuqrGzkbU_!I3n5H^X6+ zx$2wLlA@~Gn@|%d3^KEvhq0iREid)3Ho=w$o?f&%6|W6;=SiPmI1}~Wj}hRb_uv6$JWBYCNdcANPXG#3Zyh=x~^{~ zFF1lj;M4JHiDN&ZyJs7Hjv=SV_5^v}&9Xj+2-JC4gPFGRms%T4CwoIY8da%U;o~JL zvIkx-P|oLy;-BMoxSKw4OK_6lB#eQQu9$MJBlO%V5^5Z|_?+izq6p1NI=sKbEy((0 zz^9bzZxDlOvN^akOv)9Vm8DgEofx#uTq0WXgU*%Xw`>87b`8u70AQfqtUEmRHf@6l zQY=jh<@W!z9C&ZuTrXwUa?QecS5xne*R1o>2CT7|#AAXDsjZsK|8?V8&OY-|NyC3W zN2P@2=`wA^67;`kUao`H0nPqxx8vw0gFYMvV;%FLA3PTaO~r2!y@M!zrB*|p*7|_- z$YRhvb}Ne(np;eX@hpvW=d_3724#Wx)#IaNZR?^reHeQXk;m7Ku+$-3q*O3BrB*8V zXc+BcfH+cY>nUbFxi{5w_D(}|Se(LWVMn5>{32)#?0P)*V_FdjsR2T?#{T^z!LxAb z9Cbk={yZ_PQad~!RsEyz=P}v1dWh`&AZo4 zk0j^ND`@b`d}rZyf@~->&}6SZlxL8}f%3~GrZ#AQ@Tv3W3IhuMEgEe?{e|o5$~KaY z*>+KAiQ8l1_$(MvKl%9MSD0ExPOCFr#yUcKe1>0?(9_wOMW3KNR^Vus;RuBj3EK|MRJ4yU~@IukVUp>5oQf z$usm%ZNou-wjRD2s~}(DH`|TNfbp#rZ`fc{Uy4K^X#>e;Tc5&ww=b~iBlsyia&|he z^V4h6k$kTsCKC{lk=Tpa3;S+{i4cwsFNk~MpBu@mb7-Kbjs_6iAl#y*$00A!HV(lt z>!yc-6tZ!l_=^UB%{74W%vQ{nak7&dMsnSMqnc@+Cfq8M;*;b3+BxpR4NX>`u*IJM z5iNKri>Z?WmF9bEiH&eFT}%E{k?P>v9t3xv37(r6p5g2P=&A+)&Dm9_-dEJr!-7&|;-YU37!C z+2qOeF}t+6Ae2Y3{7lmISq;nMcD;re=Mxl*f@i149~~{L>vbCpm|vI$pTsew8SXiR zVduh)KI8uSPi*HOYaT{9{c{8KGwUHEWrBfc+KnZ$`+WD4n!k34PiLn!zD$vks>g2u z`3JUQ7XrpPgf6FF4)k*!%TAYw23&NOrnFI5Mr}T=*w4_^ixDo*4LI>M@&t=va|xP;eDQAQ-6Z|5Xb{%c`*6?xIh3t;C=d;+;`(I z;B0ujToZfF{@5=|_VxthQawO(NAGUJ{A@3CxekhMpJ#CYF~9zZLVR9* zN6rZ33klqp&h5ftC1geZ{*wI}IvOLVWc~fW4%yOAVzb!IPeH@e?)ez1E#-?cFJ#*w z=>5*W-~Vlm0w)nEIM>uH#a&T3peu7h1}vpZl|aMApFn}la*!!6<>ARbVFXfdWQ?;5 zDna-~l@^9< z>B!mizYGGH;8CE3hC}oX%!D<-^Rfm)HO469!7;uw(23=rIL<%wZH#yy_z#13y3M9R zYHw*Nq?$tXRU8F$X#dHH{&S--=4Bv29%3n@bQ!@fhdSqa?+*9_8{ngJrSJT2$BUb< zbim{~n3Qv#_H@gsjya6&hm5O}v-GOfGBRYxIiimiEFxu9{}8s3L!cVo=t<4>s7aM3 z3$7jw(1JQ*X%*HWc?Z6%Lf(F`4x~YK{eN9{`z$a1v!(^;6U9-kmpU^hWB`^5$+hg~ z|IgTo@zLl4^OrGG@$@L_rlS8@k>ZI=PXGJa)j31ku93s)kpv130W_C}*wI?|Mrbj3jF3J(obk3aJ%7%3RO^NrOi3|Mi2A^i`xf zqARu9=%4qY8~$W1u@0=V|JR?um?1%=WS2Q|WA&o{Irhe@%_xBOKWFSe7FoEHQqUkX zRxG?2ir0#O-i{}%vG$NS@8&Yv`WB)6{;z9>mG!B}^Ihin>1_BihG3C(F=E2J83d{T z-}DAd9qA8@1dWy9Ekb4=Cv3-;sJVTcF!vLf9gs`)-{by&{S?n1)5b3g64&5$aOgqZ z<-Q0EM$u)myF(vx82_)|f_Az)R!NG5ZfP(kFB4_L_>FaD$3TbX;eUSu9DgBgs)6sW zTo@8}p@;bxBI3i@xkz*ntXIj~gN7;-Y^-1Fp8S*@in50{rA^4PC`7u7P9KrSS zRb)DC6qK{BM(Z%^Tc{ql^Z>N}-*23HGZ1z^uSD(*2pL@D7JZCtC*ae!@1`y8;;8=)fY~}s%`dp4 zX+V;lCq3<_AQ&)QuB*`o%vUn_=uw>Yf4{W5{Nb42*bC!8I4|M*{S>Kd{eXn6yk z8XSSFmP&v)>PdCg^Odr=Hvo9R2|45 zV_`eQ`JTF5(8^}B^|-+s!MJd7vO5CUcV3uO@RP!BX(OH}T890`=4zqqs-NhG}! z{VPhi9VMPG5eplUa$nYU!m7L2!C~#3)_n-SeFPMy?-+)!+mcY3Q#$1T$TfbX5XGuB zj>9I;dQW*L-Ev=8Q41WGV!K?y{15})`A?JM7r|HFT3H0W*#7J%Mn61)cDXJ6w+#H2 zLTKadfUHQuXVFfX=q^usg?jO;99+Y?=by0!P8M5r6&Dx@{+`8$KzGyU&JkI)RfOr1@X_ zVS?&pa>q}Ln5fwX7H-Si35;W0qqJ?9&*_#2P0sw=J-QYoZEzU;6t9U2AePDQz3^83 z53ExYj7PkUdT-rIpl=lg!mW??r`lmXvIEoDyoB{ zrvoZemTvz9)2;2>a^Y8@LTYSjAiQ`ApXHQH2`HO$)qVG1MZrdCe+Qd{e6Qubx~g2? zz@ck`S_eispD*%3_4W*{G{~@v*!JMjHq2d9|qxgQ9HnAytQ9QxBcA$&WWk4lRL3&zvOcWMZ|3Q!GK#J_PXr$JT&>-3Whw zs`=NipR*{7T$Cshbnao=} z`U^x(!10JnDXSEou7C~}Q10I)ErHkaswRydepqt5&fpzDcz54}a^eT&dpC#+$NxQ3 zRw+;pQ{Rt6LdUD{338$lahCo0rgTgroF0K%pvf`ygw&rg95A2%yL0U)4{<_{w|@#* z&A?f>q$=c)l|^MndPIbQ7^hfs$$wa&t;aXhO;TuPhE+&BZ7#-;X!lRIQ+0&-dUUDN za9P#-Ca+0kKt&-E@L(m-P)zj0qXlB}1DANg&j?v&CwJBB+SsM^-|W&-3NwE8^*RTZ zvF?;%ht&^X6L(b zGRad6Lim~xzrVB-UmMLsNV5xn3q`dBX_rN&JCxMftcjU&b^^UfIaL9rP+7>wimnul zKk4hpYD?>pdyg2E)SusDx8S!Y*gfl7fo=^ zaM*6LMEgktKqhBo+(Ni`+f2o9>r(?^8+2x0c|82~)&ljHI%%dCSf`MiEVWX7AM= zHkB?SIEW|O@)AzY8=slLDO@49Zc>+|RPQ=M~OujhC??vMLuB)138(1Kww z{(H_h|D<5{_ha*o-dw`@BV3P`IwO7cO+D0Ad6;>_oQwkxv)Xrg=!x^~J!E_BcT!bL zN@S{`FA$AZ?D5iiAq@?(hp}T3O`?;WOr$8-?UXw4S^!NliOOj<IbspLINMK9MBr{;U>ZBFHmY3iU!hyA+uL44R$P%ohUqV z_MeQIQ5CBHKUCR5kP0eIT~1cD&jKloxWWF5yp`^_9PYA%RJ#=9o?F~8qsfYF_sRyy ztnQFDy8u-({KPI#=rnCY3V^xytL=c^L?Yni5U0mk#!gkL+Xi7gl3tsiJq0{!>#a?a z6Q_|~!JT`JX$}3^g?{l@2b`x!s5_6(6iijimc>|xpZ@I*zIYdvlzVY{6Ap% zJ(^|7^=@a4`>I1%o^w>00g_KFoc@2?RZ>Z%y)2!LBZ27g-!ZL2(1Stz+*A!HvHn-d zV%D!tc-SH#&!nYi_kdR19>m*l&_?kT^urnd;?;g*#u?R5u25Wn!|LhcG0u_%UIG~B z{wHWwb4U;r8zdeP7#n;F-efR2nv?I&5&;*3VfHaG6oY#8N#N;o)@`S}I6m<_#P{EO zrYBxhih8h>g8RW<`2Mh_h_U5%$e5$12WyAHk(qf!2%&d?PQR$K8kfV8MbdDDcjuhU zPY(}{ZQrHxp-}l|{=Mv1x}gL6rI?O|)$22GH+wdwA(H_9BGL272|kdf=Zg0Vj_}0s z92)dV7+C6U9aNdLuE}!{H*bBy>+R!X;(4TZvT>C8bekB<&|!`A#j{sZlOm&?=}>&ei{MoL&?xi^9Nhap}p*8kkRU!9Dq;BMO&T!iO6_cB!!7=QzN z>Gz8*d7ql0Qd~rKhL72UM3d#xLEvVM5^E7`V6=}x;{$unei_iFA~~rY-A%rwE{Fmm zx%7q0gCvDhFYXAa16DZhhlHiwWb)iCq?dg~?1qEv(iWO!N<3>p>eY}S!=%acgy&Ih zt@GMRoN+5z*IJSLU)AABkOUg908zwQ?bp?iKwd*721+e^PWuFD0jj5x1Cv%ABoIEV z{s5KIEpYqxNZ1u#ZO+F+qaoL6(F_Im?6YoGOs7GeK*g9LHl7-B5h-Rtp_dYTn`x_e zSz5lYM2RdDA#EhGDlTI%$Hdul8+8`O28mgft-BZ}57RmBN$*-a^i5(W426CX&;4hq z;KJ5W0ptl$(BC{fG!)J;>Y2e&Rk?@;3AclV(&C$XCa%Vx1T5J}ZRp_aS-SMIry1V? z6p+qG<{yZUtSELhcKARA89SjDi`)6Zr{(|LaSOXk@w@a^UeVNn8sGMK0q-!h=$b|W z8p z%VaFmbH`tB+ZaGRA7S85XX|d^YkZ*+0nS9Oqn` zu08bpf(YLGoNqJ1jBmU9Eivb941D#Ud3wci_J^}k-cVE}C?C%NP<2*4M(FK!yLskK zRH1iOJUO@gyICxw<)t`Hl=KoLi#t1!B{`t>ZADxPvK0-;F=dpS-f`dJABg#UUx4Tu zveN6Xy^Ux`MHy_I~DqOz&_7~!|gr>cQY=ntpfU7P2eJ4nhS*&{Zz zSE9TJN?KDG6|lLK5|f8OCsR1M6H&txx+r_FR0?6=M*G;xIJA>0mD_%F{?BJa+F8;W zgvQ^BMOE$=k?`Og#<5l>xp)x=NZ|i&ib-lCz=>gZJlM3w)dM21KzHcOd$aC;neKnB zlHe*pezz-y)6nptru!BPL`s#_{hO-%4^Utu=>5xKql*a1lBDM%cZZtvRiyrxT?EAH z#yK~sr2Th~7P?K@+<3@vSYiH&pbC@;cI5y5qbSD!`A-1ApiD&2t@%S}psP?46{sRm zehkTW0ABb40xOaG#n$FV35ZE@>6p)bmX@DF&iVh}Kb;FnJsTw=kkx~}DDkEIgHG#d z_zu%33rhO`LPD0uB%Akgc&0hjO93 z4%`MQyF%O%aVhYiu=|c-sVA46n`dJ@VFLIwr6mjykDyHDMu$QXl07Vuyn8bER3 z$TB+Y*5rytMTMr|1-1(SB1nI^&uI^1#PSDy7t(=<8I6Ra(B+5vqS)+>SQq3m4$r!D zrfd~>L$aI^xn|rH5c2vj&!d4!5$Eg< zLqL$7H7vdpe;1zvP__~ridC8}lEZk&vuM1~4)Si%NtppdI)3;t6tP33hDS^qt-yL2 zg@$lhYDbk|m>E!yXV3%Lh+PIUZ!tU{U$o-aR7D9jtnex4{(VaM?*+BKI}QyhmM2<& zGGhMDNZtui&dhl}8?LY}tLcTypDcOrwbq%vX^J*wGulmPQ$7R)>`S~|IO%7-f$fHa zJFOX}V(4$>WEmC4fa#o)x5h(Zbw~xVrNaDc6#hc{N0;VUw`33B;;cP}arBe{0=ObN zN$j$7Li_ss+EBy+_?}PE_Z0uP6~mPOK8g=WGxB8)8dS)+<6s6U-^+*fI~z}!{y0Ar zDi4Szc1z^nUvh$hBDIF7N{k8sjlY2RYa8@j(wCWqJ#${ z2)xT8!(=@TOI))yYgu3>gFZ5rMI@5nNaUxJ{4Zx1AQczD`QZKUe2{B!jzCeL06Y3G z2|?2duN>Ag+7i5D1LDfXUtv)DPWb=&E1}L2snL{JY6jCDKoYJE1 z5rK~22k580rwaw`Kg z!=FF|UXkK{ph5b;JTMVMp}5gNG>wJ20j03=LOPmgy>mn++$0>af6p9Q1`mAfG1Bso z`qh62mIq8&(fzIK1>l}f{hk~3(vmtrbf9h`_5t)ZYXFfro=NNhc;~G-kk>`p4 zPaFYI%MA~IvkMYl!A!tAdz8k$7-mP^I`Bp5|9;WxPcZxRrmu-n7KHFsbHITMH_MP| z0<5y6B#4=lxl#NTC40dIQO*AM;fFpz6S~#+SHN!v?bD`bp6?K~rUlr`!Hd)w_az_< zUWCu18Xy(XaHJr)GwH^-51si?{-xuj-)Ia54lAN+Hz5L*wigKereL$w7xz9k?d;9U zcSZ0unf^V`>(De7vB#*OZ)pI1XzpM!SaQ{nKJ75@fU1{wzqRQ&2RcX$8}t#i3-`t$%D zR;~E=i8ZxB3y7?hYGLk&I&WlVaMQ$RSr7Rf#R8os8s)vAaOEq2t(qae1)%O`@DIX` zBAo`Y&dBIaM5_(L#&N(Ei97Q9S%A5nQi%&HPQe7z3fjIgR>I*yRU;Pxwad8n4z|IG5QUIvG0SF3> zQM*hSRYiAV3PDYi6I;K4BPEC=NvpL$nGr@1URk1kG5XJ4z2;Y4TJ=V9zn{)q?L#JE z_4LI8aKD!M`0l81%K;>Kmi=n0 z#rYp0#n!V2Tt2ddpwY`}jomw?5PYpM=kf{eQ6BsmIX3`Rl# z&{pbp?Xo2rWcrr2-oZt^M>aPr{a4;OT)#&70}f(a`gZWqEs?4*LX(P;ZBQoBJ)OfT z7|h~==qB(IP@qcC0wWDtf1=wYc$}ESWd8=#E@oEk*|um%flI#d=uMTFd!sAu55jdh zE=opS-Pxv9;nWK;nSas^Y}f&0O(yXpX-2D!n=oGxN%&Y)R4I zwj=jTuit2EQTv|5=dZ190|vVrzP^$~nP!kX&RnTA5kAs1qzbmO9W}(jjgliD4@nA84nfoX0x%wtA?`*aQkHX|`x{BI$0R=^J6aw5HnM>_d7 zy`>%cs(lcEgcutns-U(di#opll0M^&pmJI!S)6$F?ZL`uWIk6DFVRlz!f*;Dk9G5L zMMHcj098oWNcN2+77x_2((p<6Kqi6z^fdBxx^fsN@wz;jp79xEX3g#~w2#$6Z(WV* z8skHN0wp(t6MWaliTe!OZ(`M!fJNj5B%tSmUldGF+y(;m~<8ruIXNu@5O)1;r)>4vl1N*g2t@FuNvhE<_t-= z;mc8D4xB4tmX zrE_%P$(Sb>T_{Wyc9f?#C)1`XN)duzinT}f{3jOyBK~^U;-ig9rplV`=fePFrK?OW_&A*#Y~QNkS=J@tVb!3?52?Tzq_q ze3{YaXxf&3zX9Q*9--~ZVO6<2uT{HM8_fS25KRi$*qn{=0j#pGTLM&hh8f=(jFtla z*zsP;p=}r5UsvQQup>abwf@7SRLK0rIWCKA5sqk~Vs$d%+9TOZ$rC#=8KPh+f*8)< zob9Z54D&MxAV0CKX;2oc9ZdHZST@9}*`ToCU@A(At zM8hh;BM~PKTbjalB;HI=)B_!4OPV-1il^}avnOrwa)+_*kr|N6v)v=?i&Hg%`i=k3 zHEI;2t#tz|j1j)!$}X&~%pO;)xo88iH=0Uta^dXIStCUe>P9%b2GH*to>LEC3f;^^ zU?Nt8DNE(*rMXNDhy#`z_#LC&Eu3ae`8NB3l|LnB%F?hII`f1fG{dkUQ~mgCkA&s? zOuVxne&bY+eV8$}_5lJ+ttRU;nO^xGCTw{_y!;Ps=T40->zV&BS;xPpony34K}PcH z1TWwv34@i8bDHXZz>QT0agGh|3-Dv?t$Xz4;SmNQ+Eew>f>mi^I(NcHymsuVv`pAn z%lJ|^YGR=F?i99=fMJde`RjOJ&!)iBoH&k?KBz)Nvo@i(GdnSl-U zOrz|FBn{q}JgsqU;h#7j+#Q@gyGx698Zs*_J9pc{#0h*1p^}#6Cbt(;4)6zQ$_upB z)1K(o8mZy@Y(r&=Ex~7G=D%_P1SrdptL< zni{4jVFb>AsV*@4d$CXhi`Y;D>iWT&lztpzEj z^Dw3==25vd@j+|{+uAJt+XiwVOTmt*a`lwzX0b4vvnqAFX{4w`B{#jwTLdh1;BWB| zJ_Ztzvxta@=I}*h$xm!fd(5U8(-d9KGaCWd3}NI{O}7^orO(MIGc#bEiuAj}nNL$5 z!NO*cWhm^_tS;K|Sg2108@B>ef)(kXK8pcxH(dQrnr>~wyOZdG=j>z6AO_ID<_P8&+t z>mc&yaK|gDgOP~Pvz0<)wf+na`nZWI_?_$a< zqb`gtqij>55svC&kri()wWm$6W*-Mw=#~|ILxx`MkyWTai{Tqx1AhkejlRfA;e?BF z1Fd4-cfUGit!o7EA9AW4i=rK(usReA-~=-4xd`VaMV|})MdOzRD7QpGrREF?+Y=cJ znf|fjl;{$N=x`ILe1sgQ!8iE~X=x|?M2>{0MF}{M&!-421kXWDVsHcedO(7r-@6$* zsZW+-%UvRbG+X0uuZSwR{wWN7JBN`lbU_4ET^3wK%j zZ?$^UqbWOJA+cTiv2Z1+@m8%IRv{N^EL4;`RK#c98P%$|d<$)_z9ijnG>r!b@MFfO zLhA{iW}uq0k(h$HPjr9?CDl-*;tJs3`A%9Cvc3rU{BNK8t_A3oqk;Pq3sXqY6?iE(1bN8oX{&Y<*|alTlRK1FBsc=YWO(gv3-qWTa%9YLt8H5iI3w zq3~g(tAYMSuYXIH=a>YLY|z|k5lGGN_{kWARF6ua783YClp|5nKqRU`rDb{!=KmW- zJwr{j;)lY(!R48UH1EHI{5HdHa@ZGc#}F zscxrkGMN8f5xo8Lf{r%%!{}^g+G4%em%^N#Wn*-BRq!!(pvvkOt$?NKdf_{w-05SX z!a){C&Rp{sgM)6`E&w}mG2*psudEeeE!RSc(8Gd_|Hm`1 zv|$3%xQi9Dkk{gBgvtJn%rxOi6ZXqjtesd-<`F=iUWy;cs&Rk5!U1)F2jQjv5k*f!5ia{d^MU2_3+6X)bN`tAHc2 zGq?wbs6Kp$Om)9>P4V$#8>^^PB4Yk=_&we}>q}<=P|0h@1L7J3mqk$l7Aj48#@_In zuc!hAf{wBqWov^2=wutY?2OK*Uck~Wf{J;{WdhXSBFLo6NfN8>Tav0kQu}YkKy8fH zlwSLsMHXrlsiQ+S2aN_5xgAX6MZ8v~Zo}`ISx3_=e=>@T5cDwZqH1Z+#BVr+{&~{* zv!LFu{-FTGI$f2oxq{DsM38|uI6D9%Hakg;6(@t);3K*XV@tTvl6quav${1JO?NL&V3FO*IrMyp`s7* zv(e}OdEnmt>Sfw6MYU=c<{3-t?}8a+k0GXPZ`IEVEPC6y<{&F5WNHBdhyT155IAvM zHS!EvCOqal-%u2((X~SI`)@xnEj@VW(t3@!6rY|!3v4_F zb&n(0u}}dYs1HzRPrhgl0Mw)!!-#?-p6C6@vUEFlVPUW5cUU;AFps?@?mTv_A_9EZ z-^ks@s_ljRmJrNt!N&~NPGS!I{_sNyXzK9aaP<2wn9bOt>O^1&(35Pi0JiUD!KxEi zX8Y`$ufe}jZIa<~;QN>fhfmqo`eYhd>#dk5a2bGXdIXjV-GP%D`A+Y63Wn50dv1{u zrl_%nbVN`UvTCXBeS!`<6v=Eyho$a8J!8~Ka>UvYRCsc*pscXviP~Zb=w4t*h6iBfX+N7`yf{%7&*Z_fa;0PG_UDX&*|qpxcj=7Wm&Xn z(^0m#>+UAEJTM_4>3ginhVCQqfcic1J3>jjI$31gZ%~dol8<%aVuUPjAgO*Ev;?GT z(h^4FykB9*cI5y7ic&u7eSz?W>)1J52`fSdu99$g!I`L9+b7yVp&IaXtSm;zo-r;!{k~#>aA~j z0Bzlv2RlYXdMT~}qIeyg{cCh3oH8QTM3G~^8Ei@*W@xI`7NqJmP;A1wAS5}f(>hz{ zTf!ZE5sm~EzMZK7XnX-TMpxe_nzJsR;F5p8Z;W5*C93Jq;9!dtd&P~SG#yGM2g4JyP& z9>dk6e7WoNviPpEF_?gsFGogl2E-N zqcu_UxZC$T@+~8vs9}Sv=o~=v6^JJ`z4=J71wpo1RH0o$Kl4%Pu=w?6h!p9sPjwa6 z=iB%7_V#7S?}Ly2`$>}lqCG>#0LibAXT{2l@RIxgDg)iG>xDXWyEIp0$~ACn*xZK` zaa69c>??9dM7cQdR>ZYVMj-!cSMg{_#O0&HKC5!wMM)f9In4#u28+njUl`j9GetP+ z+9~Y8h#;iwJR&5ol#*k|P<*`zW5S4CL!2mfqYzN}X2h;fRo>@ywdwU`!90h7|KL-K zf8?2mAQ9WNav?}riea);3Iz&%^V^$BUVVa~_W@?=Jn8aq6rNi_WFA$g&GqG-#bmPk zFM`y#caE1ofwvjUCEoZ#3smM=+4t&;35e-p$M5f)e399NYrZ;+3l@=&U%#A%;xy?LBjVNo+v=eipS~DE`|L;<2Z()Y&KE zWVO&O+mdmxSzfqbo+H}h907xvTkRmDu|p0ig61_~nj1wfJ4L!|uk0+f?JMs=LtNLb zNHmCoA9(JU5+(SAeTgF1dRz>>aWvYLvq~_lzX)pzxulE}vCwSfroK^TdO;LB)(6b? zYu}#nTf?6;MY~#+j6;O9ShFd6QU4)OoheHJ92O}9K*nv5E{W7r1q6;mJtySWiYtm0ROkWzr zELdssbYpdP7B~)sIp~dEfZjC*WZ2ONg4Rj8*6{F<%N)p+f^07|l_|l2HqEV7PA}U# zkEj*UUcLa%Ml=Z0e!#ZX89?mM)r)9h1D|N@&!>Q+foXu^RY;WoGSySkj{e**(5r}| z@7I~QcOZsitTa@T;Va!Tusdr)kIL7;914EJLO7hl9kUBi&Cj5(mmI7H8t${u9UQwX zh<$#FYWr`E?@6r}qWu74d^;j`nd-fwjh9H%I7|>4+^ph1e^X(f}56-oIYYYc%7~RSBfZ1 z_7-Vr1XK04>@pswMLc(daGBq5MU6my$iBwP?|Sr%c46az0M z@$2Kfq$0_d5{(q+mgYLYQxRXbi!38AAXvRD_!$shVvvi3uSsw6gZly;6F z;m{BpNNng8MX?)E*uxVEo-{5Dw@jhyK}}#9VnI@3oPW-OV{=6J`5RtjZE<{SJ%ESB z5So$^h-b(D6ZK$Dz}KJ*Hq9IpT<|k9OvVf8m|h?=wcmiCI7PaQk(LdzK&Vk_sCyy4 zVp!F`bO3&LA)7(5+bsJIA|U{#hD0-%{icRfE)WoRXtpHR410K0a(T}L_YNhep$Ts- z8eOcKmUNtRfwXRqIJ7B9cnZO4cx_FSdtZz=gYN!k17oiv=bsVw&p18#0)@Rgl-J}T zMc}y69aHW2a}yh|xRtKR&S$YZRWJyRPTQw8?~J%LE$ajfA}zKgOwmV3F$Cnw$4zOp z#M@z$4O@g7tr`HPm%mv@A!Z95>Dq|26zOSEcEQb2dn&0;16w%gVp8)}4)W^YPWuMF zB;)Bt29beplje9&>0SvRgi5U@?B@7(%}osQLowjQl17;!Mw7qZjqF0U?VlT`Aen;ui5@_B` z>mMQJTNGBkOGqiZyyP28vR~`Vw}7CE==t#mI{bT-NHHXJ3)fS^89!E4S;66j5-gm$ zw%;{tNYrNS3{mBGj2QXaRDIDii>!`xjyOMco#uSm)PMI$z0Ji$FK1UuTKbpDK~bQ9{fX!i8V z zu=p|Bv`kiQ31P@P;@a290z{9=gN)YJ+=^-d8=?acX!p+2wdW42b{#uElB!M~x>8hU znYcsKHK9sNJ^P@>C{${k9Wah#+k+a=w-%m|Zab-YCG9SUHR-hzSY?c=g_zW~rzhx>TKvXz;{Nt;A2dN&d4i5i$N*-ip_Szxu zNzbUcIt8xD;tK!I7QsZATz5W>R?Gpp#c2my-0(EOFlT{{aTad=a&fx32*-5NdM z&HxzAliuGna`I;CB?&tMKWuzhc&57sXhaQaV30^i=`x`ag&Gp4;9_z!`l3|!LufzJ znu@4fptiePLX~M9gl;c@5#MOb+yPDQw8aAi1c18k#bB^RT^uBtjX*qz5R6HkjJ}-i zp|FQ0zHaG!t#)LTd!MFjh`9sr#qFQk6zjj{2W{C=qu=o)&}0y35l?=x5IlR8mpsvZ z07e11qa}e#P}zvq#)=k3_{>5fYU`nVYU3BsnPNc=)-|eo=R1(;8CQxHB4Cq6qj0fJ zs?TQ72af0ixUct@moI<^tW7 zxUp-Tli7t*C4VbCbE6KLO1>#4MMH+9MYYy4v)l;}!tT=Jj`pX!6Gx?r^(PmP-lPt7 z8#Cbxdmrof=;RK>-1-kMG$-QFFY7g+*BKo6c_VD$*)3%IH%;S)G|)6hyc_hLg%cB= zA?X;BpJh+N{HFu-m|nVVOK~i3lQ#8Ck!Dl`kU<1|ye*Su5-_ph(~9w523=KQnfx7s z4FGUyfG(Q>=xzpE#5ECmQvsvPIS}FJvjtP8&rxQl;wkJs{lri7-fU5mh`H3KnH!u= zY&SPd*I*;vUnX5%K6v)mdas@p0jzBss@cV74EoVQa#1Tp9qVZm(5q*yL+vrEC&Q^2 zsM0$CW8`mNlmaf^inqaFNFA=iV&gn03!K0W=>;GoW>!~p=Qw1Vy-r_w_eLtw-=XCl zqoq*nK^M#dh2@krx~dlK;@DO(gB9t%N;bgah^kK3g)sJTZKcquwyQcWeT2;>3wC?$ z{%S81zoNo1sg2_sCW0NyrS22j^ER<#Wkn8t39DmXS+n~R$wJ+Zalx)grW*KjuET}9 zGG!@HS6p;j`L~RQVYo`lj+Q$(u3frok1s?C*^{y)qwc4Fa{_Q-x;*peVZzxh=)8C9 zDuThKz5z%QKq1pkF!%}NZ{zW!6ZbM{V~Ey@KmpLT5`hk$M21DXKL?lES9D~RgAfD}pPxT{( zc%xQr$<`<0e3+QZH16webnwcZ?-9*JCGX+zruVU{LsUWAG%9Mj+!-m(TvmHqOK&u0 z6JyM!eWkZ-h<_WpnMjLKA}GN9`CUd)DP2PZh+edLymW ztAJvXSSXfP5N{_QxpKU1>Ezq;L|rybKh0hg+@Np6sr{v@OMm=yUijnDP1qVXcPi>U zmOX2~0X}Y8>-<{1ZAKfaIg zUVoB0;VV+I9DHF`80Qif&gQPRLRwa$+|%i+m*1qWCi@p=e<7HB1N2jgIPy7pgWN?# zV;KnrJNFfdz2$bPP>f8s;(?Z(L-yH$?*pf+(_7wS{=4Bi;$aLpT^^KYX;%byk`t^SjSOs(=@01b)h$cSNX$lLiN(NgYMZS-`_Xm zE4T{D?Hdo)GQVs*GC^Ptfz7LNxE_s4Ve$5Q^b#jj;-YjV?ct1@7I~|izv<*`am?WS zmd|dDU6~05Xrec*5ZA|}_4?)<4CG!wdgGfnFQ5UAf%>L%t?Of!`St5OFP@CdF8GS# z3VyNPYY9!Sh*D%N;=Al}SGVQrv6D0A2g1h6%rp+CTioan=~x{JA3G~wrxP=uS1116 zb*1L3DPsOKU(*l(F!<{{9yoZ{DYywHAF1x?lw}-ndiUNSlPT!n9;P2^!^wS({Yj=8 zr+Qt4)1SNhR$&|!YEVZ=(Yg!-nM>#V=J@sCBy-E+ELImJ5wTK4;hF3c|hs~d+d*UiFH4()Ny+4 z*?c+FYCpC|1)hZbQ+N{3yre7UeZ5PMGYheN@G1vMTS?PcWMQ{g+zF;tFDRWTCX@9p zMSOjAh4Ay>6L;RH9Rp>GnOFHbW=~2~;TV5fl-dfDdFsQj62Dqzdt-Vy)0wHW96Up^ zj|px|-;bRtOR@l*L0t;C#QNdP?u()gW2--o*eo`86QQhi@;fSe?9cW%NqYO$@b=l# zEs||xAA32e481sN&a?blI?*CU4IB%5)Q(HcjYuVu&$AR|6!nzmf7;%t*tTv*RhhgF zi+l5huHrf1;>TvEUYqlcEIn=lG^j_Zn~|=cyrl7Jk6PiNq6opUkjdcZ=i5w$1Kc-advgC2;n~SGk=S_ zLLgB8*kzx;vuKfZmgbw$&jKJrN<Y{XR-JNEmSXk;z)Qf*jI%tXrd0DGU+_x`I;O{Ms*=Qkt#jx=@-ywSRUDc!DrF_q5NB| z`*gJZ=2O3K+Ps0E*gSk*p-Y4j_Hi}r+Bg{}lgxlefu-MS5=Y+*R+!6X#a}U%uLZ+jCC^ShGsJIn5j( zy6lOpWPGIYQY~*59ir0R`SR!O61iuEbv6y*RCg9j=p}kyL8}xvSM)Xq>ctYH+*i-a z#DHC*&o-dxz!=8vGFgL1_y>^+0~3!g()C~kWxUhT@b6A*8uk^r#2KRbe30SKD{~DI z)5(EHyCR;Wk%%qacc-xLC0cnc1lq!s)}j6&8>x+k3(LaMnW}GPs$)=fL1sJT`7?JZ zFiJyQTqi$M3(yVsuic2QdOsLs*Vu$2c4OpZ(t3;CM-J>ziV_e|PCa6?{dwZU&#WuM zx~4T=LAn+Cr)E#a2d5Wiu|JQxC1VelHF?*Cm^0EQ^D{Snogvk)!-=<~&jXX)dal>c zAN@f@MtC9Pfy-Rk8Apx(zaOKA=2&1=ICTIW=FOc`i9pDMXRf+%#@ z-D9C7EBj@|U?4ZJ`OrhH)DR->?8?{7Rt=~Hrh}iM0>5eOnZQ+c_bU$H>Y?yF^M)ic z;8RCIe&qEoP_?~ln&ZSPdV`|uQ(a>yF6!)`7gd_*EdD<)3JByoZ1R9VZV)4dep$+* zoRdvUwlR_~?Y-;(4WXC&whRaeYu*P?*iO7fIw&blF~rgw^-vqhVEKId?|v(Ck$JW7 zy6NZ%$DHWXRXqRan5--HSe!CP;|o}j0uK+mH)D?g&xl3CSm<_&m5!qT=ew#NFkl;K z;X>gNZpYWI&e0o6I+qINOZ=)_0jBaE$v$XH`&Cnb0&m!V`vYSttT7tjhcpoSsG8F3 zP5wb9&8m9U6=>3@q%SfKhx+mDI1 zyfd}O!*1S>b3!=~<7wHpM@UMX7?rIDd;T08UlZkMcbe4qIw0u&SjFP^Aq+deT@COQ)$b92!3##e17?K)*@qW{CXr7TmoZ8ZdExgY&l*8y&NZC&MP^s03}uhB?U|58?T%(EucQ&r3WlKBur;@y7C_yfWUS#@OJQX;q&#%tAtG$k_{17W}6C?%MOQERejN~_JThZ4J`DB4+ z2^&{b`3tXqqBe+$W)aa48)hgmhQUmVev;6K^_?f<_BK&;$V{k~C2;|I_*e`^xfETV zguO86vbEpwv8-Gs>c(rvQulis-8##LZ(-ewC2S4u&w9zpSpSMaTvZr9ePf*Lb)+r} ze+K)NfuYYuk=ZEJGDiRiSV$L`D$w(MVs#y8AkN3DRla6&r6zB25{eV`_I~eLK@BvX z+4(f@-uNgeHk${|f62AYovp=l^d+WC)Jo;l4}&ohXvG$23qDPVvssTuPa)mbOUGk# zCyEF3L%!;+yiumvQpqkGhP`EH>yB&%q5(`11J^r>kjv0E7{zwLuvZ;AiD>|^_ZzUN zeux3h_zQTS_jFnA_yE^v1Zu$ioLBu8sY^T++gl}UWryr?H&DF&n^_okIN_rSJk#ec&E2~Y6z?i&I9;e;IFU?R8!!e8@RV|q*!6% zSV}Ml)J>@q<%=*M-f8nHEWsDr?N`j=&KVngrBmJ3X+O_@!G%0_>AKY$t0q@2qiIf? z##c56!MbK$d>9Acp9KRbw3;f@_1axwiQH6Q0jx1cNSo3ThygvOb$jj^V!9(8$I8W} zXNXn(^oUGvb~Byr+)beyJmFAeb*YR-okQ(HUKwXp;u?%c-fmYw>Y43~+4Og0`NJMn zfD};!qn@@@PLYNd@T6BZ2{dWscaSzl%4;QmQIv86tXwWaIZm;Wqtc-_f>!lKc%n9% z+Ekon2|-X_2_hYc7!-jNQn!Sy)Ob{!pE{Sj*Ao<+sI2M)vkZ>QXm8a|gVXC-KsxG| z#0_D>bu+Ak(#%o{$p1+vmA{%es|G>oD^ZABQT=S z=vcg;@@QnaH{TY}>S-_BgrVLJ;q)&s|GvRD9dqZsUtDgdlJH&v%?lI3=mVeAxr9w7 zVr0K%e;(V=FZW&xrGDr9ayDeUyY9)tC$EQ!Wg5#~N9w&*WaZkyG?3N1+2v3XSBVRy zrt74D81(8`7`Z~H#-bhHo77ER9~g?#9>twJ1`T^u70S zaSaNmg3)AY`uN2dCt035x`g&1lQ)-E&sL;g)g64w62cjjs?PLHA~84tY&^XZbivy` zDwQe7x7F=aEbBP|)h}Mkg^S3%h&anHQE~EXcy^yK#|xGP7vlzQU*R^c)mw$Z936y; zp&_62x&r7Q8eMDnF6XAk+iN++g2ASn-4l78+d0t(s#KPP-HCK3!@QgZwj?(}iC34f zit<+6vq>^BAQ7Ylh*Iria0itUbCfw&O*S!}#oAA55L44(N&e8L8@GUCP(%yHezXSR z38i5<9+z`_<;4>;J5MgN)K6pX;(qP^4y8+7Fp+XZ8af>%%^tqcWcGi9Il9tEBQy

y9qgbRaRkOH$_F|h?zxU&(H}8}xez-w3EF5kI zD?Xb6{?^1$(#BojbHkbckaUNCn|~jxqh_(iGn{7d4fKc$L72<-d(#R7}oBsGCD(r{dgQ!uD*h5gm>{XyKw*!&_KpXz9z ztD4qkrT3dVoIHRxY;#32Cq9*63%OVB!A5V~dlQk4XXTgcM9Wu_A^X|&ZIWboz!Zf@ zh`x=}1gLkXcebI>gRjtgSSdZUGTOcpop7PAU4h9sa=d_Z4UoTSkC*jImI=<@!s+Jv ztEz8(MS9OFx4NnS6iWl>(lyN%0=y{;PuyW2&B7J))fV0joDNfBe>-|-Bp4>J_LasSlC&D%qTa{CT)0Oh6)D;epLcip*uy`33+n*UdByz}Cv~H# zv0L@%3c{_mK}tjFQHl63`*)*hExN92QC6;s8Irg7?4jZcRPfpYW^mUsycZ(h*Ncn0 z?s@%&ne$Xgh9c!XC+HsT^<1S|ge<~cs|Jo2JVQFf>>ia@z#S%dOE+@FvS|{j;iB$7 z7UpJ9m#iQ3A$o?;u2ct7?WzIInl)08vgL-qCJh5ZXVNc>BXIt#$;DuGk@PYNY5%#1LI$B z64rVJj)$g$<3A-G`rpeA?#4KLW4>nrj3{CntB4;u{+5yg#aS0VYT299w>RjJ>aECn zG$=rY5@Zw|9G^#G7ykC&@Lh`hl09-V4j9e$L5mE{q^k*f4#2c~`kbW@!>iy#Y!H4qwaRN_3A|Jw zmEeHw5m>^HN@eh)Ex&JgUO^XdMh1XXybezb62m?_vVZ1G7@a2uJ$oiBG5h_1Wkuvc z{o%{}syyS*$BLM~zx`S##`TsRR}YSm*9r{=6S>)J5IvLXCni(NWDGibqesz5Zw_3U zLBw&Wr~tO$8Tc3{{Qn}#B;RMYuY-x{yMTUH?DKV)*aYn3`mlI*m~kMGEat#B)G52h zLX82fgOJP@ zR2n-KJ))DUMsi=Lhy%!@L|tjmEmM$Bi2O<`IuK>XjO~@cziG2C5Z~VGOc82Ks_zUb zdoKi?hvOiRX#ytnjo+EE8%bg>gqR`}Ki!R$;mK)G7Bt>2^B70mYSw|xNS!Ifasq;# z)A4A_vV@yWsVdn+djxZp$-)0uS*aE-QCrI?#0hLpu5hvT)Yw9)@E_i0m4_0+8 z^d}`Ktt!JU09~T;QnwmxtNm+-m$#M<;pbbfxWnb*V(nk;te&IwGEjX@9QY7GI(m3P zj)-oRtr1|BwH=mpZvcwh=RZ`s1MUZcycb7KX$wpMQ=mu~q*FkMd=oSnGkHRQrsqOL z(PXC#NP9L`f$Tp;uX{%LwUu))%hu)x>e}P@O3$E6yO{3Ik&_{!N}tbaJYVf0%akyA zT1odDwu|E{3=WNh(4(8fxZ^3a;1s1190v?I^FBz(83dWPap*s&$y zQ;V}V-|aJa#4h@1byFBFiEgTg0X&GQP> z=}`jbhI#@t%N?v;znfI#*aWB{H;T0v$`%{HILB<}KthEotHvEEh_ap~bs^*k_*7Z~9{|Oh^F z`i+OI3TLCk$w{iH3;@-nvC0bNY>Kapuod?u&Z^o2^&2>cTok;!=M4a4U9K70F_mIY zBQKz}?yHl}sN#J;^k{{>R5$4|KTFZ@qhW7aUv>V@LWd8$DnmY0i*=urr8k#wB){%d zl>@QsD%(WM=1L@p+IMH?ythdBw^_r|Dxn`hlbJ3%QZ~CUr@!(RRU#SofLkiZ?Z|&8 zsj#n#q$dSIaVB2V!cHyPO3^^B>b!pllU0;=2M3~Qbz6WA#tAGb7G(6Pg~*}<#s#;E zxRWAhgmj)Ro;epZ+9V7$9J($o<(v6aJErr^QUt&!bd*dHB2mj?q>?cUeD{Xw)fl5jgLCwi<4$|iNF z=ciq^!Q%RO5RX6kby#n%rwwYKqYO4gpDCoqbRM7w@=Y*6R6FzQY2)G_PdwHZhLLX| z(As6U;4ph8YxM&8^q@5a1Y+!AD-;ami+sF0%He>7e`4V|SYgH>risaH%Ue^Rt0U&} zLDCyeC%0!`4woK$DmbN3rV?>tDV#dg-r_cc5%8@Cmr(TJ@a2ef@Vi+4 zOGW0e*IpOHw@59w;HK$bH8uIbxDdGbSamiOW1T`k=h~`!29Di`bNf@?YUOPX&qKVriXoLz_gZN-ic;eqP zc5L>|&OyBHwb45i{?7RPBBYjB?3paadn84jm5&z0I7CTz#oCFv6Nm-~Bw9xfy;#p& z4Y2t~w}~woR-`G4kD0sD&V&Cx-JBkRoWFp}kA>opMrG6+M{{WNbRjTSPZ{P{XHofg zr@ID2RZgOdPPU*Z3B?e0b)}n_<)8Os5Fi?&lMic|>kRn})RS)+CxF86jvtu&ga^Q8 z!_@*k`DTt9BDS~Vfela$??~3J@k%4&u^={oMYF0LA?I5k#u9X9Gpu`YN(co{b- ziyA-I1*w!X=~y5B{ytWTaTw|3k@?&fmci)*)U_DIZ-q}Qi2P;+0CpP4MFJ*672jn+ zvK;(ctmzAEAe*WeNczw>jkqWv#&^da#CAtVEq(mbZcOJoR3~Qd{mDotQE^H_CUSsTgJE^gK`=csp6)Jc zM*5)<6oFGQXoVTjl!NFLJrWPZ#{d(MQ$S;3bppXf*Q5qiF|}~6*x=O3A8PcaAXo9t zdvO=g`ONt6eQY0s@qiU&6C=9G9SOPGU1E#u3aVbHNJ_|3ZpT{ z^cXSBBV^=pTem9I=Xn*Az4j%!R>)QiY?U4%?{AO(02=be7AB~1M4C=rc?XMvLmyl| zo)TbKwy{g>5!!dW4gsR|$kQ4u;I0G1(9G{}i*ok9+Yu)x;(N7>g@9h3PgPQ8kACTpALI?FLwqJMiJ{&q#h%#sa7(3ZEjhC;*E+#FAkH zU|9RprE?!;RnOy7bx+2jV*{Si18%VG^jNehx%2$73EkOD8E+&p0Z#U9qqGNbJ~hp7 zr94t#l=6(t{G7`a11D}qmD1DUND2xO*~<(_z^_W}h|WG|SK(LO`4wj{(U2ApCs zv}(z?K2x#nEAihR*$?=l4k1o`E>k!HrggjMcFEQb{H2!|GxM42y3X@Fj`xvQ3cR>RXhM>5 z8T0}n;X~hh9Smj&sh^Or8Q)s9v1St;*Z9_K6NwH@=|YNQSle0S#>*7H^yP(p(&KNwliUBi(op(Q<3^!#h*`Pr^<-!K4-dk?P2o48u zwlS5MeeIX8(7L9C!&#g1iI$0)v_`6^AeZoUQjq#I%v`fP>tu zvk?!I;ocJ$8a*>+RjA)T9kqdukx5EzM)uYAvcHr*(Z0H=*E%cj3=Y$2?$U_Yug?(z zl}$VJUuYs{TpOSr6CV0b-?8bVtkfQxcoZ@Oc!2JNqyFd+o)Vf4Z`#mfCFy$>@3aNF zkn$WU*?;Y$yBE!OKjX9{_yvo;CAhFwP;*=pogm=?oidPN6#fh3n!FN?g@)Db_LeYS z^;5QcQ0Bwmc=3QJweJVW6W(*L9yxW=(4-$+Q+p4mr^p2~Swv`6ctXYXYhRvUVXz_7 zcZi}YtGKv?y1ZLkaH*yuSoaCc)&sV`Rl{FhuVPOv+KJIw?WkKv{WMmX>+M^lqMRbo zpcKU(@*ON@hnO>UC|fssX-;6~hLWw7HapU!eyoN=LTyNfyoX;i!xWa*1CFuu0>NDO z8V)=G{6vJIPmte&X4eu}79!A2-nI*Tj|KBB|LvDCT&OQgyri61Zg-W*v${F~L4s0b z;4GEJFDmPBLox?2TRB{0_UzI|O({5nLPd#u)Ire$nek19(1WO!m~FU_!zGhH-~>Kn z1eonJ9{%W7nKItj=5VEIZTuN|aa>K%m~)}LNwgahQzJ$x2;2rT{Fkz2qH4Ys*k5Z0 zNb{AOtEPk+>XogjVV+;;uua})2_<*FWXG!aMa`(r@5*Sa9pmSQdCbXUtP-DH2gTyBqzspC zPQ|(V$_t&hsVQ`Hr*0_9Ue+ir1RE4e$9nAEoahxnZ{Rv$`h;T8&_o$#1JfabbLlgt z7k`1G-^!KVGiuuzSQg*N)B~*R2J%ZG8}=d(UH3E$l6vf4XUOtBd{LE{6|G2hOJ(E; z&_PF@G=}HDR6BGPc!zNNtKU9Wlb7n`*2ryf5B%liBYyy*TJZL1Qor1(8mfzxPI?fw zw0CKMmr4J;+|H+d_*wI41+ua|+ciPKzU>$M7O;eene*6emeXw}xe%|cpEGgo&g{au zK6{S5teqfgG6>=|DlY*#UpbSlbEvhPci(laUs~5wT%k99TbyGL=~wQO9Bs9V_DlS& zQ9Mb|VRvhq2qp`651`^oH_|9fDZwuYzXg|%oN;#N#N!|TXGda9TJ0XUZiBcq+V>w!oLz}=0!V~B#vdwR#Jh{m zRGDNTB=GbOJ94c>@2;V#kkoAMCgWR}mSIb51f^XcCpwl!i;av)$p=J63D-+o82e+d z2)vyHFg|dCUcY`W3pZ#Drw3ogr=gLi6!u zp(mNXH$+;EWX3oOjnr@b&8&;_pR>gmxs^Qi zK(qciW2mVvDhzGhP(OJ|+AB)t7szW1gwKsT{KX<$rq=b~12#M5vxmcnv}k84EpfmJ zIE4l2Cqku-cu$V**iA<_!yKS;jc~d#oLM5@m??%4o92V?Q&H%&+{C+$MK)vGC~XGx zAHAhrV3#Bv4XmL0A9V20bk`4PM^qH}U3DHHn0vc%>sqRzHM4|txIE{rc`00> zoJcEn1~4QtZ#_Uko%7?lLQtoH2A_8Q$G(@HG$ngO$Dq<4mxA>N%qp+FcPHL+!ue6RS+?dy%6V!@mTfY z-xkHH8D`bL%VMxk4aLl#?8#5!ve9)3O0ozw3WN-Gy{2FWd zz8^lK+$3DpuV{ps=q*Weaay(eNa6)DjIy62Vzo>jEwgD$!yKz>;VD9$s9;X{C~z(H zP`yW@XFT_Gq-zfPf+Hy!5qwf~8m3gTvA0s()orhI95aSK1I;ri-!bjYFq}7Fl%bbP ziGUar{?84TEPI4GVtY7nvUA6Wq#4l-L@_)bwsR2(E3-qTZK!~%v5(g;{wme1gS5eiiTwq z+n72SIMm#wq?|!Hur^5EkcO!@Xjbgkm*uyAqE-5maEW-fjK*V8vdvwl@%n_f=`%1G z`wBA>TyA1o1!o^+>dd-*h(}(GfA_LP`J&Q4=i)C6<@SeHUQPCuzFE7`F0p4tZT4V) zLS}8)j7{d_%u_C%vL#7hWyIQ;s9-VN)SUEQOQMVBb0Weh(bxA@&fAzP}RAJt1OOI%Q^X0Fodf+LP9=+Y-y?bH&!D4Ar~R|k)Z z+B1Pekq|iwbSRwe;;;UxCs&(M45Hm_c(eAnucHww$~n3aFw{=TbS?48sn21_0{!X2 zo0;b2LbGOeTKD*DyB**4EYZD^uGY8i61BKD^3+;JAUH%2>D~GLHr%(hD`>e(1^Xyh zf0Hy;cepbqVqWE2WheGB%1Jq>J<){iDtT+g{O~~szU|yE?as9}fAEZLsl%_K_j32~ z-!J9UzrBi10ElIEALIk20C4wW;WOdx>|&muo5%}!CQ``t#x1YvAG1ey?sYo$*1o+I zxIGt~ei`&0y+%ntD}tPvoWg6A^n?9B#5VrCR2jNlX>j+HGg~qg239t z2zdP^au%o1w8)aN@y`hs_q8Z$joitt2Ue^#ZFK@@>nyW+b52mvDZ{XH$|Zv(3wt*> zTr4i5GHLgdl{E;r&CBMOBkXx=(a9%n^KzuO?ObaW<6+dsvECO4{EbCzUAwu%jYra{ zRUWhNE8=_@)^SDHGf9pQuDfPoX@HTqPRWHXLM)Rr7&Tg~gCnuC2TpgU)l9ZYt%!3@ z-2aU~!y6mtxk1iWr~I_tFFC@FIo6Sfd+B&6z(9-dJy=WsZrZ=HSqyv#mVT0~(!J7` zueUlzKfj!*UL;n*H=5*C*b~?>rdh=R{dar)+bOOkJwdpPNrXK?WxTRmh@adr;KQY9co1`*EdmO(W-0ev`LzjcaAfN*_@_X2i@IB+Rii z6N)BXK^UP(kc-z|J&gTReNJU(j#v#|9K%}7r*iY zK7I(9eKPeiC3<{F!+iiY^s?YKrldPp>}8T_Gqza3vA)P@|K32?(-!aV&If9hJwRlH z0iE&t?aGF>dv*MYl_&=ewKR%qL)lZK-wwsyyd+l7cSb>sD4--1n!%k}={em-^=41S zXZvuqE@ltBiBkQnWF2NP<|t(`=RHs?w&kas=-o<6q2+eQKKVP| z$~hnvR89R<2fD$f4_3buhd!g8AEeG%y5V{I<6VmosA0jL7)aGn{_9!aEBKz`B9PG| zP!)h^PDRA5n9*^+^!n%u^9Q2F9T%=%7RC&AyiltanU~TOIFGsKT}~L2!uf~@)LZ0u zWu?`Y&|`*2R_mJcX_d^91Jeq%vud=Y0>^Q-xI@V+_nW_T?PalI3^i8~Y4meI=i#Bp zM6XQY%Bs}2o@%!ll2_DU%W`scl&U)HN#5FSN6O*AH~kbKbfrwB5(CBtHh|8VPx^uj zk(rGk$=d(c*fEgk`mt(#UO@bwau8}#Sv@+K9L_DHw6M=v#)BMu6FdBHZnNbG|Mt2k zY?)~CGg^xgJMY}N{S=d%cG;0+`Z?{8Rex7j6b9hPQ#K<#fmgVEi0QGm*wuNT&Ic6) z$-{TfYbOO_nGejtTq#mnAn9oqQJOEzo8E#T`id^g^`hna9-&>2XwSkN6*#wqMqQSC z6?RbWfQfr0%6~$-jt?n8bTbUeU9F491QM%Hn*FSqZu&wox%|=W(W*k^ITl&GVey<>{EPMIKT^rziE<-AI@MuCtcmnSi+Owgd$YE5IWVGA`oD4t><#`mEN~b zxo(<2{=$?4N`9NTm~!_S+_q3+wWY_g&MmkeHBw!MM*4n7)1+ea^+M?WML{w6cc^>4 z^D9d06X8xK&SXv5duew@8mkImlCI78!w@@_ajeqA?YR+ZrNa&N#NS5*!H&HTVhloS z^}+;xdRKzg)N0)wW4?MI$JW2CgD3V0erwvHq?v}tJOAe8D8+vrn`dD$03Jy!t3C-+ zAg~^2CiaHHM?QoAcSb?Cij~lHK~37yLs(Ny27&U8HZ)ERtrl8au)?{Bf{D`zto z%PMpAaHOJBI$C1K8;_@W`}@bCJ@Cc$?F6tmB5%sb~*3d&!R337yPfZCZ-aYIS0T zO%VPGzeALVnfD~o*;hYH&&i=g^*m9aBI8;N2M#)>bKnjQ8ex|yxvN7s#G zm&>WQv-~TAR{GV~WEKa@ibe-?7k{ouwk_(iwXQDyDs!419JExs{=s-pi_k>__v)(A zvJuZ`2P>+ED3u^mJzr6oW7Z>J$UOfxcO`KAT8}5@3oie3pIbl?k5|gO)bVTBv;8q z=8WdF=5y5pju@WXO)*DCKW7eP{nhgPs#ti(;pABj3RyaWoDhX2g-Q_yi)|-5jrmJkJma%aauB;SE-P0*G<|yT z>b_(pqMjj*yyDqwG05^vt>sD=7p8VKlwPZsv|BmI=PY(d!aYo_4=GSZ2$vCjwuHNs zoouP_+LpEsIT+pFGd~FlC1Kiy>DnLk1k>6BnL|OuEt6Fc~s`LRgx+d zCF6ZUEgI%WW8MNqXv!t=(hK9$RQ)O?iys{h-O}>GBjTDe*}@i_A?6hG(-hmXm>~85 z*{V``Fe#V#X?s#L-!jQ@c%Re)(+P{o61W-ObDRw@c*lIsNn|Oqq>V zly-~P6)Jlj?PG8W;Kp49u)e<`}3J6nqn(|{AR3i zPV=W^rW}*adad)A)v7;HZpGMG$@cda1z{u3JbdO>^_DZ44vc);)P0MH(hte4GSy@o zT$x<%zCNC$M^vN+sd}%@P*xfAnMIXc>N;$xKd-V_IT4k5%7A*6?ZuFqNZIYNX^@@V z1ruBSHR-=y2W&gUJwVC)D_3TfmcycN@ua;@C;4~4CpUp$S#edCXWO~O<887M-EU(h zU(H)Q*>!Nt-7@L5m;=x6BY{#jjSnL`CkO?0xSkbz5snY7E(az&Zk=pxRtflAw*G*| z{zNQu^sYtd9&Sy_+nU+PEp_hIi@%~c_?Os?yI6B~C&+b0*v?}%5>-yE*P5erBbhjT zHu}|E)#tnDo;UrwgVA~Ig{O~o^ZA&6HUgi@!xq}+%j2Ffa;n4NA>-A@9=Ph|*W+=X!~^AZY}y=W0A?m314`&2F|ZRJmW-y3jA) zL{Mji)*YZX?(2I%?@)U`U(duT8HwCs>*&eZb1|kBi{{PDV{c|FISGnK{Mz1-&L-I= zR@~8=cry#lM7m=Fo8vur7Ias*dfu^;Vn?*pqGYNd$)+*I-Wqmg5+J4d8xxil<|Lrk zdmgnbXLHO8Ubf+WKC-1M&6+-Bwq$ralGfqqn(KiLBj1w@a=xtCs6@V(od5K3u`~is zd-A_dPDmwg;|sX1l~@s;_0_EKEVtC4Ic*)3+CB4-N^tMwV_8=#h#In520NjFVw!nw z^sZqe&eY~w+mQJIvZQKKa}KEHiVW;>9+EAe<25Z*{(U0ME>=m+Fnyw{Q~K|l;dfC` zUN1Et!T8t0x$sgQ}Ww?a&p60&eCgfR5Bdm zTm-2vYn8~Ulzr4vDR{HQ^`<}{vdiU}c?}WdAN|IUAM|pdctQ}3otEDw=O(cEhB!(M zg6B1NmOmoUI8!UzichXF_R#6JN`A?VdKk3oqIX8(Mo7QtK-=*BT_9h3daAYg4mfHY zS*HHe%8A`<<(%$WH!GCtG5NmSfO;6^H9O1Gy>(;xbamhgEVOopaUIA=$0XS-9k!pir`-ley zUTSs}N+{l}TQc;5*g{gt*i@VSI1KqV{Tvu0Q*d7wQ3!&VJeKm7BIM*t9!Bc3UlV{& zgg{!(mQW8hnrE9LcQ<|oCWa-2pHODUx;eBa#M@V$3DW7cjbQu;GyP2s@=OW|Gz1?P ze4cEEN3v$5wbGCIo^(B{a3Veze{;-PPEQ2}ti`vs3hYe`cK=5+C!&RwOiOsY=&~Z` zHYjE+MC4t{TgjSZpA$I~jAzD+%bSQkZvT0I*FoN;IhcBEl}c6c_YBobjE_Y*;T}?q z`OD468}FvhDwBV_HxwtjR4Qe~A3-17E;OW9Io=BSfd|nwc>el3I#{{Ao+VrX4}}#& z-b)bANC-Y12X&?ppg{s%`jOMUOA%zqFjKc{fKj*!nv|j;8H%Aqna{|T&}%!D`p71C zTCy#X0xSozo9rVPb@X-s!)0!=yFHBS@d5-7Vvb=!6nC`2m!i;~H_O3r24tp;J07{r zvC}}aONkMKiSksQTA4H(cd|i;iNk%^*2ma`FXv`n(uB{;WQ++qLHC~7>>V6;_GIa6 z`ECkO)FMhiu}XZn4?b?F)}nV)H^<}|$8(qm)Qfd92G3=s22n}{42|Rre0+-sX8CQ& zEDsou86rs2`<(&>!cxN{CH@PQl8h>bfVxTncvg{5`uAKl9akz647q1XX*Z@HI^83R z{j!-@9PiZNOENVnX>O=m@~!f_@8|)yf4-ffMUGf)&z%3W{lbe+cgz!U z1Qm8#jCPGiLt`--pszixyda}}6VL&L%sGgdEoADo`0+LE%#Smo^UHWf`r{6`#M4J= zW168s4cCut1$Usa?}INvjhwXuac%EODtyVgvI8ON@dDM+9qkH&$=eQKRE7AmVC=R) zog8|@u=$hATe6SRc@-qs+k!Ju7<33_Z2JyeB`1caIBf*+4c!^2n{fQHlwt(^<9l(R++^iCNNSr zIVLp%ajx%coh5O3d24f*0|?W)U4B2mkAfXGg>97LOh`-ulCqiAc#G_GH%u(xW50^L z0e`ONf1%I(zn2#mV%h$=xPZ~~=WDuSJJ6r==TZbd&Y!R86zJjh{&QX04^*>1|DX#& zT%kYL|9Gf(sL!?Rxk24HS%7lOf9VpxgNX71V!=Q^6$Px>qe5!)deRQxnpl6lI)b3p zs1u1yawudC!Eu&>+5=K6ODo%K2q{1;dBjXXL)_aSkGoy-WZy{D`b1Sja=6iW+({wy zL3+Cf{N45^yK75#(S1fT@qZrX1N6!p0P{Aoe#OHM8VXU!QnnTr>0<=Ky%ZY%8yGit zA@Ju~7#7`8-uWnJD`axmV5`>;wA*c7HNzmqD&BJVhcuYPafks6lF}gl&!;or^}?Lv z&|Sl_Z|DcUXE6Tz1J^;&Jd#(-egPJPQ#!$_ybh+ZVh(-Auksl;$x9Py zF!gMN-jr3bJ92%!{1oH9&>dOCw0mIm?toY_2F0-TNBx~NbNJM~fm*kmk2Gv+C8ut7 zSDr4oGidywfyP=eKVDxlS*+TH+^+GFPLoJE`~~DH`v5B$e5rI}bs6G9*?77(r%y95 zIl0ZZ=V0^QnWkb#vBY@${-v|f2MbD>u z7~7DO!E4V^FcxAiZE_(^lg1$*Ck)BEAY#YZfV7n@9)wfj6m0lB6A*3sH- z)4kI`oOV$xr%~00_ddj`~yo z5JfMHu1&b}Ujw7(S#2z!T0S^8(`1_posBtroVdgXk58Ftc^BK-dyL(Mo`ciLBXgqs zb*}Zisiv+rjJx=@r&DXt>Xx2YJ+!e(ksd+OKFx%vu%Gt@F3pW@ART10sOS29?I`$7yElp5p0`F~)vNc5s!Ydx zq4WCNRuiP4aA88ahF0NbC<~Ffse5P=U=f8(6TsQSxiH3=C;8J$1oNy#$0^O8aY5(| zH2CjjaS(-@q6D02lO`vUf3};S!1zmmIQ>npJR>`=$g5T}5b!vh{PPq~NScZKan@X= z=Aei;f5q}T54i2y}c&-vsEe_8Ldn7H?f3zP%Y zb=s|Y*XnZX0|rk&ZrWxz0P(!$2CKVcO{DbR@HE-L7qV_u(=m&FakJAN#Y8z;`uBYT z$%2}CikQP;H;Hzi$Lt_-Ft-%+&{H(EcgnchArRM1ZG8AVKKo=*M2=>)g>L%KLd=*X z%fJ-;6$QAi!kz*KHjC@=kjP2;Q>#dtLPo|yDaw`t>y=rcm49!$_kf|X-pQTmzEhs}@E^krWsZ0aMc&=9 zUf`k~CX`d$Pf+wu3}$PzR!%r`UEyq-TS7@EO?Ok(!V_CP0Rx%8ML0GnYTGm%z8wuk zf~A6F1Jz3?Pwme#(A{KSKg5jQ8Mg~4PECa8SrT<66>pgy}U$!c@_r_URow`eBaQSag zmOxEB1vR%R&xuc6$!wf{b1q!>V!nxnZ=uWcqiS>f^Cxp$>yZxNBl^C6sCrN<4lnh612uf zU@sAb6NS^s?(jN~QEK#^vd>MbLUtopXJMauEA%O)U;>OPrLe*pC_X}S_EsF6>*G~t z&fH@}aD!@}TsevV$<$ z&M?y@7@%2|(%V4{fkX1ERZ3`^{mMtT7_P5&$UgP&AuHjM8bd)_S$dwXKmG;;X?Eqf z!Q!9{G-(r~)P0I^SRsoDw78>0(kEO7aI=yE(!wJJgX>*gLWp0dD^3inMqF;>VbS1uksvt4=3WRTu`sFtgFOjlXbUNxTV5WU|*GMHp5!_2L ze8i+DE};DA@2SbToQGD^r9WrQI*i3J8_0LlEqBgi%HgSdV>2k#J6Zvc^plEmDzZ6OuKWByfU`Ah literal 0 HcmV?d00001 diff --git a/doc/figures/mpc.ipe b/doc/figures/mpc.ipe new file mode 100644 index 00000000..1e78da07 --- /dev/null +++ b/doc/figures/mpc.ipe @@ -0,0 +1,370 @@ + + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +320 256 m +320 256 l +320 256 l + + +128 256 m +128 448 l + + +128 256 m +320 256 l + + +384 272 m +352 304 l +400 352 l +432 320 l +h + + +160 320 m +176 336 l +168 344 l +152 328 l +h + + +192 288 m +208 304 l +216 296 l +200 280 l +h + + +224 348 m +320 444 +382.995 388.391 c + + +232 340 m +328 436 +379.445 375.964 c + + +216 356 m +312 452 +390.687 399.041 c + + +224 348 m +352 476 +405.48 418.568 c + + +224 348 m +328 452 +393.646 395.491 c + + +342.698 432.308 m +346.309 417.976 l + +x +y + + +184 308 m +240 364 l + + +184 308 m +232 308 l + +Desired trajectory \\ +Error Bounds \\ +Predicted Path \\ +Predicted path with altered velocity by MPC + +380 348 m +348 348 l +348 348 l + + +380 332 m +348 332 l + + +380 320 m +348 320 l + + +380 304 m +348 304 l + +P + +340 356 m +340 276 l +516 276 l +516 356 l +h + + +184 308 m +184 256 l +184 256 l + + +184 308 m +128 308 l + + + diff --git a/doc/figures/mpc.png b/doc/figures/mpc.png new file mode 100644 index 0000000000000000000000000000000000000000..5bc6cbcb0082449c45513350d3bb2403abfe4a44 GIT binary patch literal 29720 zcmXtA1y~hbv>iI61nCweBvnGXySqC@Qt3`b;GNHKWVzr6qOpvGDD?RBC!;wp z7J?`sDN!L+*R+Er7hRPZeCP;Aeb$4jSMm9a#SmCjX8YCO+BIT}(}hkClU6m;(^_4T z>JC#d^|T>j{eQm_O%w2%vU$D}5H#tdnSz5VwqQ5*|i z@ijZU(`>m>I3Cl@{pCtQWCWHlGG+f59Sg#rp;VXM@mKWp^fa;bR8%51_JQEvlw|x* zLNYTmp|-70;C8ZZ|NCzEx-tbnKmWUTv7QHqhYJ7q*{|!$tMfG<&!g;CTJ$tDBy2qY zy9M}g+sH`SgM*e9p+1!$GG*XA*J~NSt1DN;0Loea8D#K1na*GPl~+%39!qrUj)-8u zpeBFZYoIQ+c-?k|;(VMgrc9rnn$ptLtlklQaHCh$*Z=yz*IgW#&_~C|tLf|i+uU>n z4^X+aHIQh{@$32-95oFMM?JS-$47YZor)e86)*A&3sZPp{w@7-`Fn8Cn_w~WBa)P- z_S&yhzj@x0=l=#*6_w+glamt{H#t?LArV0!R11%URG0q(t0__u>;FIV_y<*0L!;I0 z^mltZ3^Z%)Y-+lhuauoa_zE2S{0}l^%MG6;dKsUCN1=YHd>*CAh$n=_=WziK85kJI z$jG3hN1BU|O;0=9+xPbM!T6@6q}<(ka&d9-@$o^?adFjkb@)Pmz`(Jx+GrP^|DCN6 zQC6;$h$2J6BqQN-KLZb;^ZaGH!lxh1R=`0?vVZob zind46SrVQWf^sr}e;7Bvq~z~P>*LW{C(LT1fwxwzb#g;PgC>s1(;Vqsy?-s3H?cjG znwOn@x?k3+t);aV4Lw~ecIV<}cz<(lJO~HIBy22nJRAt#RtgLZ6craotu5Cq&QnM? zH#H4mcooG8YSXq&uJ^MKN-e#W7 z#AJg~0_`vFf<-+~q(FSX*6P-P$@|a#;y|^(v4*I2#t3^36VQG$$vgPoLgGKXY;b1Pl)h z6yZ$5L^e9@tAF_Lq0V-x>pQaWWBZd?IFFByi${@p?Tk{k{~k)^#K*^vbva#_ou#9r z`}y;yskwO=y~NmjrJ0PYYkh2NtRYow-wO?XWS9h4Saxtdj*uSyf7dhsJi7S1P^R~bX7;`LAOUcB znq{#BSUroON3gCv-Q9A@Z^afvGfJNUcfm5d@@!e12zrL!*wAo(FjpleWNmF77Z<0j zqVj7P!_VvH?7645l2V0H=a-X{PYzoFjYNzHSi+NY`E`8$T z;$Cp+tzMsQzsQ>vvR-bwZu5hiDKo%#d6{3-2BuLod3JUdaUZruUL%!xv~4^`#^-7c z2@@03f!4nf_36GPN*Wq7tKO|OA7@%TE{CV5)vs1f>5n%1aMnbeL;(`;I3FYh!asP6 z+raxwVhJ;;YiL+}_`vmXnz3CR4lV$3rb)-ij z0<7m=+J9!66cG`TooxbE!pJC-Ze2!73b+hR+eDs1c6K%ZLjXn+eL2Bj>8PkSd>(IC z=Bg}!J9U45wwGp)2^%;%V`gePIXQ`JTePveTacem7baxQpH5Cp%<$?J0yL1wQf81K zC?(bR`}dcVfAg&XH*arl0NU23+Yl(x1_1ZnVK_NG^|<`UXWSLi*4B0-{`@aiy>WL~ zkw#fULPCrJLL#lQni{TH6lq8Ghl#H5uzQ0?urS~<;OPMKN{fpbU5)V&m=itLI)ZMv zGyHP#Y4(&+pFP7Q;|)=A^(J-qqHX{C$e&KJ~z9x?c(4mYInWP_oh0(pnp7jB_}sj z+n4!0d(4tSqja&!?X=!uqYKOn_0#ux2`ebrT^+4;cX!v+)Yz>20zh3?S65wqw#~AZ zFUZHqStsytC9}6?G78~w@wo(6>lmcgXw>-F_DApwCR zmhG-`68p2?k#NMstei-sqy&78-#X>fASWk94J2+-Q&T}bQd!y6grHNR+g|(2Xf}`RWf*FCv#bvQvhif7Z>MD7#J7^UYM)|(b3R{57F`G z$5C~h5d>pLy+sZ zy-35yQ&$T`mWDMsqh#rbl??OXFC{I~r_Y%gP*_+-KJw(`+4(uh@$UP+Wo91zLD$so zSZ$BI48gKOc`}f}=~BI05Yzw$-D#lOzdyQQ3Jex;eg6WlV|#yAM@OVgf3%JzDarTS za{@x4I@{4A4XUy25jzLr{{EvJ6yKN_`|V*LsXyBy>UeC8gJb9W_B&V*6p7gE(K(vQ z>PR=3g0Q#8G>k+EFPQCVR}_p%E}z8O-q*LgJCQdjE{Y|369Ws)wvzu_^vkC+ ztl)2?j8(7RafKs)tIKH)?UbLp_Wil_*!#{oy3_ms`HO$no=*- z)!;nQNk#R~xU}lx%3}XCHa+Bw3rP8~N-=X43W!Najc5+AX(jgUleA=$@< zw9TxDu8y-ne>QQU`QVrXZCffAw0=qHk97I@{qzNW^hN#jg}w9z{jdLfr7!G%{jCVK zf1p=d%CpHtQCV17NW3&gZ)kn8@_h}v(?Ov&O{z{$VUgMNRNSQMrbfaG)xlT&tRih@+Dr=eObVtHhE)VhmcMUA)%Y zO_8-|%%Eu``Ci%Y%we)vd-e9JP$mW=JNr~`uVnue?)W$)h;8VQ&d=)12!;v|^WwV~ z7q6|ZzO<>jrzsv4$E6ojqRJySx}~^PBOW&m-aHL>{iD4!wYlJ^CCKh(2F_>S70gjK zRbRBQ^m`h_9kR-1*|q0?;<l8{d*h z!63;tHN{UBb-6{1i0De?5MK9x8URc>CMBi)Um~b&EZa0SGExrY8Q3^DYj{>AsDY>)NB*SqTEIE5V1{O_F&u*fxb^z`;?u)6gSAt3~z zqAF@4KmH?ARaY19n{FmFb8DwBOV1JQ-fY>%@vIzERI>;%N^ciW;P~QI{eou=vAy6G zS5&M4pZERyP(A6|(T1kAT{E2O!SCva(bJRIcJ9bEge16v`J_74$ey0Q#WUz=9pVvm z|5jGi)N3D{jP@XYp{_r#1Z3Rq8MmCYKQPbH(NlgfneD0QF_fqhLVTUhggo1jWJSO@ zmekVa?5^(CzQL&wkd@9#T@>J9i6|1OfiUA6>_pJ;RG0 zI2eF&b#rU>zSllJ?M%r_(t2P$-trQ+6c!?y%gV{;DD1(?BMSh_wO)V$oyD;)S(Mz7 zh2$3p?pG*~wMMzdU;k74!{g+(ML+JoazU8$&huyeXVDP%_pg1U>&3*{3smg4P^oE@ z*&huQnd<&s2k-ofh)(?QMwcHG+EIQ>HH0W%=~&LW%W{2K$j_vv**iE?(m!0v$Yk~= zIjz5czOZmUg$N_IkDkS1X3~Dvx-=~_+>PD z0=!PfB}QZQb-;Dx7n&i`1Q|qP$TAPZLm$XixDuOPyEuPOY%4lt(+K~C@x~C8l(qV} zwx{;q{e^xjA`~6%o2Ov3+;lU8gN^O{Z_(|~AmOzSElRDbr+1-&tfJw%%!xiEg(8JC z0fWQz_xm8?fHY_Akb+5A_{ZgD23biuc?Fh{zJkHQ;$H9|eFer<4J8AGO4!(JR8;X~ ze3Kp*s)utb*%~a7KlrBPWG8>-bT=55W9u2^p6?J&XJ@S&=ryj9G7hv@&^@kpk|yHf zsVwMv9uye&mrH!*RHN@m;!=PWd6a6pbP?qyt%pgFXz8j zN(bCz+gj3-=d-kHgbt?R1>&4D{8TiTgvV32kc^Dh;WF-90mgtJbtstmE!mxy%xcIvUK!f)iA(v&oGD8hGU;l`-kL1Bx_rH@rINy%zTzUGp)45;2-QOB0E-yd0Jd`0K z0pZ-tDAVjifth*kbV+JuOH0hRZ(-nJQ&Oz6VRV9r{^b6QqoGX%XS7eNk3meL29ql& zH%uQN8cnrUS4W#YTyRlwEL#q;4yrVo|4o`sNsU8>^$xAr* zGvDv1!`)Rw>i1NcHXx{8HKX6~>!^D{Ly z|5%7f4RnZHkBucHden;4*#$gFqi~W4oL}FGn``kyk}}d<=LEc+?$tIfMI)Hrm!+%} z6t7s1L@X`EZT|XrAn)$I#vq;^8XmTsEw>Fo+zmIwYT=TW)-;tn0%MevM2x+5V1RdX z`#?*(alGNtSbxU)?&CyGbibAwZD;9i3;O!8Tjlr}S3e(KUkaV)JETidanffXD+xsm zhSMl*TW)SM^sVLinHQj|^6^|lUHxKzCaTXgH!qLV;)fwu(cWHa8-pF}fR-9$n32U- zwUYF~iP z2m@*(u*|9^RcmJ--rKKkD7QB@mYbs&>&`jSL(sQx-}d(QfcwdjCunGSuAMihI<#+k z!8F%I;?K$we&kp6zgVFiYiip2I72W~`bNKHaQ&0O@XW+$G%?Tg;N09`cTRAr?ZS6@ zF{XD_{>3Q%{>nu~`;ee=%%4ruw=ShqXS;a){WffU0(YInBRUeX$TU#Wc7ol|x9a5< zaz>_wT$jMWr$&>i&M!O+GI)sfT>v_9+Y+fMak^2@#xeCP9af@r-847mbz8x8nNM!M z4~T!BS<^(Z5=#a|&73?#O#;KZyWbEId3Wsz3sJxNAo7KvzW&VKVfT$|c}soiOZT3paiIL+mv?GOg5sYGqD&%r+K9Hds@J|@DcwX17ct1l&pPHP!v+8wmqhB_fG5Fc? z$~QVgDq`1c!|2_da!;IRO@98L8>cVn2A;smq9h>S>gukfH@Q{j=gGWtx_Zq^HZK{i z#KhP^rj*Fs{Uz8UwsmcAh+Vyu1az_Qn6V7U@;QrKBRrY~FrUPcKmMaA`+f{ajFBhR@uU#*HhsoS2^_e0fB0v=PN& z!}^Q0fYqyKDLbOl8X~Z;{bhFMk2vuj%Sr%LG@d`_f+#43Qr}l>D+tVeB{F+C?%m+IJQu>_6SR`Q^+|Orj)U7vU~F9I zC*R9SZZHdzuh-Vf&7Q#{1q2~@9n#^S;TF*wKm~JU&Sb3jmIdz zE0$m);+B(9D#`t_aX9kci~FWKT=t|_znKk>ndAseJNE>e;F#O_>uoR=U|oBAuF&eL zVfE*{*zD{INgg?tp{47cvit*hh+xX3*LlT$>Up-|u5!~FRt@>nV1$#Ck6Iu7%2=@A z=@>faYsx=NXv}1>Ffo09XEB^16yW`Me@TNeP(a5^=zLGMfeO?uoMC2V9aUG;MNEK(y2I=IA>MCw`IggREdAj@x{xomK#=UpT3BMF_v*?@t zTe=;&zdh`GECATQ;=a6nj@y6z6`@s?atCcIQ7Fy*O3qy6$FthJ3%|BKny$5XR_)P$IrmrP7c z%^sJCe@Vz@SZH>x1Rkbp*x53;lF&6j2rNiSJ+60avXt2)W+OwUW;uU`s`OhtkJ|j) zO_Fk%Us0^PCWPD~N!e~!vZ&!^@^jdm+^$jMmD6BwpWyruF9U5?3C)ySyTpBx;U z#h^vE)_M*!k3qYPn2&;kObD-|bldF8l%mHBKM zm-=6473k;;9uY1qJhPqvv^UDJHa96a>(-3w>^dh4(1{sT1Cu z-remn>HJ6&An)A9;pRs5xqIXOaQA?X{gO*6aZ=UV!}Tt`ngF`WeXNhnUd*AM-*0`p z2RrZ1!xG4i$U*FU_@&Nn?c(n=0v$cQ+vUF{a-Tbgo3mX+mMt|rk04zPSu`qN#dsBM z4$nV^TS7<|LAO`DtDmmfh$0A08m*>ArDEM{!|DCl*|A8`-QAC_!+H~$g&Z9_15mQo zEOecm%B$eGxblnduJW>ARf!giXO~Eqn_%eacgEnXt@$4a-v*ACZHGHL;d*)I7Hb++ zSxt2&vQ*(QH+??P++6G2_W|An!9@sByv0VuwAD)j+iLH5cqR~RnGL5a(`#b($dDmBt>$m2VyQIjapZplCpsz$gbz8`py3J?^i#qB^s>G7!Z@WJ@Bfk z`sUK??D!_&Sz7t$()1!%q@{;H>*^Bpe(+(sxrvUQxbZFN*v0{Mvi0&^eTJ`8w%;$0 zM>wS4p_V_)9i7)3d4A-u$KSlx)>oNH{m7ms?XO7u)cl6IzAks5y1VDZf=^0ruP|9X zkX1X_CeLyf;7ZldLW8IV5#`yF93L#lw+0+|$tana-gr2#l;}vD?M)TmJ{R2|A|`l$ z)S8ElZD@-Tj@Le4V-qLn#12L#3NI?^`vsANZ~n-4(==mDo7<);CrNhr;r{UecztaG z*jKZ^yXxAVU=kfu5>neuS@8M2?UNxRWk+5IpGYZeg9%*yAwCJY^NWxh*XVBf2OR79 zvmBd1qOFY$98yxRv(r;NL2<3+e?9z0gbvWC&VuWgu}y~HO4}8AYUJo|-yB^I>)fgW zLPKAh8AzVu;`zrYcQWBL=Lwu?bIjw%4-i=3A)QWd?B{>@B?z6IcnJyny2Fud=S{&v zFUaEz^!aV{w68pFupvVk6Khcd^R}Dm!cI0x9H<&DaeY4}`ylsCKeK_BWloqMXp*g!0y1<5^uCDIQ`9AIJ@Ydd~7aebaiu89S!F%A}B2_;_@#s zk83$U!gbwzt&O1tQnrLkXt%B1BIMyQI-Sd5B`jPK+*6`a_6g*a3w1mXcU1u)(`17A zLx%LejXyjtLB#JJLXeO^#<10ka5-E?CwV_QI?Bq=uYa*J7jzi@whqbl9jY=+o9dLBFfA|pUb^Cj{;k4w-7hOfg z!uYAsj11L6f2zp~&|P#Q#3XCW5{phms;n&c;sJ}HJXu^z7<=n2vD!BoLqas#STUGR z7Hz18EiL_4r|zp}ja7be@y^UyaY+#2yVW(3)l(P<4ZXb+j+Zz8o^Fw{*$vL)(!W*U z=sF_{*5%M2F5cy@g}M6R4=*N0MjhptT9e?71SZ!C$Vn2kZ5ku?ta=9mGDRp~qjK0- zeIOOY3T$ol2^eok&~c&LJh@}wQI`3nO0C9Vo3!#6^t<#WNEzg%wHq9=^>ij-Ai+GJ zMI_4`$9f;~@o@mspsNiLutKH!LHTq9?5LZY89qK%oy3F%ygvUbyzlM#(`Rd$T-Un> zHWPY~ofw}+5R=Mjbh0^Z6SBRvRbxJs@}0-UcA+-X+ATJ=o^RDuT#F3d@-5*~)cQ{@LVcE;#OV`LFS|%4<|l9D}6{&N}wH4ljoV@JIT)qYideN zlri1J9-D@dk}7wxqx#F|VN802`?3fQYMZLjFx@nkktqobT+b9yKtjq3FJ%d&14-iE zPt<1xk$L&~S756JY}<*O{vLT>xAw^qf=ak$FQ+(~_b^Xn5;3IZ^=sVE6{V!U>An6M z9-vj#Hjd3<PQ!8|925kIk2A^q#ajR%$$Ij9`I zV+gfniWnNh8W{^t&}i|ZQ8#*zP#M$N>uwoAzIVSFnrOna^KCAG39YKHvgJUg&3VPQcP z9NQ&-1MlZFv%TM_iV6$Y*VmCLn8gIryc|9&8*!J2b8H1FHMUN79sfC8zAp!J{%wVq z!U@<6`qE4@cO>a6=sIAwg|(gu_1&_>2S!MpD=Z0O9AwPyFkL3+`|jNbNB0-MwRt zn%eMnR65s$@>N`Fka$3Ox%!B+sKNFs^yUpHhe-r58X8o^NO`8&7_Bow-z`P^S#6`Z zB;(TpcmI^KKOnd}d1gHeVNasDbgP-)Ip3E_bHp0TSlsNX{$?0TweUL12+z+&c!Aq~ zx*cg<6yW5W-`Dq)x$L2u{8l8Waj}y_&8DOVRbIxhaTl_r437C>G9YAtzTG|G*FcJD z1B>@p2ls*akm9*M<$}hFro`>EiiU)QN#V)8s2*wBdzghfxL6dg8{eQHNp~T+&koOB z5RpScvY(IhJPTvh{yjdkuIu&57GNd_NqC~sZ>W=1psx$cTVV9WGmKW3QAtlzK{}C~L zwW@OKEz3SJ-R`10_tf%oNJq;J-Dxle)MZ9s50}TAje9!P+wgOF=4a_R8}Vz5bVo9D3jvRKf$mt^)hh zn;RO*hl|5@Xfjb-QiV?-L!o6>b+?d8f(Y+4B-%~#aQy428SqL!sf~c;j^?2V|aQQb-+7p2V$K+ zMHMV>Z&p`URsusKm~Y>Nr(vL{wx)uD!pz*n3x%Z%F)mF$1Heh4n(z zOmK2oN~(Rb03fp|%Hl8Df7V#os9#YtoHy3RG%Mj}tqJ%^O7-CQ_$W6(d(iUn;eJF} zf8yX*)pc>BG2sQ(X=-rGXZs9y%C*yNqp8<`P=E;M=tPh4b-v_$AMge+NPO*D{4yvA z_;nXiC?};sE-)YL53AWsG;0m+?;j9U1_v451s;FCyjSJaO&zjOqoH%QhzN6UN?7O4 zs3$NV^u0dKti%SJ=ltpYtNHf}b(k%;7*kS-pGRJBV%jzo6%C+}Ukh9w2E^ko*29E_*`H7^wkrHJ56y?m z7+W1ABoH8zmN7ci|L?YI2(Xj35kv^t;wXev3=9Jxdu@3Cd2Xt?`v6-%QuUf|<1^3p zullnL>YhhHao}8mVo^H3vKYl3cKDB>rU(Zg_)3tpiU6+T=obFVHLi=tn zg-gk7z4Dsq3umLH;4nMiP{vC789_3b{IMeAi@FJNYVu`h+JKEuO!TneZ2 z+uw2oDeb$aHM87pN)W&aUOy}OZsK!yo0f{a4+UcFF&^Q+veL2U09mz0sc#_s&m1M( znj|g$$Hh`cb&mEN2VwWcrPJ)18Vof+cV*Ut#e0Uwl=YTI7`X-$#2gM)W!Q;45lQ6U z{*JjSR*%}~V?aJlVA5&Psj-|I>Xgl2SCu5r!8mQMR1^zp4D$YHWw<4MbPTM|{uAuG zcl|G^0u^t4b08}QG(_1n)kx5@qfX2=Q~!!Ly3ME}!ODarfSioZh@hxL$WH)qXYC@sBtZ{#o8@iI`6Ctx3yVrS$BbC2-pU(`3zC-! zn8>3K|AHkWX+O-;}o0lnVN1%Xk3>^r3~bLr>l`sw0e3yi8R!o#Y!NA1ut~x zeP39eO(}vmm2Is;Dk5M*@7-vHZmjwz9JF;NT!4CK;&aW#vTuebI?C>q) z1~%Yn0+O@I?}*F{qSa%=W8?Sskd+k$msXM_VCd2+{9t?cZlU^v3ZQG6iJ=IMy2qpY ztT)8U=j}|lsN}Wvp`bonn`WXwwlW3=OY5H_NGmZ71L6+{2S;vh?wdDnI@IBuoLDU| z(zxIpxvy>jURE=s1$Q>suleWXY*m^KR902>i8JN#*E*TpKWe_B6zY}>;3c!3hnGo& z4Th6R@YEHCC<90H?B@+F64KHRHaFv9W8YbhX88L0?(JF8(&i;4B>}3TfZK_vib_@Y z^JLkiH*m0ULQ*KUD~Pmt>ZWQM8i20b0O*fYK$nR8)&nTs9+;DC>|HzUaWO= z^+r<4FORlZS4vLvX{zm2GbXT$;r#vm0o4=@9X%i*U}$jA*u(@7@|RL7{CpwTGh{Zl z)#DBINtjJZ!E}DF1ZKVY48Z$x`>dg?jsyuRJ#X?xPe}o{o`wOo0MCfPoj0-%RqD-J z8U98)y7;m^(*8R)hyJ^OPYa-6{`>b2tVnumVV<39|9)Mhke-kWSniyF;yxk)yLW$lD^E{f$f6NRf|8s2qDK@tQyxji zZ#Z|C+gG$b-XCv#V&e4Z=xY3lSaAAh2*(KnNC1d}as>0n)3|*nFQ(c+Vf*wHJ2EhDwg$DSO_ApQ&X%? zuOmpG)!9A6MWPIBqz1zcO_+xHnOo?0_vHJxmqvZq(r+2*=*o%MlDbW$`=y`NKI5SV zof<0fw)2Ptn%33~HK5W*cFj~3yl39xv60qO<3d!!fJ ze@1KKNfS$*UD6e5FL%a7@+tjhHOp&)IPC!E4j8)~o*;6jdYf`aCr&N~1$#cCw284{ z7jj5c@NzTddjd4Hz(H0Xo`u0m7cFlxJ)ODhUo}z97q?ew$45sQ;K^K29X137q1ic5 z*r1VPl<5l{E(`y88XQh3$7}_P0aA3YRa;*lJr76i@89Chr5MGfqcw3F_jlVZQ{jOl<&+EzdnQ>N8X6i% zpb1x&828hXM;a7)yly9(=le6D;{pIhGAkcW@_Y*7vdfs75+tEA+L7c@cOcZ0aT_S3 z_15AKE<{tZ0OWFJ?(fY17_s(*y>E8&07V^6p7v)dQa@K*V0j!>A_K)=-z5QJsG%nkj5hwdT-Nh0hlEt9lps zMHvg7GB6qf))!M-04G^egowSIhfz*E$jqGm@ijS@x5McP%Rin`SVF?pRj}EgQTjFf zNq4gazJhhW5`&_qnkd94o%d*kyKVbmsM}VBPVyx`86d^dqcAd2@{(+$0Nu?Nz@whg z3n!@Hx(ZpO^X2cxuSHA+86RJ#^SFGz=u&#&__&Tx-w1-!>|FcOuTgaacHv3mx5h9Hm*W@TlKV7i_%`;}(GLymH0t-&L7`&WPDgr&pmonBd4(S?W3 z6Y{4kJTMzq&8-gu7Ian?`iKfcNTs#B_poWu^qLQeB+0t>?PU z9OZC*T7837Wb^7xkdUy*TC?c_ghzN&- z5yBIW$H2mJd3M(1eqLCQb1iy1SopnjMCY%nM-iN)e-*OL_uuB*UinM~Ss(Kkf1SHV zEE%G5_YQxtU7`yKPz)XidI6w3YERhQ+yvy#iQTi!O;atcr)|raqPKauxi?o=NvWyt zk_W*ab2vT=kQvrMwFD4y3CwhT-Ts4 zBPWXJ3v^6?oZdGu!0`HY<=nxt!NT8D(4zz@8jwp|EZA)x<`cd%mz0+FF8Iiw?sLAU zMibk&as2k}+qk$mz}#lIJ=8aTYR`CFSW8KvxDnnKt)N07U%y_Qo=(inBqk=3vssP; zy-7=4e0DY!^y0;ftE($ZOUt9{yRfh@u$%d<$?7!6ZU(4@Tu%SUIXRsJ1}l?(69I(B zth=+Z@f50oxC7|54C;L@E&@+JAiuX!bTv+=ovj`mlr=FikwV~tqQ?~7Z%)_|gswe} zI}wG&ftmewBhH^ zuI}zvAc1&TG27~o2iDKFb19>T1MjdinhB&`)8w^46#|qyoFOz|t!cVE#y#V`$#R`+26W69P zZyj#%#Cnvr8j2uweaHSyBODC?J|el1WN`PU)7dAb%%ZusbJ_?D7BG>qi=o+LFj zHRRiE{7qq|MCTIdH)mQKfwC)GBI?AI7YYdtEl5YVFfo~EzU_JRI>+4m7JGPe@t5Jh z;}?*badvb>5ezm1baWP$m%TwJKyLeBZS5E!1JF^O?B3sA z&J=5jJa{~@aQBcB-9S1k$|KDQYKek{nu3DNqXTBcUQ+CaoooQd9dxv7wIFdPW#iLIJEG@KrQNhwH6G# zl;c}fLc%oAq&7DrX z^=&rVjMH(y~ z^`z2GVgK0C*%=-luBWHx^KiZW=W<-s6sWh{+^(yutJSGlb_SE*$u-?{VuH=8y8t9i zZXTZfy**UsiqV}TK^&A=I~GPpYx5^Tqa$~7J8mGru|LJ%S)C6w-Q3)ODha4I>wo>? z%|avb=sReI&a=!9Y)60DPL?=RucMRZ_C|>Ug8| z20>a<(iI%;?x5_s*csa>>Twa(L=7?MDS?v&HPAppdxnr|Zn)wv(I5FlX^bKwBEW4v zKis*si?gtl0SJ!=LKB74lM@28FEKIL*ONUx!Jg4EG3%?Vg8VdxLWWgy=R$==MFNMV zp}qS0`pe79o4+7miV%RUGZMBykrU9D`GC)&f3`n!D5Ez30s;fWI|8&5`t*^gkPt-` z6%fdP%G3HtCx;8=aIUInVj{M4Eqd-tP|%bDiZxpi$T)!Hy9t0QF9jdp{qaUm>)nYA z&Pu5}85vncMaB7+_IAJoVs~-&49w3zhuVRP_+6%NHOlnmyi?NOm>@QgD%sfBfL#?p zntFm2HQ4jBGf;N{d%OMuM zMxedSxiBEBJH5DA2KB1CdIFHo0@h_>Bhd7D-<%cXN>mh+*}$VEL>b%Kw+k` zBe_x@K5qn=P-f67uZy}`E#$Cge+r+mba z$rHb59ezVbv9qs%`ueruG$WR~6}E7GtX93f(c&MS{{QC!*w`>Zn_FAo$fM`sTwGk< z76Dx&i*~i-;%*350_^(OH7Jc#rUXIfgBa=I;b_SvOPYvs^377KPkK}6Dv;M2G-{4s zWMyXqpOXfMj7%^Z;md!HqR88*Awm|wCL>3P=~$2~EJ8vc5s~*U@}{9wf!seqcHh(0 zrQ7I8&&ipnJDA7!{?leJz|`+R9iNrT!Sj+4VP1Q?6iyPo&<`Zd@UZP%rP(|4!Hl0oJKRoCXfa~-(BQwyx#YKRa{wE| zf+v?E5DS}!5rL@3kX1aANbDcv4Co$fMSi8~Pi5I^V6xU@BX{0mtKCq47f9|!(PaF@ zsb9T{Bz%{fk%(FHwdt5Hj9qOySu3FK{0 zN^&!^8X#i@SvN=u0gnp-9$q$;qXc(KPy@`xiv#T^9lq%ihQ9(^K3wnLMvsdFHy~FB zS!nE38!%=N+foU!_95(2*@(tU5as~Jh)YQ1{rnk0#@7g{Z8jow&~~u`aSQYqKqmJ@ zh6JH2ja%DP=0Q3iKX zA`9zsIikd|n9e%jlgZ1c*Nq*_#IQsK1%2t{)k4~Wf(2%F zTzx4%S!Y}W(I4;XXV)Ca$!yaCg}&g6Nd+)DpxG}dD#DRaex$5B{}{at@R_U%goKWcPB_mqYHdBQdBbpU*NH0R3Jgn( zG_jDz_sof@>FKSY^ww{7|HcFW8p0wXa$oNX1v=WmprD|@K#-<^l?F)VT|f#XNT{Bk zo}iC*<>@UZ)*U7AAc=9#d7SVEV9W5xh#hPCu?(NPo7>;{nxdaSso%UATU;c8NJvOP zL~PJy7Q*tUZ+1JSVP#d+(n{_&ZUz*60Cm7}ilYnwBlKYQ4G3{SNz0K6{$mQe_GJ|n z6)#WE!J(mKS#yv+gV{GRHr9S%@#Po0W(&k%vnk(o^OyVRh(|e$WBnXFQAjD94J*il zPgmMNBtXXi`{S^_!0D^Ne>xFf10a@W{`03#05Uk*IiF_y^kD{6 zMLm(kbs)C|fu_AP<=-26dK3sqz4oJgj@N@$L%+y=xHy;t6Jt66(jh<^((nO_>d^3T z8F_h(>$s8aGTjDcR)3Iad);MZP$;E!H)4R)HU%WM=?3&|$G!<23<&wpUiu)+Oi$wxl76A5W4(!U-D#-S;m66Klc_?7QjVs;DEOFozH`(mzUQGeWduu zBfYTDP|6uU1c;b8)tRR!%Vakz$2Zyuk}4;%cXoDmbnN2J$SEfh)B`DOL`4PQiF&{j zPmGW6k^AhPG#T>zmkB0-jBgbNeEccn8`sh9nw`A@Wh5B;a=DSnI}htWDGULK=!HJA z`ZW1bKA`mZl-S{p?)^W7eFr?%fB*I|lOhTIvXvyOWzUn9vPXr8mdwcBqs%1ZXJ;m5 zM9LmTLlR{j;aHVOWE2fDp6hhq_y2kB=f0oUb9(*CkK?TG_w#;V@9Vlg*H@(@?NH0W zz`*O*Gk`#19HL4}k1klT+v}VZqx);~3)mUZ(K z=8<2-Iyc==!C=Oenx2mN7`t5$4}H#Ip1kQyw0!hFdul2?qWOiiYyTbH7}^Y##`s;) z*Eo5T6S`lXDLyHqa#>uwWLe1tArILqL8sRfuys1md8;FW8ZI16V+y>AS@G|@MJuLW z9rQzd=0bBGjyADY8aJ3o#f@61b|;S??_F%q9czi58>&4dD(ZmN`pEaY@8YCkbe(3* zlxse5Rap(V8)eRJtuDk0NgrDDhQ`L5Hg9J87L+Mm@ku`THg<#;6)_UV$H!?)_lBDn z)p5Rh#ksb$vLUL);UoMYv4K}w9Y4Md(m!dn(bTwS<++`td1K(}%N96$1@DrjvP(-P zGW9MVOuG^Tp_Mcec|SMz$$J-+Zu7o#(QmaofnQQmwsZ3=4Iy*^U}_sL?`aE*FH=*? z3mqylD$2_Vr5+nX-YKgEt$dA`_aN%&y+K+HAS+%H)aWv1+c4xRT)g;ca?%zR4mjEM z@QG0KroQd68R0S6fwQ!Pva0az?(i6Q-(6H4nOBYPa!LoTHP#=AouGFN2V{d8yIR9J zJ|Te8si~^Ih~*ot{YIn@@_19UL9+ zg@tJZ>`G~4SF{1Ig5u08?U-6rG?l9sp`)W?{#7HFzRqATv2}%T_&kq+iHYF8ed|aw z@cgxHKwv8P^4JRvO-)Xu3Z!g&p1E}@SXpIS=9m#{y3bd=!&GaT97lACKocrDZnZ=7 zqRSY1c@#Va_v}HI0u9l_L(1QC?YGfy;CbXgL~Pxq8JDdFf*1hMA5)ajNVhxoYUyKt z(c}0h*7q#y-wqDerzuervF(3)&uZDg7tEJbL|t8`CDES?i7#7d56AJ*U?6$C`eP6^ zBVqeihAXlG%N5qNg!!*ucfgrLKv;*YtgPgxVl~}qk$l2iD$@=8f)YAHZU{K|$V=OE zM|XPUk%AAoxU}?5d;9g+*nPsn{GUDyBjtdC3*SvlNLx1%kYiA2d{?^b(%G}FQ=~#H z9Xoq_R$BC`%QIxDZ-UGD)smSb*LNHOYfly2GIrd=q|8)IMMdR_S)lHsmOPXIEHAJQ zW@0BWY+tSyn^HT^b*<=e`V+ts&OmK%akFfcQZlwXY1Q#?tK!yf{{Wb%XI zzrLJ)^{+3}doSb>T=9oMDC?5?pD#&t|MO*E@2!8{p+YOg_j%{cU^OhjQZvCr;4!vZ0&Qh1D9KSdz022*T1#x^O_-l$2jm{g{H8gBC&1UC_1c2#n+gm!Wr{+ z>D?0=H#awRs^q$f9(wdC|Jflq)DVb8;P4Ag$aR6d%KkO>y(0F36#N%YV@@SUAX25Y z?GD}FxWpj9!NCC{*!;oSF!~KfrU(0*nu6QLA6hNo-Iq5sSQzKYXskkB$%IsMuPAiA zH~$tv;Q5m$*hv{AdGO#def`lo4+qiiCcx^?9uWuh%RJ&}2>o1gt{?9=u8}=gfB*Ug zJX5AJmXMJ#74z1r1zDbrHtWHhzo-1K9P{q!{Ih0WYZt;zp-WKSm9VjAXFKt3D(z zzYIzUj0S>ESffz=CqWjFiUD&#MbgvL;pP@&jbf9?hFNsCpdgx`t1LGc7ng&BgX&8Zm1`*7u1lbFz;bCbq)9ovP26j2 zvbn_9T`D$QM<_Zrb~|I3QDFsmJ@o3UTa@=pN}3q%(GW?R9jw;3vTA(%cnxpjrnJ*k zaG#F?N9~=}Ut_VMESOVbehy(e-9Zu|14$*6Wztl!vmjkKKa5yGiz)9g>niR6dDK1Lf!b!(I78(cSl>>9O;Yg zsfJco`i(_p<=1itu9v>)>IXU*s&)JP8Z0P=Y!i)4_uohO0v7Sw?QzJ-&CU4(zV60& zodSZ2iVdPisN|;qd3xFMc-$iD=xi4F7{M24E&b-EOe_ncjh;D5;)JTIvZ*q#ifeb4 zt+k-UV{KCf{7j2Mg$-p1AvQVs&s6rE4)}iDrVS`$qX2xp_C@AXZLzQCG#KFg4pz;J zh&Tc;`pC?}qW!&#AZzOtx=kO(#?t(-zV~jvJzw&9V#0HZwAZ(XOs<2TP?&jqvv^9| z!1WPf=_9-F%AY(58ENoeas}itAswx5dGVtACg%D>2M^j;@eZpW2`Tjn34SqgjH+}~ z!InNcq9_D+NO%Q*4_1Jnv+KF@Xt7(n{RL1C(Br{V^vv9;adFmX&OC8+cX81I8MN?P z=Fb}fgNHp^Hqx`?O6|4ApKa)Nq5=YkjrsTiCEInQ)DzOvA$H1RCs(XXKJx8LV@MJhCA zH{ZN@bE1NyRMpmLpQo`F zV|kT0Q9!{;ZQuUs<}K(Gd**8zyFFv;O}3nd5DnK0jaIk($d_2PERPOx%0(j%-2N(Xyk8 zfS`=bNny6`n-&r$#W=R9iinEtW<6@GhM~w417Ut%`AJL500fHxoz#V;$=%>pDl)jM zN#AQ$$4bW_fIuLqM-wix@MvHVzT-+rX?xw#(f4a5SUW{RPG0`tDIm6)nVEr~!Q6z- zW&^8gL@O0}0pab63CNuVm=X~|S-(d12XY5JjY$U|17^ztudS*$XCN(a6xbObX`dVD zy|yAZd>s6xx;m`~*+PQCco=r9<>{pEe~+Zd&^SD!Vf8N=Dar2%Q(dP5BxSkvAQd1- zt57#h2cKdkIN=kR8%QLan!9Aezu`TEAl;U4*+}W2l9!Zer z^p)E=EH6rYzp#WnximR98)%AIStB5K)5;A=NC3@0aPGyuQ`M#Hb2Go>a*B%V0jfby z0C<4^CIY)nesQ-gCHx&7y-6y1KkQ~;bkj3kU6H6a?w8g-$tFiwpdk?I{^E(D4!x`& zV$(yA+R%cG#(d{L$giYxSeQr_V*2>$6DI#ENk;w+1VV7tq&pkGQyjg->4ul@U0z(@ z@oY^Tpn{O2QO8RcInhn1^zOTu9Uzo+4C7QmvvhhGH&7%wKiZ2$IInLn>MMU}#cbA< zlU4y97)e0wTv+Eo1t4_FXR!Y|#r1lp2>=&?AWy9xvTomjmW2P|p}p0#ZrdZt|92}I z(H0(gchK+nrC|X!#?_S-KzdlU?&#j6sD4dNM2HY{n>e5u)&Y@SJ*S(k{0NEyloHM_ z<>lA)1|OW;5b+FIilSm-q)WRisw9pMuZ^ew7>iY5RpkZ_9&xM3QOZfKM}znQR4Unh z*Ho;kyxec~dmkQX73sRK*5c*Zp+`tR2Bq>B`URB5R_ z`0Dj*L_i3#+Kgoft=V0?c+$k=_LC3_m$Tv+8xvDmQNb47EH=6Z*o6~JH)e#HQ@rN( z?NAmY1A|UxI!WKqq+%Ujs=Ub`3U8Otld5*f_WFDeIIx(U?Y>=smM?HqUyGX^uxX=Fxw4?%IWrl~C39 zn9SI0wg%!2l0KwX;JAPbIg zRr}DVV@k&!xymyc^%L zfAB-Apw;8KY7^;qB@ul&&PZj_nOROvVtVmv1|IlH%tZ>DpC=&dPd=!&NX}PO^7wwc z`{mRz_7z1{5{3Dz4)5;;$Hc@mHa4o}ecMZ=`VxcHXZ;PYaGoT2aY68Ljfi=%6Ct%> zb0wPFg?JCyTvv^t4&fIU*IiS!m01+u@f4QtEOp>bGhlarHRUTi?npFgi9m;j`Rk;+(MJ3BiZR>=EoO8RpMTe@DobDd)tgq7!?Kybx z8}wL;E2OY7sLFXXD>G9GK^X}N3Ba>BvU|DDupcipHYz`Ib;{~JpShH8{8=3?$E-Ux zTQRtwGBew%I?<6m+S!@>0f&=L)K|X9)lQe;!fzu4wFA4RIYz`^)ifD1Nnd6j{z_ z=3tuG{{3Lcj6rJ}?&^wPV@p49PBS1tX;s8fzIBMy>8R-psJb(hGVmWv zyKY|#KMo8ST7tKve}ZQ7nzityWUOLFNgTN9`sClHlsb)*`@O#OKN)xZ+!_o=!}-%@ zfDR*d9-`mNziqu#BqF7qAmb&OF3+Z$yN`vRvFui%PyrPDbt(jaWuqwcX7tD zfxlM{`K7l$d$w!Oo`?}K2?>BR)~NGOuF!q``n6rDa3h(K=U}4%hFB6wc8*`fcKdW^o?zh}_}fCzb2v41*g=Hxb-*NITTi z)pufwef{dywcjlrD(e^w+g`oGasKktBcEy9m??jGCbX*9pt-f% zuJz;Ysh>U2K_>1H`3vsfw`%Y{;3gl2eX8IMoSZ1UTK`WAm^KiHves&&%cRR-*E}i4 z#H;uVC5Y5jV8NYY8#_*z>9ivy0j05a1uwCLV+Ze+DYCu~K z16%?9uOT)OTwhgzsBY5pHCthw4}h>$*DfjiOfge1R$&sesB!7OeH4WZqX_6K;zbjF zVXgL~Bv1CD#lS+Z1%=9t!os~NZ4~qYzf0;2gkl6T*VLr0zE)7=y^n9l@~POkars(U zWIzzlQ>(t@o(#6p+RpCiY6EO3n>IyT)_WS>1i!s_36d6LP&z__5j1L7bRDJ)E>2E7 z~N!UbjBiZym09wjS zoc^mX(4v{^f&!a%)`e5<+DeIHIBk~#xcT_z;5PpJ*=Otp(?rUHDbZBy@hPjWMvs-} zp>uU}qYyGjvJBkD83gdESfkXNJ5j|brv~b7`)iySZ&D9jWik3Qy*$B?(!CFZRgVgv zlCLDfY;)h8xIV(h9C65Pq8+CM*1D0GB=k~bnx9+$8MJ;xpX_|>lu7TGF57@U@$|AL z&6Eu3f*kQIouNzn^;zWEhSQ|Al)P)|>de1iBJt2dfHAV&Co1ZPVRZ810Js-JneB?$ zmW1Mo8emedBLq*QP$)x~YLSL88>?z56nQTX4BB9NjlEc2o_`1*$OI9EUfFlCz(PVF zQoyTMMh7Hsb`9`{-ZOo0WkeBK;*TCF9X|~q4ciek2%SfQNyjMRK|mM4W)N-B@9uP6 z`8=BL^Oy5*od!ehoU!#@DXAnDS7giCACi-+MP);J_r=*f|4^=yW2*@!d`we!dWME9 zel%us@E#x~xsUkJ$np_7*Fsg#7#J9MdP)mC9~kgKd^@atF{ew`G0TD)V@Ru+#5Dr8 z6;iGdAmFb&iVo88$(#Dp>r8h=`kMY2daOQT!w&6Iq^)eFcL8e>-%3fLq)S z1+I9jY5@}wiA{Lt1zj*E)}>xqPRXJ=;{CX7Q!-Cu&U3A{N7 z*yEdrzf6AF7w+rrjWp?6G_QT98(5;$@%HY%wMVpX3nc8FJ9olW`Fz|JZW9_B2%LXX zf?=tEkzCr`#mI>D`>EecOS)N#K2Y)S__f|&cM3emy*oPemyZ6>z3l4xu(C3ruX7eo zekUhgmAYU#js*tMllk!pCfqR?c5s*@Sy)+E-c3&r-kRP{8~c}0#GNc<(xo_>9GZmb z_FdsQd&L9xGutw*MhXn+^!sK`GpkVs0b90P!RGSXoWc?kyz=h0o}NGA1k^y1=8WZ( z=6LmQ(dP*f&MD={580yp%NmF?Dr&}8#^mzluc-&lK{nkefKcPV6un*iZZH!%6QNyT zL2grprLpEh8;4U$UcLPC_>5l2s@PM2*3o(|{WHg(No$M~;ruYX@$e)RtscO0YFdiyxoDHb{Cqrf=e*a`zcrNi)p4x_v> zucjJlAdKje@`n!rY{0mZud$0FBNt;9hnOk0Yfk}+X)1t#Hmh~bZ){u#(Skh|&z?O4 zLxMKPRcg}`I82@FenxI*GTEp17KCAAhiE2F$w^@A?WU12G5N`Zt{xuj(albl0>KA| z&2CIW%v!vrJTnIT=k!%T9}g}N3cpXmfIh@Fiedg!tmchgc+n({1mNNv4q98_Q+Pf=~t*!Bz$URdY$#~TH@&V_;^O5OF z!eg$rppw(!>O~7mw9@@E;WG<_K6@|R5Y2My{{0Tein21~LGA`8R=N}Jhf(`}L z?C;-DwdpgheDi~EKYo-J7c00;Y%YILQ`7(S=`9W9vL#;UlDRQ51i>C`L8W~8!5^DM ztEqVvhj{x{(rWT2KQS(NH$X5z^mtt$t|tHp`WUAIdh#P&u*Cb|fnfSf*~_q`=v!V zwA{e847R%CNnUNY^6w(8Vf8`y6jN|M@cPZ+xN~Id9vF*H`+nZ78i|=#(`GAt$gy=h zo5#LFrg&nLm1B?CrrT@9;G=?<$EnJ46?*88o-^)8#5<71QRmWA)e_JqmTN)UvaqtT z2|`dfd)d~uV5%Da@OEfpS(m_?Vjt4R6CvJ(Z&B%7*_q^)*9cHH}nt}@4!U}e2sU;!@&tReUT(M z^M1#IoWivsaI5iIu<3O+!A1A_(SA|6!)aFe*62kJ_5 zYk9f*E)_i!=XpH&dJM8$$uP?-lYYW7U04~lQ9x{8DK1TbNiwU>DTIYGn%y(6EIL&^ z(fAbX=wx{r&>ses?0_F>@;pXE!^1$7@GM`p2Os+Lwalo=YV!AW6a_yWopxYX%^TdU zLx=HVBvQ)7h~+GFvwy>7U3#d(?UP>2$6z#mTzQ5!HYGisd@F4qC^Ob5 zK~SJj4rcjO)>kMlA_m$;WylkbBStV%CMIH6NXwAf8D76U3h_NR7xZiXj6JIbre zD{xkq9ydIHZ~*L2frmTN5VdOv;d0&NO*Px#2p`vy_bKMy3@ zd&-`98rpZ-ZwAa0aGP`2j<7rTENZ`DEBdRS!bafG;n(fu%QuXqCU?~-Y>uZP$nTR# z5#>PT0`4cRe}g^O&C9E!we`#_2Opol?q7~3#?D(A!?LonfQ+rH%GVE_!!Qeh0!sbv z1u>c5-{6Ry{qVsRTsB44e4#McpvC~W7=i1_fS353g7Jdg^(yuFIgQ@2>1i(T) zM9Wrs)pu@nqwHOQ@zdb0+Qhg&A73_jd6a%8KIM$@!Q=0~dQ;N#P>;T81CIXs5WSBx z;A5~RKclU#b`QJ=#JkdX%MeRgRRK)ZM|E)}li5E;BoX@s2)Y=pNMlX##A=4_5@N>AdZj=aAt6vHd38hQWz98Q_k>iI$&Yik#H1D4wqAo3_E z_yK#B%+ABG)ULq$$nOm= zC=)j*F&SRbhC94Y6hI<{)k@EuVVyRc40ks~d0E9VUpPWeUIo+m|ARtM3RmPCP31w8t$c1jht|{M6AeVXq)ct4WQC& z{AP==O9mG{0sw$PYuJ_(ei{iKi4^07UKs0cY&aESk6mmi>(^ppX3BUxC2%z>D4I{) z=;Ud8PcmZGAhs<1`bFyN+op>4`@pvA7IXiZixI+|0+LDe_%Q*s5nFE{58y&mx$)EE zoO5O6&n!Z2sQQc$?d(oRL$37~zFL{A7VrQ99U{-=ov z<$z_A@0XzIVc!ZW84wR;h%Kdgk?>Sr?T?y6FwD7{jy+Mwj$Mn7H}v&ghF*0lg?_&Z zo;E#9xbt$C*I!i{p);5kcQ=PxX+}RMv z0SXewVK&>7*p&c;4#S$mXD+ilRDb5az3L%)c?17Yd`bY0(+adGO9X8$k~&i-&+d=q z$sF%u!BC3^^ueE>*_)=78Q<#LucNOIpCnX(=4I;6fdHIfphh}9*$MJ+v_vjb;k~Z{ zG=pwR-GAn3*_mggFW1bUK>{qVs(MQzK@5|>k89|(o_`3s%)Xw5V0rug{aE47Qb;5q zg985{_I~$+J@*(KwNX(66?WTP3GPOal~A_ykmBPhK&XS=+jgQo4KH{QyU2ue({Im+ z5J(T9$e@Ng)H4!>!|bl&bCPE=#NWib0+eeHPD-ldZ4xrW?XJ$)zsZopbV)(eRh#hL zUtG6{^+|u z`~E9mTWIJvcY1daat=GbVVw7F*sYy>ol~kP;21Tbq;tiLiZIEB;dvgSk<4;84$Cip zWyEhvJDMt-q$A91(dVKKZWO@1gm514K&B|*i^B)XEx|&VEC9C3RPuW}FtB4I%F;j2 z7say-|9STOmxJ-|NAtg(a{uL*rb}n22}B3@3@~ZlHu?-D30?(s5}3y!h6GBa6wh=M za#r8nBLvSL)6@GgKR>^)@VAyA#w_ebMvWe>AKOPu7{0Y9rLEGEhG5$Qf`W4K9|qV- zP0{dRJ*mWDvZcbl=2E|JAEOEj?KCvPP!y^hU{GtKqb69w#aD>}Y9#j#ONT76fk5WM zy>Ecwf%K&)@DW291*e53CM9j8rFQDW2g$UZ;m;WY{{@(%N$k|aPW3(5U!bN&k;5G7 nB?z{ZbB=Omnr~Y3nE

C;Xb#+(YbN1P{>)d_zT5Inx1vv>sI9#}Q@7^Iweiv1G_YR5~@`nBd1NlYN*Mk@G z@zGgWQsoom<@3oT^xeC!?<7TqR6H_H*4=$%@7}sE7hCSk&gpMIp#mf#wHkm{QEH{S zJQk%Zohr$iKr8<;k@E07tLh}|1~jROj}4L}wIH)KM?ZhEgs&~cu|aoppc} z=XLK%PyYqg?L1PIpfAFoh=ab|aXIhNeGUsFBO}vL*%%mrR##W2c~l*~)?g4DtfjmE z!W)xCOd){t4q06yZLvahD#?{vd7OVLMl*#viGmqcMEIH)6JqBtW5clwoWG7?fn(KN z|5_)JfDD*Hcll>6;ZL!6as7X-3K<=aE@|}dD6w!d=D(d04kZzb!~W;GXe^0jY@|1(Xo;5#bmKhvm`oWRD(Ne*DTq{TM$f9CJR*VHhfqM?O_g;D06O+$O{xQXu53%#PLJ3-> z;ahT9K)_#r5Wah7eLuQOyo%l0x=&(e@ny!}uJqT@Q&7ODs>1Uid?m#kO>Qi)$)aTb zRVYcrX~(J0f0FxV$w&FmUA>!cd)0PG&`F%UJ~R1i1Y>%2)}S-$-)oWthL>TU#I2Ii zOvR_H$|{N3j#Q;D&3VZEHOS~xnpy}#Kda{6Mif%USjnwS0qHE&Ws)=Fkq#@db4h=9Y?lM zD)Ihq(>a63l}29MkdSfD683-yuKWBX= zhJ%&PW(0fj>-%wIp*y&_<|^$<(cNEC3JZ&TM&$2jjKER`IEmq(oHCcwTz95skL|0E z?M|Kd^9jd)`R1jC>(xOhrr7Lqh>Ah`*s~0q?CS1LNlg4cN?b%si^sCma%p*4!0gYm zN56Bc7Z`WF)fE&Y*l579={*-671c8~2Ji0f{`zQ*wb}?BpOFz+Tud32ZJnSV3m+>O zDWTD}1!vmklH2;ctY%YAsrfo(FOTsQl%aQqvOV)5ISQ65f|gxJwz zOa8moAE-Ps z_Y~@5ZXy|D3k9{_Je&*}TiFEr+^x>!5o%LgF^M9jX)#Um#G+f~mZ*l-*3xrkvMuc% z%*>qjc~o#1&!cr7m#8W#qlt_XH&GUl&;np~jKpUqSsV!~yecH$Q`AOQQc^m1X{l{& z6fYio9U1QH1IaBXpuZS5>g-H$zo!ZpZY3s&A90!bz+}I%=ySL3Q>D`)EM*19?{)JP z5?|_;Ghi#3g?ETR0N08xjW{G-D?`(zfi_bYs&IPSVmbp8TEVrGOlqLRrwBP+!^IoI zd8)l1nbw|K?OoBd?pFTFTy<8cjGQGuiaXdjko4}^k46$2-s0d=7^m_J17QZrD2NEh z*1ku9nz9HjFbE1aSHcMs`~Ce4=ObLHL3SZgmGI=&u7=}ROgAlDMC{~|h39-R+35ss zgs=5PO<7~9^fH4Y*>i=Rf`xpUfV#%s$x1TdYUjjV2pY>OpRCR@S85e_blcEAsv#48 z!}NpY+I|RcCDdN}Q|g@>Z&|WS&%z4QofFt|H(ypqYKnmdtyrKk(7PIlb1t#dRNtsr zQ^pb?f^735wRLiY>`@p?q=&?@#Pxu>WJV=uGHBUaA6nI^g>LKUMb{CG1GVL>yG#Qg2bjiRYk zWMG?+YgxBc){5wOX?~uuZ)NT(c4sdc8?MsZ zCdWVQPZv|(rs$`ErZ^k^d=&`1HsqXHP_wmk?^^?v$7u*7wVH^cUc8O zY$<%l+URMw4UhXHBJ5FLjRkSl=>wa}`4`k0xt?IAeY$O4TQ4_fCIGq5OnI!Xqh1E~ z#u4%Ao@f3=1lXyve< zz?U5=6X04}bJc5l1NJ7dRCs#ooSt059Srwp7f+9>3C$#$-s=Z@$CT4gS?gCD_Z6Mu zFLOriR|v^+Pd-k(#rfukV}J+kcDi#xI-%{oXNR^IY6v=;Y{5-k+{t~7!lA`kR$D0; zs)gbWRtN{1*(vGF1;xc^7#L#A`b6y=9br*X#O&;e2PGCoK1?|FjMYAPXv!sV*d2N? z;)+b^6A3h`;qjF6Kg*0*@Xj^g#l^?B_`UFi=NXt5ic~YUOCtQ}F=%ECJ6x{A;`hGY zPE^+`e7K$h?d;qy4aJj*^WligRp}0ObrN_2&ejRu=00cfWL2(=syIpSt^^QaKkFds zJsy#eTd8yl9Fp*MVvoOZP`o#*VkGLmX7nE&6&Xo5=KA4hD~;;z08Qj---BnJrzq7i zp5w+qH5gB)f6EE;t)nG?JRQVzOf%zc;A}JLD;<;clG<^G5IwPJ_UxTW?dAzp=hbBL zC;SuLX??=Q6Rgio404nM9IahV5a9bzo{bmC54IG=J*D+^1 zc-!Lj`v)Zs)|R*83T~^Sg`6;%RHn!cdRhanH%88hZ!3lZzPX5fd-l6p0J+E%ri4AW z;(OcChh_7ln=qF?F)-efo# zk1Oq9?7HLeTB;nbAUKiaY_tB1GQnSGdB}{v8!~kVTF>P? zeP+AMN`7Uea66o8*Ts?qSBZ>yE!g02X6i0eH2u(=+9jn7V}lBZk)VVf8xqy@yk0A5 zX;&YDn@2~gTI~hvbPYi^yQEz0#F*RZo^LQZ`uTC!swy4b2ljdRmWcOH!QgC}xtN}= zQ%=&$ z#Wc^JJ+>&p=X9Vm=Cys(jLNuUMss-Mpa+Ld_V*lysF!@}>6J)*c5}F< z3hzBu+N4%*!hQyed%|DE{EZJ;xDcg@LNZYe|7Jh@;pp1-OVQXPwGvj913hw;9~d5l+?5-Gh+xWSIlR zdZU2LsQ2QACg|o!QF?3o>H&7YN!?fV*%!vlX3fz8f&%Gzv59g(Y8MJ+jhRCOu%}}V zL6Sfhnbu>S%F&L|3qtZ4zKS<+pv9O1pdCZ1r}aF3;Cfg7xJ*k>Fy1LQU|u zS;RXE=81B3)Ad)f6;%SMrAyZ4wzl}ZMZsBS`rFWb7wyCyG_ne~_jTb`YCRb(f{;%b zpiNQ5h|JOq_#Q7(2VpZjN`g83jvzjSl2~+uDN}c9MLDZJdxmMuMvv#i(R?={bY%!0 zE;RZR?tHlwBCfek2K$e&n;RDsz3fabgbv{1Yr@nqGR=DyWlu_*ek1t z?1$%GsiZSp;qJK113>Xa_lC-RnuhlSF2y%~jwp(OvrK3)=Pf!c8I9llR71xU6_i>M{o`8tX;Ns<*gId`xM+@x_wz^*?{m z<#rEX!!`ItiWm};1AYUW>*j?I-jRzVBR!vQGQWJW<`h=~sGFCWcy6)`_Nxqtl=p4q zBLLI8#((HVQB^@Z@92-?3Iv?Lag8SSyNtVUkP(bXHqoV9iK@r$<38jF^~{W+c|$AB z&JC9;Zu_at+bl=E;Y!?OXmSMKnN^tB-y_}>wdF}lQY@Fm{eG}7aI6#orM*q00sd3XZ4wK8GPN)!2xJ=di}ue({CcEwJvt0Rv$5jV^r!1g$Q0jfHUr0SUm97qa za-vd-ms$buu#%I41vmOG-UQ zY!(lF1nUK|nmCxsR%L``niyMd=W!czO3O)2HWml8@?IqULbTn8-MIdpbM*m zv0Gs`?cJ7Sd#m23G7F4W@?qXVV=?%LJQ+nxk3zI$OZ zseYKeU2sbkcfvL*{X=lqV(pd|cdL7;+a@B3vCOzm+WK7aVxAS$`IQx~@UuR)D5M)1Lyc^VTC&fH z%#~6(%TFk}6n5msCgDG&N0gc^{)lgHp71A)!n?8xu8NZuwGEkTL0>r$aW>Mg^{rK9 zI_Px;P)9kZZDXwxnX#5Fi#|@^J$T6p^Pygob5!t$ORt8p0sJFhEJiPncy;ej{@9Ds;eH?6!74R{ zGXyQ{?TkvhY6@nFvy)hB%5nHug#UD|iAG0r`5_dh@fX%LxzsDypo9 zpOcqfj&Y-L9?q?kUSUO!pJQlU@st~Mm^0>Hx0K@G!dbJ<7M9pOJY>z{2()Rpp*KCW<-bFXHvHBH?50IrD;*@A0Owl7joDyw#|ib`GV&T(#q zzT>+W-DLGuO#5rO^<5bx?#akTA^hXnlwC6qbS`8h0SP}w8j{D z9(Un7KdAU@XU~FZn{@DgLwFYxC0_#aYSXrbv3c+7QZGBJC!ZDY+d(7}+x^D3n_sLt zk+LGH6;vC3Fq`eVmjTAvL0_i6z44-Vlp$KyWkb5U~UDsiG0HeGvz`uGYPaRF|n|)Xt0p;%tj{h zx!n`du&P1(U_SP-hW?=;SYBRUd;$VMZEfouvq)zDD}&7MM0eAF>~DpIgpdYUntS5k|t3YOE3 za^IKD_ePYrtH|!hBi(oI^fSlR-1Ma(FHsyzrR7a#v0ovhm{|EH3J||Zy`&{-s(rue zrSwt^#bIz8@F#4RXa}P{=dJg4eFG ztIJ#NKgZeZWRcrXzIw$2-LseCWY&@iho6thJZ{3@x21EnUGnZtzQks#U;B%;6sZXA zjh{-qY*Rn8U(0Sh(0%MeqGzGNX`U$JK;wYEK5Zi?IrB)gjlilgxu#CI!f=?ccqra)ISvjVI2Ys;BwMaI4!}TU+}Pd>RoE0dH1^6F)QVO}bI2 zG7^_SrI^8-zcFOOX2_B1=;Dy0zuCF8vZBRNBA6^0iHb#FID-jc?MsPN7>AsmL}I4v zEsa&2&+KK67i@D}Qdziocp%}DgM$O|#mL@%Di{7(QYtJz_077Vpx}2xf=e-&oSQo} zH1rdX`_b=X;}2B0i2g4PA-AWif(|cx6X`?`8I8|?#IyA_XB#8rQ8wG?R}&`eIYrsS z5(=oYkxy%2x$diKAs9$(HX#C&=36Hp}>iZ2vvt&emEj0;tlh6b)!_tGsL`s|o zlxeRhlWS%zxRQ*7so??wD4FH(P1oD~vVkSDhZ0rV{8Q@tjm-50H(*rZo$QHp&bghI z;JcSBLpg!x12h<<1Bp_l!ZS~P5|TLkuD2}ecuIbgG1xf^wv&yv>|n6fuEwpwcZ)6D zrR=!0QDC{cq@-l77x*Yeh5|z9y}OuI8Jg4Eoh_8UGqI(V&k`-3GaWwxm*`g0EuT5U z+?JauO=gQDUWjzK1+4lipzof^>EAt*gB*f~xQpuhL!S*umW$cZ1ned8iZi4m^2mV- zS!YB(;)~*O-F(~>a+5w;8?a@wxnY85k7yOz%|DOJqFFk`FQh(88U%fLI9D9>lh0zi zoNRQbRT~dMrg&joe4<5HrrYHFu_!zuDOqo`%%Z;P;q47o194h{P%uv;rJwKURaI4t zJw;Io(ITGJTuV8@yNz&-UtIh>u)>HV_4+O&XqcV$Lu(AIfzsR0DAAp4Ni;Z~pXM_t zolB1Av{?Q}KNscY_YPRXBEL*E4`uw~oH82zCCtvdoYrv6<$1(ry;zki8ioqlY9EM* zh#;q?ql-)bAVt30v;ouaq$F{O;2@qKtKD>?)8>{a1og=8)Si^!xf9W@DaM|rIE!Er zr+jYZ*g|XBa;-FIZB`eaXF*8oh)N=e-ZGUW5XXvT7nj9uGXQs2n@Dx0@QI>_3XU0$ zILN~aBbZ2v5uKhn=23iUl@}GwIewv1+t}3fmt1k*!UATcT1Z(L1Zx?xq<)u{7O7s0 zkBj>`V?jnujk0Of`S95NRasfw~RDJm+?gPZnmZ|fN! z%B4|B`$4sg;)UHt&;(+);lhy+g(geN&aNtD!VFz(1l*j`o;PCwVYZrETD00_b8*pF z-EKY=O+9CXh7Rl4?jLQBHZ$kL?WUyU@H9Z!G4iTjd;}PgNxa{ePcUb8c93Z!DgOvz zvGzJTIuCcfVx!5lfq9A^SAui7Mrsq!-mfH8Rd$&6swG-RW$NXFv-*S`P!5JRVecK< zGJo&P8ow|^{VS`Hz`lR~K10Aa?V|1t6$?xMD>KCY_R&#rSC`-=C%yIf@HJTx6Px62 zF!_YKJc#@A-&)msoG=;1_VfUU`To?v&);>Rf0Z zjs9Ri`T#{~24B=Z+27ijs3MslG~Is`JF+m8f3z%g>i?5M;QzZ>T_M=7e=i)K_1~)C zfr2RU-quzCBq}D-s!z_nNd2oPI+4@SA>_}5W@i)m`T0Q;L{sfl>9&b`cz9${u>9+6 zs15}9l2K8W^IISBr$+`i?D zFgiPQe1D0e&4UR#?b_ykTP{0!JVf-nlxgGd?2bF*rj*MVzV6%={-ijMF5}MPOy^(y z5@l<6Sptpv10sSdavgxZwzs!GAs~pV0`Zf(yT2J38_yhrp$S?c#J&mq$#?v zGadGYqrg+hPR1j z{*)s%Dme;yQ?z4bW4Yrq&1qz2Am{s2aln<%!c)i>WJ>;)pf54xyGBg#W-^9Hy-=dX zyj$6(zK;-s;8Y+z;cQNWr5qhAS;j=T>1^q2bzsR#?e3h(lM=)OHjaJzV#aX(;~L-@ zNwApeHyNVISt8TO&RBj&V7{;tC^|r~w@VEdTP;)CbYG;dt3dDXBqJ?N%Eg6kZocmm zDZ@t{9gXzyqe{=*9P6snUOy9uA8R7A89m6OU1}NYO!tkfy6JA3$Y9cY)1hU6sS{uG zbNX0%D!(TUKkU7O&#}VjMiVJ3R;PEA9NN^Z&*%q%D%`2Zn4#%n*83noaWq(IWV$TV zzqSQoChpYUh&>DAExB2R%@FdsnK`vXM5S}rp2bMh!||%Sg#i@X_1Tc2ybnLuv0#(p%iuXsjN+tiO|RpIf719x)052a6Gbj5_){*&qJ{wmX>|nCvGYqAu3svuo`)-n!wD*70QQXupuQ z(qwPc=xPD%PprN@-;D&&v!A!X;c(Sb2Gzp_T59U0KcvCSYQyI0HpaZ5m>nf}d8`|_ z=zL(;#c#>Vki~)_hQrAx&JWg6RUOkZX30LYsi*(|CLbZ%R#`#8Fus%p6}Jb$eu#xa zq(mwo`PdPqPshaQ=;K*Z7J42D+H^wscg@omNRYY%oabR67P0NRY3D6eL(`ASZtwF; zFx}YwY8a5foTB-w=pc#tK(aY8OH;nW;PlVW4h$+O4zOtv6^Y%CnCd&-b<}zftZU&8 z6rlueX-_a_Z0u($fq~1G;|fMCgaaD#?la z?D2pn?v|IRx<)|Hpm@6MfoLSr_2jt#MIX+`bZw+Z)|mxupI>w-OZ5 zQahD9z{|N<6ldoKoFj^2K>!&uiNs%9i!M(em~6!FPYH4yOJazyuPJh$0+VSqu$y+kufm% zV|>0Zsb^0#J`8L)zY^|>y7zyD)pxy#voj>w{Hv^@Ce(IJ{?i8k&&W|eM8d>V=cNzR zUZ7(re^Jp(ee#?zyx1A4jZP2q${l6=3+lZ{uRlDBW>m2)0^~H|9|!RCvrnv{d0`&r zn5mBEKM14SLEtqR8yk9ExY+uL3VOX3J-v40xZ~Gmk6^y7C>QQ?EqOo1(g9uy6H(JB=!!@Ws67Q zLH4^bE~lO7k3}dR9`nNTUT4I$&s+7GomZFr zwr&e@!cLHodQ>`55PeP6JT7klO-)0P$`=)7@rh=uNwfDNUNj603`?up3b9|a2%vpB zWVvrXufy6Fj_zw~B*=b%%AsEV9c+q#zt)&uFuo|W-&FVVlRzlS`s#=0d{DD|4DctT0{W*)v4FfCbRe%TF^6Dz+>`YE$2}fnT zBwScLB+DC1Rq;D#0Scxc3?$!@G!n&x_UKS0)qLH;ms9o6%yqrV;PFbxmxyt?IbdcO zOz-iDBCk&9rFS4(jelS6)y|!V^GM!@bH9w+_;!g#`Xsw=u^iBTmsji}goa6-BJp7$ ze~kt;T_KO6*P?ol1xKT(Z$uaNd`^R-bY0gocrG zma7PX_AkaFsT41o^%l4j_}71;zD#{2XW;40Bh>vbEK`*=K*&q=cW$HTVY|_u3PB`T zF4%dW05_~?{;b7^gREDmrAz;o68%?(5~_1~aY1$PMHD4|F&pRKq9xHP2ove>e1G{9 zNdz5bD28YV2xOPPqjrAzClk^s)oEeb+S&>V3WA73%F|n+!sC5?P=6Iysg@-aKPCQK z(6o~_9!Cim=R%40|0gf1i~l=S3)O*#hbLubRxHxMT)w)v=pGm#pE9q5D5HNnvm>RU zfi0eoA}A>M{QL~ZgReNO+vXN^adE*XYx1AesU+}W{sX-G&eGv{dk0cLly|(^V4M|KC zQ1kblh8g5EVP_hPV5NFKIwC8NQ@fpoF_|zL1b#9%YdZ6BTf6wt@qUCCXR(wBK7NJC_5s(=qYm49 z(6wkYCi%CU1TkEeUNxnF0HG?ob~}Xbkw%S&9S<|C$eu|K273XR_`scDXwMGScvQ*R z1mo#h5qxeZGGlJ8bT>C-J~RfOJKRhP!knE|c#XPeWvxN_9QpmQ6F-evQb>?kavU8V zkv}p3fBu0XG#686ZzEPkPx!6)@|YVh6`^X^;{Hl>Z}LbwhyS-%Xf;gK-qZRJ7gp-W zNet|(<4vELmFSxYycC{WQp{J+Nb9D0u7?EjoYyaV40v(`1mAeDIOR;oi=~%6PvBst zaNC(|F!dvCelDJ>Ft}n=^U$=zQ8D;x;5CHGaUe(NlbiJ!9J#?4?Usr7Z1zBjb@Y^V zx*lS2gl4u0=0u^p9R9vfNQgE+%r@~mA`$cmt%z1PUK>_3nVY)$#-qsDN@MH8UD6WQd$oZ&96|({QoS7lmBO;!}V(@iooiSP8 z;v-aC@}+*TxIr!MT%lq6qK0h51fhVgv0EHCdept2ez#>{X@ftn%Lhq~5~c%b8M= zb{kTwTc+z>zpR)(EfEt)fvH4F)Q_o@P1Xwk1Aaw`zA35((?-I<0Yw7!MaDJl%Zp)O?sFofX98x#W**$V1 ztZk$=!5)`5I`9_`o}0N;+OT53!%ufFD(thjS@X&ItV)~5o@D~slTJ98PSslZglUF% z{;jsW9EC(5`-W^yYRbn%vBu4zX9EtMJUw9rU92DR;u73heqM%oZlqVjNfJ7`qA%1T zW@7RoT;9>=tkr$x3B6B~e+*2;YT~RM6)l}p6hXQe9~I*iEc^)`8xdz6Rzsab7f0gEP+&o*7xoZT-K1GsiklIUX>1k`7b)LVY!knjay z=feRl^QU|GNW&>iZEa&y%4I`JD7m9yePztB1@F~3xzjLqc$^>jRmL!GNa3R> zmabVkGfSX(iVViCa+Ma zGViJ$RUOTre2ee}oGT1ozyBP^@Iy=0_1WbHLB#|54fdI%U}gg;6IWX2Cmd4J0(J@6 zSw{b{e!7RX@h8Mid!g*bH;Jb}XN$9pYk>?crD=JMu$c-*jN8gNCEhIqgw#alhKZk|030VLUMV7aKX=Y)xc9;i_BwjTLOhySax`n=S*1>Hf4~c@KDL^l?7* zR;T%@Et&pNOBr39@#u-J+KVZ8s>JfGYJ9_)o9z4RKPb_JFX^nDPDiU%`Fq{DY^>F$ zqIb-6&(6qgoQx)u;b?2iO3FXgH_oLG(dG_ck0j_;ngextIRG{r?lx^%u^*RI%4IQb zFZ|>E48|sHD_$MRL*kscT&@TOe4ez&1NWH=HN{$eU!m7g_pfh5yuUZ@_|{ks=e(+N zu|Mf{hIwT5bi7P!VOMV-gcWB7^p*P)lL_ysV*N^ls@%kp0*}27{ch?q1l=Nzdpm#H z)TtbpcGogME+9&gu|?MB85a!s41I2QKGQXCSOg}TnDVzV2UomDcw>*kJVz{YBR+md z+g~@lu|Wt?%M@gB*saRA)1eU#OCh6(P+?sTk5!y#v1pAFR!1y%=pZY)d5Btq=Yq@L~2 z$ND_hFD6lvM5oPBRacTx7@4eAe$KBi}Y-?>ttnt6(lk=8#e0RRyb^R-US6Tj|b^ zq*s?T367LLy%jGw(H3;j6xp^{u;*!h~8nx9Ayhelj^a%EQek5MbdMVoEgVY>F5 zwktQA`jM1CXs9gpa!hx%N>{bA4s*{2p}Vl7Q3u~auQ^q4G9kI@;t!Og#)5HM8P0iK zR{J_#v}7vB85B%Je|~IKsh1ot?j&b0-R)8s z5X9=Z4Qic#X(8v8^A{JfO#TZEUa3F;l%is6|G9vdSty;A!B|R==6BU9xnJ``|GcE8#q?T!xK#;DA!;G zgyTV1{D?2;hp(=_$hThnA2~stM!iY~(l7&Izx(?k{V-g4U#=mj69`O%G+pc-`jP+3 z^0!dbO@{rvQ9(Gt`o~K?-v7W&;s1iy|Laa5vwuNx3nnI}xRFtw41lh7_iyT6I%~>| z^55>D;Y0|zA1O`>A?@!U9xx!z{BCnQ#f4yR&$QwHGz&SAGc(8L$>OFWZjaQI)CIw-2^c=_F6b>lcsF zH~b5Be0`Wt;BTOknQ}{2g(M*CKM9syjAc!M^z>i2wOz0cf@GsV9LW_y(63n|ev}Y> z%DXe;aBi`SmGulxOc$~r+*!VY6MxeGNT%gVSJ0s;d12M6;OkKuV-h=bn2(S$MMgN_&VjONdO?Go#fJN9Yt+&usMUH>y$ zEHrOebY}rD?|z-L8C;=HLaN^6(;Wfyb*y*pZxUA*c)s>(+3(yD>%Hc_8(TFjCQ#_KcOYj*7GMqI0BK;0RznOl-~JYu%UA9y@F$pH~DjiE501m z>%{;QHEITi)Zrkzb-;R41OP}ljmZ4}h5f&rXD-~>PSBql^T!|~GxM6c z);vBxAL{*k)r*^(@x}~p=E$f`ncYCmF)qStrQ2WLi-IG^r+sTYY*AR=3>33+BWE(r z7wgt8X&y;?%9t09K#v7~zF>jO9?BcmG%mj0fC1KG2nr*m6f6w{Qod9BC@VPG3BYJ+ z^mvy!SiNs!!qS|Szcq%I%3IDDaQp3uxD%Hx?cALRq+^20rEV}>?SCz1%{$9zXK=I> zqjA5_T(v&Cy`!A;<547~&4$a><=vsq8doB7HiPLtfEJt2`l7c+(}%hCvtCnIVQIMs zP;pE3nN<|1S@B*yPo zqm-)yt}CU|In6GB_etn&zB+y696o$;BZ79j^RkEnKs*JK(N7!P9U(N#creOM+D!E7 z1lvzYgj7d}YT!!gt{S0}wU_s?zUsN(*0{9UCSvz2kPe+}#q9ZluUB30D;lu|@veC2 zwI&eY*?I%MHcYqr%oUFboF`ArY!;!Odvf zYE?S?MHKU#M18tk_F@+E2@l-6+izJcqH=IdVKduzgVh!Im_IO~pNw{=^LonN7%BXC ze<%^E^}k)SqKS9*bSr_EI2qpAYGJRcor-Z@NT_w`z#Q*<#4lgX5wG5_#9V(^2eTJF zUr;x!MLoB|1!W`RAAhx{2-+0=TMz|FiEbHvVAzrRG z+B@T}Z-Np#7Pmakm?Vzs%XTXO>CnPVKBpeOH;eWYOjE3G4*w%N|HMuOPLDxD9Azia*RyyvW0TB8_6 zD-0Bl1A-Q^$IY#?hFG1@vJr@+u$(_fzg>J#!lG2r@jTpTbx}nhSMS^#syL1+UTzb_ zyfdpT^=%XGZbYg~YrX=$c4RO&7Vhz@6GKVd2K`nuWTGFlX4QAjwLJb>KXwZc?3Kw> zLaxS({0|g`@B70+495tOkJR-3m5-=w|CWzR-Q@Yg|5x%+N6aCgAc9YP;3-yJo4u#s z6L?GPzxAVzYr0&pwNQjp{JrNC3POi=EQmD4V-LdsHQxZF8qi&)dj7e$N=jR=D()fq z5h!_YObvBQKoa(j<;d^<6zaFKWHp`T`-c}R0cAkroV z$>=^?E>w}bx_-MJ6+JH*mB71oD7Qz(N8`K(-kFy^zY6)`sKe9#*i&t3@gp{-^TJ!V zD?eE++t*s2g;NeLc zT?PJ6f*QwIPs>H00)_5rUl|6ygK}OiBLRj`!jZUZc3YMz+s3^YZNHdkEKFQ~C_Y9l zGcyc>eOAxC<=7IQTa-tJBU4fCf;91I?&6Y4bP&EsXC?QHnZ^*_Gg^An8ggF z8n-!FE2L97a$6eOm<*DaBW9lFP1PxKsV6D)o}cECc0`VTYD;81XOyRkq^CUN%d;Ps1=Db87@2H zLfaD8Fe4Osau$NgZ^n917G-cc8<5b#E2x4`wU__nS>v2_pmCS-<2hmMLksYKrZ3Rn zeEa{m=!+%h;p}K3&0@?i!X~cQd$PhJBFyC)^O-VYLgm2OTGKIn*zAZ8R}dQOO)+fs z2L70Y(eUVGqIX<KYkej^Ohy(@bAcc>Ip954rS+-rjpeSh7;(ey*x#KpU3g~b)-)w8) zh6 zErR4iwFm`9IMSWGZkSS?vfawH5iT9{Ni@Mr9?))kQ9>GzNyU*b1W1|Tg%(phe9MKb z{EVBkO{lvB+e_eUzcUW6)RWEMBC$0fORE8I zdYBo<8DUHeJu%@Ig%!-EcQ(|@JwzNtpu9jL#B3+Yhn~f(%Vid(Sb2?RScN(@4)UhW3BHT^7z{>3GQ3%M~moMhiCU^r$1Cf(F9v3vdUpU$T3F&tufAVI&5@)pzioHCp29 zHH2Q^IoZ2kvbnJxJPGVF_{+u!urAlw^0Rt@4Iu4??VC-6USD0`KGC#z&bcgjB3NA5 zyr7(8a*sE=!`A-Ux2;H9`sF8Bpm3glrV>XTDZKLu(iB==PK$zKu86IOi-;2+O6W@! z6+B;$PBNJ>Fg7L~m%>MXx1wFCZ20`)!>9Y>t%&4Im`Z1qXP>nhoNB=RDM|CkUp1#l z2C9jAFUP;@n~8KOE6rA?Hc@6h`G&&Xck>t5cPCAH>00K=9dZu$oe#WS-x;2>8MWIQ z(%hv=qyt`31n+QPghQsSHf)s|+_=0k3w(}+HTAUSlTHTW8b*BhY>-5>d$9%bx1!AJA8~^%(ssZ0(VDd+sXULztdf!W0`fqVV|rf^7Zi4Hotd=E0>;n|x>%vUXEX1zRvwDNu~mvXm2L@x;cSKhZph3f!;28xgj#|Hs}7H<^{u- zYE6;Cuvo`W=>&Q7&e)NhM`llfNBhxedUn5pS4QqbXt%Ks1U`!NmFZJLz;K4iBvcfU zLH^w}LK};w=k)8+=yM8Mc((Deifhrnxx<&W5S0l$)q6E5$#->eqv=J~O5nH*lU%@6 zd#1INnCuH46%>1$W?Sdt_63vD8_eIh-RMB=?k_eo7@CrzVHNQ8Dk+u6SbOr&fSvI0 z$#i^8()H+coH^bq7J}kukG=NiY($#t=RWl53`{>5?YvJH;|6JaH7At61;(f(M&t3w zPUMm{%zORI)t&yOPD@-<2%w1kQgmnI%*N1U`QL zpR0Om218?GFr=gf*c~Zpe+ZEV0Re%ijLg8o0-BD_#wx7HuI)m(*3M`WBm2LhOf@k+ zzR%|7<|?efE>d}UxnDp4z$C5u??-A7B8iW*6gLA1{ihtpLnt38JoG|$rh?W_nA_$7 zpPEbSkvt-jpph+3c0(M=9w;jlJJLWAbgK-#f%KIJrZOo=u~VQXJ?INszdtI&>w0An zPFwz)>AhJtF{V+Svgx$V1b^h!?vAAV$R!;#fB2CCe+@Oy@z)J_;o@Ki=BM`3CAK*AOutSt{c4pgF$DLM^hvTSYTr0Fcq~CE^Xa&pV-9oTv zN>6y8#hkXotHm(W^9Ncq-2;@{kF*lRsq<}!@JXv72+;to0@u3FH+7@u8`~~MU`lCJ&S2el0L_q0OcG%>OkcLJY z*vxZ23Yh-Wdm%Jgi{^r}lR-+#7$hmRtmHQMP8bNM4?L3nA9x~RRD(Iur-#kq9cyhc&S z6}){)bCw*d+ENiz8sWPBz*&@H{m{`vnCAGA>c_KGgUi7#kINqE@m}@K?Q5u>_j|%~ zaMPM~lHTj=?M0?+G)6sjdR-yHc2n-hA%qoE`g6NeEpU zcX!TDAQC{CEM174a3KM9v&YbFJ*_R#Er+9eKkC$Z3HuIZ0P0?M;iO%0;iDs0lU*i_ z?o7j!v$ViXM6Al{i!H-7jgmocbCJXK41dkhVk+e3*pNNjvg^s~TMWhxE#uaODOFk7 z87+cKShh-oETILQ$jo}d!;U9;%&FJ9s@d18dQC~a+K0Or9~8XL*+RbUKj74Sg1Wx#%pyL&S~^jwj)J&vg1&=;j|ssZHWM! z(Qn68DVp=I2cms$)ZKoJfE8L41#Udi!)$1ZRYe9-rKpGYt|>g4xKK8`;~E>1o`*&{ zR9y9)6*1L=hRyqD&#U!Sbd?>Vjk+{Yl<}aCVlf-Zxw=EV&bu(00KOtYe))c+8kX=- z?&;}&6Ap$9+eGJ9`ZiUcj1N1%mz~6 zIqL)?6TTpepkQjga@n$4Y-Og+eVyikt`Es8I-xLkJ^L$D$5}{MoiqpqwLQZCH+bw;1_S+7mG@Q?_*L>R0sB>9&&=-CR3_Ytj={H4W~Y4tGJJ ze*G-HxCBw( z7}~Askjs{$e>q~Gp)uH4>5+xx=olS^H{H;feI$FCknYkud?h#n8)<(C2OgDIS&yzX zn*G@PhIogZH!z*ixK<@0`~_1aIF?2LgVP?aD^7UFc-m0|`pKao_r=E%8KXMI2Sv(= zjwE7^=Ec1K)nJH|QiZW8X@-yx(e#;d1Nd8*KV@LsPi$o%zId*PgVHryE>!iQSVb0l z{f1*iIip~Ml!u6vC@TL0lj^}qWmq#`tX)>>f2NgGU&Q4}5!@1e$D{sd5FlL%# zenPT8{Cjn8ujq_F6Kaw-04uxE7OAzB8&91wv#;g{_|ZO0Hg3jz-WehWMnr$fS8cdG z3b%?+Y{mAP5e)n+(TN=xLCX)2Ao0hCwYBH^K{LLxQgzS3gAU1I{rfHPovVDvFBlE6 zI)vp20fbJ6`6IW19E#jJ9XVj9*fh`w!6LD+u59Qll9j+GFUSTStm*a zVX_Z+!lkyR-%M0VSy(dq`a82u;K&19*M63U&xvkl`D#up8}5!okdg}%Fs_jZo{95% z$L?GXrNUkc@NRb1KnWy&JTGAt?PyLiYWG}L$M)R*T_rz_SWsGfLn_(&w<17`>tXa z1r`=IA37=X3=W=~A|(B%+ta@HdV?t7C>hRl+7;xZf|rpSBivy@Ab`X6oDxGDLxDgH5dymxMDe?JIb*)Q+* z@DrK>C)}^g>lm+(j#Ru%e#5j&DXSy1RTMc0lU%oD!)7V)P2%LS{XwWR&WP%OEn6lS zWg4NWrUyn(chK!nj0rU~h;CtbC>bd#*6ip<^OG@jsa+|5iAVvE9|_d+8<|z##5gK=6fNc zMA+Cl5Rs8@EaFVKFYkV4SPOF3l5KR34Vo~eue8txdw0wZl|6X7bC;9r zHRrm#8$MG?bfBR=7xuMkDP;15hUR!S(vWAANx{O$TC3wB> zd5PDKqVI8^ENldg?9Ss?HYqpOO7K2JEL3Y*#F|OaJCiwF(g2gM3MJma;e;mMAlW2O zOaBkRzz96p4<}>#loF(k)lb3Gikrg;OL_-Hq3Af#5l0QL1Qc{QF6W0XiiUIFOncFc zfV)whRN$)Np+-8xoim#QMSM2*F;{&ca%VgPa~qx~MHv`#a9)+ch!x@rbKgfg&Bgxu zkowPXC7nHZDrIe?eJ3$NiBln!JN^f3?Zj3jJmq6QK}=AwFGdIARe>ndWwn6 zX_8q$q9fno<%tv$=nwxGY-p-oeAd0IrNN#&{lkpo>mG#0eebq%3CWwx%|mISA|?9T z{mP=#)VDl&^{+l_8a?!}E(w_HOA>l#I8Ui8=a%_5v!_%iCkcBFbRpgQ4bS`rz&9Vd z1Rh14v%yKQt=~Me{Jgimf7veIOOMUZgIXy!AX9JrH?k z$%tgAB_@UesCK*%9kxcx_5O86^N&B1Br*7Wpy*IzUj7V znYm}@KwSr5$BNn(dnqE{hbDL91&P8TS5A3Fd}!M%Dg-;_N-l$;m7xmBdeKY#94ya9 zJY@gG)m-~I8`d(Gwx ztQD0}qxoU1SiH7hwznCcPeNV=3&*y0cApGC0kXlyOqLhDBkoGoM7BQjV@nM+cerfJLM@!I6&n+~Yr#+w?8*&Ry66`kjX{Lf3C zKH`d7n`Tit!Vb3ub`;Nhw*@m(|mPe#vh zf^-H?SvU@R>Dii;?0qz%($yL%-tT|;S1ldsJUW7b@y63gfA|T4)553PQ0Yh5#b;HG z#C)1Ey!_;ZOGP*HwkwJi`LIO?A?|}QqI0Zrie_Y4DWkOa<0f|m3B``5yx5X3=_R!C z>}9Ftvh1+oT;L@C_L>npR$S1~=c_@)bJMtC%&4gpvQAZ+3i4b+9dv(BKXn#@HbDrG zL>(QI_!lcUa{KRCL3DAd?&fYHg`%2l#%TemoItq*6u!0%0s?}dkPspkR)Q%89Gvs& z8qk-xe@7!k;%^OPv0N71=?}B#{1O$B=sw=N#^71d4zdM)y}UN`hJn(MaE_+a%RU+K zDLhzXKp7s>2{VI6M?nq|9lzK6+qZ8YK7OPkcQ2*$9QROdtA0jFaZCgBazJd}O&S7jzy^QRAN*&f&)b+^vyE)Yw zS=6CYy7AIq*AiIU+uM(v!VB`m%m7BAqk6e$pMLtp(mtG@|BE9eu(Pw?*<*5o{%`>T4PXTk z1T5@dyl3m^NE{g144)_e8*m8L4sDM*HS)L@%&!0a7gM#qM$;lf8etR%) zRjXop)8W^1oMq@2`r3!3l!)_i*`_O|eg(TuRQ$W(U<9mi*~uYTX$yeP0O7Umg&`aW znxnw7G*X{1u*&V1p4)so-COwNgxmgg;fNL;t>y^`QkII)pS#)rd0?;lzn3+P>lrK> zw9pF98}vuR6KI^hHpRIj2_tZL^G+4Pe&(_d6Q&t=4cPA)mvf%w>W>Z5UrUrZSp3uq zE1g}BLv)HOK2v{vdBE$dv-||#VUOr3kUTlG4`JSnI%daYG`Ef%F@OS+%FzeL-XA3NJyylEA<6iP0fqk{li0 z+88AhA4?%{y??g(irbz((=g#xhucRvcP<5BTEh@p=#l=s`F4e$-@zK$)DmIO!+?M- zF})K&C~5v(;wa-766Xfv+M9j6u0@S#AmOYaB1D-__6}@d=xkKhymc z)8lu78Gf^!%gvs+|7|uwe~Fr_rT3Z$FYM8hq|iWhL7|eyFL${4I9bw*qjwgm>(RFL z0bq4ZrriQU*zsl0@*O-5tFg+G!B=?Hs3*#tXazNMw1x4}R9;_`NzL*(z?ZrfP=X&JvUp;70+*BqD8rk%7>h%OP8zH( z>Jwe%j*Ix!klg~XRd%`>hY-7`&^bvL*VmXd(IPWTQ+mN%P4?;vbuO7c^6hGs(?^YK z2$-}EWA$}{Wk~C^Nze&k&Kg?~)_!+@-TA2%$lD%Vl)*a-NuR2}p8|v7Q3IOA$Re-g zou1e8Jo5;H(-#q%lMH)yg3)b0X7t^HJ@v5X=cQ=GZnNdr@Z{14$jbcZQ5wC4kHnrx*dx8BKH5=FyyKBI+vc_4})JsyN{Y5y?NJbqe%)dd5v{ z=#)}ktgDLtnfIHc^87(5BTgOy|H}v>5~0-p6nZ?EtSIi!EsslX+*2CQV99SqY3pE$ z-227IS99JmaM``+qD;|>=k`V~X}mwNELg;);<>^(FF z8!LTm-xMvQ>st8Q*NZx7x|!-rW9&k>8Yxp@UyHaJygX;_A-B1TztJ z;uCIR3Tq$(kFxd}F#`i`mqV@+uF|Qs-BFaaZGVDMs`;|1-rs+Gz@J z+R11WuYpu3ysKYK4k7RsWkNHenfKdGAr)Bq>WK@52+4*8BJ~ z-M#iPoNFupSD{MF`z9U3>fY=)h~Nrm4sS>JUcfKeM`CxzyP3?cz{|_1sDK=0s8Ta} zHvpA7J9lddXt=+_4}=`~q}B{IOQdafIjB6_?7o{czj;>|Y$@M32{{LvH^~icA2$7d z`+3lxl~d{63P>Yig$8e!?u>uS5OMTmnb$czCpOE{?L%`1$T#KmqeXb5egw=#-wA?D zQL(?(Y#MjGe$)QF0{|A6D^N~%-hsg%tV~(r_32iMh5(g+mrz}%wQUoCCHJycM`y#x zfEiZ6njR5xZFX`&u^G7Rokm88+}!c1t%!Vy3B0n}9fc%KVx(m%bA^UNYNPw-xK|SF zky%?S5RvjHMoM%b>4&VNBI@n9Ho|4RJ?nZ&R1CKK<}B*$!?AGr7WkA%YOU12%?#-? zpqW9Uvl;KBNTok?t8biT030a1d0#?WbYTu|>Q#$f^mb|(Hw6DtE|P8b>%xgkS8SY! z63cu3aWmc|5y_yTVkt#F?wCqzUUt4`B?6mk$xJFfj(nr|tY&9B;6%g2lSfiw$29>myEBOO0h-h=9lSiUmDeL#+ADK zSttR%=sJ~dN1Uz3Y0N%whTJ(IAh2CViLKobS=PUB@Gfn5q7Yu=FwMb44K`iB>KhRN zs@=zYlEfVb+JbmVR!Dkz!h$;f>Z4Qfcri~}23c#DnF~;g4;cI22MH4HB)naoX7rwa zDu`Mv3JDH_y3DJ}X^!u1jgsl7Kt)=QvsG?w3T1Rl-dYooRdEkE(crDjH6|Wjpb!>Z zlx=U)N*IR;G33rr00=<#80C9kQoAGK*vzjJU%lwERA`PsRq?spEz(f6-w3KSjQp*{wsYZ_V}!(e=v~^kMvJ2=L8clYitR5BE?@tRWaSfd z&U=LOOK*(=mix&Ry-f zU>PfMo%@5^G+b&Wd9eF7Pxn*AcV8*A_Vg>TzA~R#lkwL(ED>+5iPj0X8RH|h;c)FP z)->_#=ZK6EWo6~nHur?WLDAJ!ExsH+Uoc#EpwUrVt2my@9Z-_OxxT*Mgw+VYoxvzv z?Rm$VdgGaJuS3b-zwBxrjM6sn%XCK7e#k!?R?Y(QbfWQ92r`NL*Ar-M?in86+|{Yt z@ePM%N#mC6bO@FNDZRZtHYk*at?e8gfA?GFuyGBB>>Tk3-|jVFLZMi(DD=Hh6!re7 z^V0y2JY!(G){~m zunq|J|BtZfl>}X1@!{lNspqS;p>>{8d_7IE&3CCevthnlV4?g8J~QC;SFt1?xS^$K z-bvewQW_s`Q6e~Pk^F21mT*Z!$s-vW7Nw!#dEO_DG1-D?Dd1=F5;~2HilZY<@l5ls z2y3oLaav4EA;(+#@c6R)VJcJ+1I$2?C%KxElfr50q_Pr?L4^a0)o?|Zh}qIo$1$@e zl!D`}+n#dk&*JxS_}j4xR>h-h_EL{yU_!`jApF#9u2?CF=k8>w3@8%VaVp~X)`)W! z^HAp~w1eF7%%kQPNgM|h?Hh8%t*ckG4%aIZvS`V{wco|u!>FKWK@|dnC-1kbZGpta zIbHW8D&?<7LB&W(#!iLlRVOZ~PGQ&o5Gb`I&p6hsStLHTs<;6nQsLJC$uXFLe z&(Db__kAIND0>p<{e9x(#RDv(u?!A)uSl~lC8>COC))4QjuS~fG5W=_hJ2p&8R>#B zO*G_xsT%tKnHxE_T!yyp`Zrp~{V&)jn(B)gHKDA8i>)@u+!`Ij63jX|`Ul#rZf~8< z6J~n-wDkrAVV)ooxv3U46Z$&8X-Z)0*w`3#g8=-aq;v)&?SVseYLvq( z6Tm9}qLx)u|4=w6lYq^j`snnJnKA+dDFVPMHp7wlu~S(=9vdJUa^S!LCTU1U-U{+# zxc^6d^!WU2zdwr6-P6k7H6T@b~7PD9g9Eweu~$(c0C?r=(y3Suwj8E9ajMj(R~s!N~OV@QMoB z_V)HBobbUN;O}TZP=c{ziu%K#3?%sZKXr9=HR1dkBn39nLBhdJOmP?j?|^{cz;6&W ziJzpJvS8v*u?J8V5noT0j&}=w4|@*RfEk!D{K~-5NJz z(Ee&aE|E*fSD|g|g}Oqq1VBPCS8vim13af5ig8hKV2I<9=;ighCJ`k@~~Mdi0(LeBqeOba&Er zeG^Vf(UmX_E!J@0*ti4kBTk0v+TMY427SJaIhCi(wIlb6_v3@0cC3V0Xw=AtbJk;| zV`@k%$T-iei#fV35+}mV!o4>Ksd0zYUYGj=_wuI-yRbuMD~60&+CWq~VM}9=4WH4M z$Io)6cP`+ydfmM93h!_c>{`TbBt`)Rnb)0H99hTTF4&a_a z%{Pe*2@{xT{Lr~?H@N=Vyd#w}oQLNiXz{Hw_dzL-uieag(a81dhAw4*&-rjgG9RHk zU3|!C6O24IG0Vp-18>{7L*324hJ>f>U5$N~FI$B)TTNMa)LUx&&fMbFAziHSHH_ye zcnQROD_^dc#>B^&)_i~CDXQM-1K7A^q1a>Puth7IuiJb->2@Wpy6@fVvcl#d+{?u^ z-}r#KeB%>mXc}iyeO_tJTTJfTc&>^$!!LxBl_&bVvafp$vPZLT$=mjDRlAnwlAOW) zydAk#owC?Qng*UKGE5jUSB7gJgk5R3Ck(B((VYXE1b$uK{sKvDDa&y=g$;dSh;4Ya zcW=%dL`50MM`%k#mCo!*|ItqM{6amf_6fy^QMZHk)R=n9 z0?_r@gb~j5AJF)@25u(OBO&RuY zSq|-kHBQE0j2(GURcDU8`>(Mxegsy^Z7+?^$;fmEYWcV}Uyz}L2(mNv*lk}(PYAmp z{qs_UP^+n*wQC&=+43AIziRbws-7gZcziL>iZmlcbNvbqkrrKE;ZyDlrtI#-IpS(J z_bYS|uMWXa%2v}R_&Wgh1?WzaPl1!tAcZzYkk-vSQ$#k;y-s(lR3Zo4ON!NP19{Xl z=nO|vZYYmwN@=YbCW4}`F|Qi1JCgwL`TCo$ufrt$w|G(++>QslZcyjX%Ql{C=udF& z0didkk`*P4JKU{!A`YM=R4F2cT3iGBFnZtLy&meZ`Yx;kcA`9Dmq30^mN}c6y-lZN ztF>{@^WCIy2j4OaPf?aaYVeBc%1>Nsn!iz-uMczie5@q9!VDYs=ti#i{|%p--rE3sB=|QX&;!K4-CF*@2vgmPfq;-F<&3-COaGiWz zc$&{ipD3VOw2!TYoePX;fu^x)d~qb;kZ-TXD}4`BFf~Z(mHY=lc8;bJG1H?q2FXKR zI}Ja~qTSr}_AM>7q9O1H2K;T?Bw4YpDYp;r^u+ZakXezNf4Ni%Y}Y`CYoWp%F^*hY z9Hppu6|h1gCEP| z9aXLK_a`sZX{ie*qFGRt|VRz@zCp14;}Z%FQB)&xSF^A-y4t3hNz zUb75mH;_j)v@{SbAZKSQ-NhuF&bdOetki1Ygmw-@%2bYJ`w-1Q*9>}sy$&!P3oP}~ zm}c{OGfGrB^}xZ;Zu1WmXqb6NDJp60c{F6|WLl<<1))Z)TgS8O$}Sn|%0)DS^T$4+ zbK8TooYiL~r4e`?yE&C4JY@TDnz|1#*gEU-!;bFxUd4Ztq6iSWurJ%A;(fuGGfVbt zB!HVVsb<4RWMv>eaV`!I{(kgosy#W>_*YbB=@;jT9}a@Bx#`!D&%=?W=f` z?D%n3i5+UXi@RB18-fs&TXDN4#%ZN>Qgso#ehP9Q#TEmgVofNoAX0CTWBp*!g@SF# z{#CkpX&^B@{<29$Jjt1P|4k#irdrsTTCEA5;2B67YgVo3mXxmq5p$xcZK!P0Y0VN0 zM2pk6>o&G#Xh|=u*aJwxH8c2}^nt;(RZc$D3`oIQ^zl?y&G+Ef4Gx|t);)Q|nEpK} zByQc<=d2Aj3HnRi2*niDtRCY zN3aH*cK0-rHC$j=P|u6nL&P*2T7IOj&x~AyTw`X}r;1PMs=0lLDTSTmvm(og9XvJY zm-?VQ+POJ%-C?yiOa&N~xuYr8h)h;s=1!zjrE-`WD%L%+&IHu0C~~~GJ8nnke#$k* zFFMcYViI-Pt1uv62}x;#Z>~kyaWh5_;dyUBaJfq8axpI2-F$CKQcU+(5^#z>pt?A$ z4EEhi&K2h=X1I=qxdcSAdO$F${_qE(dE!LZ(&@fF-1*{t`G*^F)EEcksS|C$X{~i& z#?*mlAeyJaru|k_1@G9|*mhmg<}0N47yXC6w~o~Hm?vGy>9{dT_dY)GtMA2V6cqRN zNV=DRX7o82J?6s?bH%Q;LJW|;HED|Jefh7wAv@Z*sgF(G_mB-L9p5*aU`tbYV=8G^ zhCMdMbAGVvPr3!b+P2 zJ!5{78OVCJR$i6l;|A=RVoT4JPAF62I<^dqrrf{TS#3=JrvZMB&KCr`r~z{SX|^f7 zCu#m;F0SzWuHif~4m9t4>xysv4?n<;Kk+S^$ZY+cAqs250G2Hl*+Aa#ga1#c8OCxk z*p70=o{2;0Qh=^#JM?KuX6#fc(LNuh{r)Q6Z*@D$QaU{RGbpH`n(* zalL-`g7KofBL$@s>CDk>s>W+N@WYV00`U%Mxs^Re!M*YcFi{kvrgxPZA+syh6y2Nq z?v=PPP03*70*8sx@Ik&&aS&?kcJyA(c4euS7NsISj?AM&!SYZ$T^Ypa(ybx>rgSoz z7H|saIdr6-Di}hu;BK>FIN6_VVk&&1#fjR2+rPu^T&GZ2m+6S;R{-Q)oC_$R}Tf(mFxVn zyZk6mQXobf#BVgVqmlfAlW)}ZkqLTyoqN`5t+}tu7o2figfSZZ4}z@GP6VFXzeft` zUat(7?!BM{pY$6kyY4u~-<$OK^*XE7u!nr!hqMFT?;ubCT1UTwNAtgfN3mz}kTO~f z#@0MMnAV%iT~Zb_njb8MUzqN84tDhsuneZ1sYmiMBfrXur%SwX+93}`7CM>S62c6} z3NNuK*W)sDCPe+j{%iHf-s=hUhjzul&=6$o$5^oHmtD1gg~4K0fmQjJFNtQ9VGF9O zX0J>vIbEYZeIL z9s{Vh_KJx?gku+dFNyR-ON6dljRTI&=MA;KZ=ErgGo0JtO+hLrpYsr@{(ExTp!XzDrP_gfOnlc!&&Tgi^qBW#wlrR2poNwCA8odOLD%Gq`=f0z4Em z=B*GMyy+q%AAr-G5-_L-^fm;0)m~mD3?=nFhe0v&d}?QrfAhhFgan}f83-5@)G%%J z-cl_JCEufn(kN)SoU08=ZF7Z_01(8MtR&>=2TZNbqCH)Qj~TRB6ESe5@+wr=)SEN6 zdmqG6fdo{ZyO1d{z3*QJwTdEPdG?e-a)Ecl$x}BL57~{Mfu^&Q0cij<5H|$|2NSWd z(9AI4;+{3OxNn}Hztai>c|?=>OuiJ8)hb&<5By_b6F_pctn_>q9K&@8+I|a&0PNKN zvVN8{WPZ1S2LjIfo?I1r)Y=QJ!uH>AMsd9t>s3YfcZz6PP#81vS!BF*m@sjeN#CO$ zOTArn{_0*4#J$qf3YtPK7|Hf+b-F)f+@(qlzVwLy2=K%#i1$G8ihU?BqQE?XLK))9 ziMt1`&h>+bv;7awPe@$v_b`#7E0ef3LLQc_^eI2+%8Oc4Rw zva!pqK_&i`2I{@uKR=%U@)HO+H~_F6^*2)K`K{$RIrUFW(25-YsVy}~S&ft1%||oW z?f46$z;FBi>0|l-wEpw^b?HoHCY&&D`vFhO0R%}HUo;;!zW+L>m;mUs3qUq`kpf>l zgTt<51p9o=wBI0R0UF{mJLasA&Nu4VU8T})0`#nSHC*U7JX(p4jt&I2_mh!9iX>?NK^{A+*n`O__0EI8;4_v4yp6k#zMk+r}WP(n4Q7b zaKOEy{&NB*+VfcMSlnk`j)y#isj6bd{_w7{^74qla#Auwre_2+G-D^k7&}|gHD7Ep zW*a=y2n#X5kqn`0TPo{a{hdpKx9?Pk=0K#7){t=m!j<|5T&H?A)H30LBKM}(FP7%O z?P9H}1{_KMk4ig=5cs3O+k%nNJEd<)8#~fs^xR(!Bm=+qjLHg2YYqpo30;yzSjlj+ zq_Zwd?rW#mQl+c<@uI=$n;f$&o~#XJRx8UWyBLG}rZ^jAnZvcK&La=(FrqUxf~MiX z)!en|Enyyxk5mPrDV!0}QzXL(a4?w7L5}MLk$UA_xe*4E;o@FSjwcd+>-7!U>LKMp87UcG)#o$)fBE@8&#fw;4LO`KMaywV_Z zH1n3aYzk1ls&5#I(dzpz9T{&dS$YfT)2_h2XEs;*WTG<4IPfz4`)@Fe8(4#@YXj_f zNf6=0hnGDF`OKBXdl7S?7!MfN4XFhE#dL8g_qr#3Z>=YB2toLI_?|qe{rmcWM%0vF zmRqbzDD9E;xXA_JiBA&|QxAZISs3)-p3soe^P?b=9kKROtWL6Xp_ir;snjoWC#iRk zgaFI^!9Gs5&$9(9i*!7e@`4Onm#qJpqzs)k#X3c*Db(xEcoAQNv-Tjk%(4FRMd43s zc7v$nSgd+o0o8;!7^$Av$WA4e)n!4td!F{1M;vzo-XgGEN0OGp4lq>G@YSP!C3N8r z^yd2`;xKb1dEE1M58md1JlG_qI~&vHuNQ`wx*9xdu>^SbRdstVGPlsqIex>DDaS69 zSiG^kNt=OxCEP<{^-djal|aF$vZ7dfshuq4U@`vWVt<7l?u090Si||jm7V1>dl;DR zi(+(lvZ+6iMV%-P5x8QH_k2R|bvw^qx`-M@m;F4})b|DEF_MPZ%*IBLor)Rub3Jxv z0yWf*VajL|p2!q!F85iSG<}m1{Hsxfo@dy5jq`@B(%d`9fDS-=vzqq#r+a2*BY$5M zmdN;Qsib}W{sOfryB!m@sgY^Zu$@5>?^s&0c)2Wo%q$dkQcT_tXtn$F4ZLqnKSHv< zY)lO7aN#;mxZ!aD>?fXU&J@8W%m>nS$vs~7C25>rlBFtk(>uR&jddri(9zT*&7i%A zYOOGqdO0I|HXo7QuY5UDBSCEUgdSM)eQ?v(_(EhiAd6u+jOz4$0gd|FVbnuuSk>%^ z^Nh(c+c@?G*2AA#+xOvOMNLukEzCM!a$GRW|BnlhZMh=o^|t5v$)fXNg2!voJ%q)+ z0}zG@OQciRIl&yjKYBD(A*ayHOsnXgvea^CU*pY+{gK&=*F9Wq`XJ1RExLQk6z70$ zBKoZ+p0Or4=z?~DVSjF6^1V6YD~2&yHJP-7npbdn@Xy0-fX8bmSksyWoDo*O1is&k zzSwJ#l+Eai_DLd~r<5 zP+Q11)F<7?d-7K}v@CR-(9ppgfc+IMZ~N5;r5Z|(TS^@?Qwyg%iwvnwB4uS=E>Rz; zPswM$s@d+=WUUoanJG(pc%t;oE8M8!K3iW1<};7TcKFJhH)=ggOkR z`Jj`L%hv>pOD5Fj5$!T<5H%I43SE!omyct^CWxFOlM~CuLTIB@T36Gy)0B* zOc`Frtks z!{1$ZJ{i+&2usP}6BoeF{pcD?@^g?7=QzHat`Pt(zECSBVc+5XUC`2BiVgN{%Ali+ z{#T(Yor~+LQdLo3&kPPTOECJ3=SoRKP?rDmC0-rfvV!SS&n31P0LWmo|ng)r6gyZy4 z_rx4K8>T5OuFo_L!mxmT4!Jplxpo<)if{`KI22g(HhyG9@d4PGL|2c z!0+}zAFd;_Uy(Vy7(TQt(wAt|dn}Jk0#upz#du?`+hV;;(QsM=YFJUe`9A-^W4{-+ z5}A`X9h7embxF%C-%B!{{n7u?cts;oGkstOU#sYxB})Rt$YOQ0s?l$tjSJt8E`1*2hqi_5Suc7BiFdGrhW{*`4oBaa5qzeBcT9MFsWN%j;uiaK%dn_}doQe_( z6&A3;RLEg!qmF-SQ&m3U)QcUP{u-)UeYqgt>zd z7YbboMQLir^o=K8NM0kdh1RwjB6_Dt9+o_10#>cS4_cg0xZY16 zfzQMOaJv>HG;~j{p0YCo3TPQifLGQ2q}!(9k2E>@mxW4QA{GwVz8>g^=M5BO&M)cV zg#i`t`ah$Sj?a60ZL}^sM&_M5@X%;wKLZu<{|3lJu4hf^Ko4XhR4_kyh-hnk_S9qa@? zQni}PkG-J|yh~<|t}UD&3kdm2?SDk3OJaaBz^{%vZZp^us{0jYGFylchRH4SqXB6d zxQ$DeC5*5I&W7=tt-r$;lE+x!TaNlY(UiJ7+;F@zV7VA_lv6ueTR?}XAPXo$%fVY% zS^~EKWnwa#(JCpxLIdTRoP}fHipsIu#ii=P;p^M9rh0FUAP3o9{AY{ZKHbn{g2pfq$OAZxVe+N|R|?1F4`Z5RcTU7y90I zd(!h*bwSr7!gTbtuy|hU85EBF)emV_i<~(ZG&z|^4dtwx!=Hnt-0!NK2RPzA+LLDbLp`z6{EOZRh5qt6@$_OkeXWNh{I^}xnhcm!K06r|$p$SK~Pbs;5W`5F1ZKra8T z?=G!GV4UhOc;aEuI&>z3{Pm#LhFzO{RINAI0s#%3sG{6l7~U$h%d{N62%r%O!ZQsw zaz-~eBUfo%7Kv*SQ5yx5dN%2rSCE2ra(?+>I~NP`>{V%$LbcEA`6?@~Kx-4W2R8^WTthb~rvuQ-!3SPS#U z_mb%EsrxK@^5$$%GTV;p*9K=%hB#N~9VNmg-3)))rWU`@d=|4v-OTjI(adKH6>?#DaF@RIC#8_tOh#>6f$(jhx*x&szS+!0((f2OqJwAtk$r@t*LkQ>c4@XN&p(6FQ_M|u*TE8MWzqQ;s3b?jfY zd32;ci62Nn(s7N3-C}(kI+uD=5yGQ^2AKRS+0|A4BqQF+FCZ|(%z5qff zdKDEFpix(-*<{y_dygUdui0JDYa{|=&d)`$&-?FMQ}}GvKN19lT<$+X95VTTf;j(O z32RcC>!SvNeTi+i)e*D$93f!gR_v_dGG5tS%t4P4U)hmjQ^FP=By-h@TI)c{l zpti}>l6ZLu@oar{KgSVQRKyiZL{Wa!)DB3^S z|5he8zzKgV6Wf=fz;uxyj|5;koZO*&%fBs*TbsG%YF$Q!Zog9Gs)+e>-Cf_>IM@Kg z?Ey2ZpZQTt5Dhtb5YRILu1AqoVqWN*n1}$DpaO^cbjff05Vjey_Q&W4fNyzK0irF& zQfGT5Zj7n4;LKUsA=LFW>r6N=%mdbYHVn5E9ixY0-!MchPPhuMy3yy;#D?4_#mo0e!l{{C9@Yv4yP;uHE+NjI+q&#bD;+F5^E0+g3CS5jdcmO zIM0ey3BY)fo*qH58G}1;9bGJN%gbeNl9gcPUd}pG z;fk+e)DT?hM4g9{{-dsq`!B1h%jK8b(u{isU>CV5uZ02&iU7$twQE{95}bnBpgc=? zlJNI#c{j)8L7$W82#Z2iYa0cHbk_FgIQ@ZeLn@K|i=SgL+!ZcPD8NlD%;AS| zKFt3&=H4kN#b)|&MqR#ec(#V4{ys{OlUh> zD%O}YG~E+QQmTF}x?i&Ma27H9=G!kx-~KVsOcoYgir2>V<1UK_Y(b8tx2!W}GhLK( zwd;oACA+NYjK0QYt=dB!y@MBKQ>??*@-O2pqE^p79I0TH$>g_|>=hReE-oyF^a@*L zEKkbIh)rP#tZbFUaEeTZJ$oa@7Kk0Kk}C%Nr520}(+twX7>nHN@F)} z-lcT(lVM}XKeg6p)zF~2BZb*V;wmwffkk`~J#qSFB5rux4Pw~e3uz!Q6Azvd{`sZ2 zaYTB6J%R1ajpOCnN+H?C^!mE8${V~XMrG{4yJavX4&P3DJ}T&Fl2xL~XXxLP7lX#+ zX?qI?ouUQ?+vQD89kEs|kw-I2XzTJQCkT1@bLd%eo8dxtbG^{aoF=aQe7@39=`C&> zYs>(4o52JHD1dQ3TFJ-4|xwX2nrT5BC{w@09D8jK!MU4WTjhR8KU!pFfr_+sPPz zMhAhhbXr}hDD8b<2DIprFg?XjyYv?Bj4m=O)Jr=uEcZ4r>}}<7&uJS21zNQ=q9?_wJ#i73G*%URSUk=UB%f+?q;ebH8U z0x&#!s?2?wB-WE3Gip&C&3+(^$K?=I8Hra)a(2)EfnHXmfL^XicgH5H430uaS)WC8dF5-12pGB>zJINBnsUP}(YuEHGlj{cN%#?e_Sq{{ zOz{I%oe=o7RSc+PvFjeBijw&N#Ug{WACNPS9zt#3G#f-vOzKS{CbHcR*-dl3%Y$`Y z`J~dYe%NVo?}S|znz4O7^;VVpIQga=tD1~r-_9&_$0yC~o-*9vl9E<^ez?vGN+sMp z>}Ux0R&N=1G7YnR@kxL5)um}6*3xnk;vufHG-@qGPXN*m(-gM^^w5Rc4VPT21OmQZ+u9|307$;F0N&ZUzyoEFBk^GWQ&WO zOKFqzgNDc7ro~b10IulxKv2BkPHR+lUn>Ou%-K|syWG3Id70SL8%x;mKoXo}2A%9O z?O$u@A?mtfA83o{8Q)DW)`Dr^yoHgb&Gnk=^GhyiA3J|njj5>TRz$gl**2Uz!j)|a z7YJ2G#yvujuI`_Y7G0)i%mGcXT?9jJ@{0W(q|jvOj4%xk3{iyddH4Bpm6n7?5XsQ= zYocZTBzCeS`$AMXoT{L{15?oSS8R2Khu%<}ZAL*A?q_m!QID>8@innx&4k=CBTpTc1mxOE=FrXG;2llZYJMJ0JkT?0R$q+-^Sj9M@@c}8p>eVv&-KNnYN;Y_ui za5%ReR3h&@CT3}*Q=eDBudhQo*wM@r4KAN;BumA(G(~>gAm+thsBVlS*ei&%esH1l zgpluc5Szv!?`9y-NXVuE27*bk=+1oZGg`3H1uoZ$hn!vDEQSelRb|GN4ING@pLQt&Ld@rVyKC=E zT#*YlD0cC=??0t`@y2v&>HDJyj)yB#qr8n*h$kgRCYH-?!xk=Ze9GLqkW^l&!iN`u zx~W(o9bUADo``#YC15hfPH1g)6shJ^O07SX)c9q_D}9NgYWI2i{Z%?{F#ENx8AlAN z3&cjD^TjSxjB2_2VD=mwnZA2ksqO+jqp4;fs+~!h?rNQSXI*o*BpN2o4@tCPH|7lU zCS4;NefENZ+5$dC`lrdpyXGq!R#Z!+9!puy+skM8wMAzz%ELaVHJJ;$Vo$zAlP4{=uFJi+rDF`rBzepIHlWs zcqO?z?uGG@+4eJ4`{DcWzWOYov_YQxZAX$&4tIs_mw3|PJ z?D#2(D(wV&>6w3+BAwNrB)CtStso-6wn+!!rOIYQ{W34!KTq~dMB82McDs$vyYI%^ zJ85*sh}*GB{jvITD5Rmz%ZupgaV7K0(Dv3~;oQixk#7QFey!}wKCaL$=OZhZljD2p z*ojwUsno{7Hbl0HUBPG?=HXLbwVxlhi4iaF#`>kw`l+30JRf=Ef$)f5qM=U1C2ch* z_V-s=^AF`ppi+V!_=N183j++Wtd`$Jc-vFbmn7ztWo8yO3RDOhZJFxAdDL~0>fZ%& zQZ-N@%cw|8LghYmr~AVEU@{3B0UPlqdJ2Ct>i$vel$GEquI48k{2@%2QWiCw&9SdY z;44iw@SAP2&SJ%KBvjM@k*<XzW8jw1N~pk&VY!Wddw zO6k&qBd05<$TSt(@y58Y4~RxKGsji22g05uMF4PKCu8Z;IA5vDi@U5^ow8J1Flzcs zlBF4|^I(fnt8uVMQFP-FjsKMLXBCyN5F?TCl<=KrbJS|u%KJ$};4>Z1jWizChiZqBChmf*`6_Hw6u6rj4|>*In|#@Ou0pl_NevB2K1 zMIRWMp8BrQ4ZqGzZ+xaf9&wxKvBlsOk<61pGi%N)OKgd^T!KS@)BIsF5NRwSOC49k zZp6v2Mqa4uaG>?a;YY~?LHqBjZEzEkPSVvTeb344*bAf2Q%H#=s;4SFVHiB0A2l~n zEK$rmo-VEiItI0qWt#8WhFJ(z$M-$m3m;C(m?m8OMw3yW)O*UY+?5Jlh*NB&3DLn;$kop&{L|WD9B%Azgp)nahfBG*i znwP4R)Czc)lR|1}G{@bVTUI^{!P}ufQ1)TBX&v}b>9tPlF?=aD*>=*QM1k_`$-h3Z zIiAp{IpB!Lh6e})ePZ#W^Y@7^F~yz`=eNCz&RO~&H837g3@H!{FOR8rce6Pf0F;v) z{T@wUA?`71lx81da>$;Ygn6U~z9{qi*ve>Sza6`#pc){PB;z|F5hdI}+D--Liv4mj2nK2A6Su`c+YaSZU`rG5fT)<#Qr=eqkbEI?f5OO4wl3bG6yi_cl8XqDD93W<*) zupzZnv)?j`ez1FCDqgDR|HeIJcmoSBum|rZ36LI(Pe1?z2e&m}Yx)v`-U%=OfMrj% z%~t=d3m}hsIU^o_df#Ap_9v8Q@5kSV28fM+n1n{*|78S{E&QMP2R(;B-9HS0M*%Pe zd_Zm~tBt+A03hPOH*@&Qi-aRMDCiR>C$^=fWphgl?C;#tR=1?y-d?wVlwoioHU_$FLF8YV$9kZLnIPL z6S}u9=~lPOOb+Lo0#B6pTJnR1vU}~&-oDk@W}n+h!HaK75J_=wb2#P*Rd%T%44eBU zLjD8GaxM2<_1-Jbf}M``!EB^^&vM!67Cd@SPZN?M<~>6uTEkdcX zc>5CO0<^yhdkJYnO1(q4xa4SNvpKHDUK+jAHvAY5y zg5cob??ruwKnRZXi$XFh%Ekg4>=Elb!_|A^>yo|}H+jo|^Q}x`IlIcP>8_M~-?r_W zQY?vljjaS$P8iaSpzt&(c@6ZAMtWy=)~{RZ$E?j`zt9j|v6+VT3(v>(>ePF)vgtJe zOrS52&z{s>44ZLsATz4X0SVzAif~&fz|#|j`^S}Ia>bM_ZCe#Ayi*vW8saV{rd!Li zPzpTUNCV>PkF89pH`uPU{Rci7lp05FfFPRb0onsAvg$Uxigmii44Sq5P+TY|xD9k- z*uA;%`A`~4?z}n^u(bqc#e8#ebaBdFGzhwumO^pl@-NC5Yxaq7=sL>O;n=xi)~N3? zWlJpwwv?JvN*?Ho=~?aa&aO``>5A9%u7g_hBuz+9L!m#oq)v?17o%tPSU?Oz`BYoJ zCc$R6tL85DZ}?_=80zTH-^9SvJ6`Zo)u4A9^2Cj)&km>dP3$d?{0vs-BfI{>lP=>-ZZMCxKhK zCCQ!`XiVg>SmYg86)(mvpNwqyQlV75k#TJ1=^HLcyqVz=j0Bdah;SQUm|6xUmUG`verN z^Aw&$dMJU&0q>Dxoz?2n*F&tO5bc$xzO4hnL(!@H4OQD)Gox1$Z@s>*j}bM~8^9k# zrtU=eGG9J2EJ{7QzHYfz8K0dl2d94I^q1w1%nXB+DnGNomw8vK%JcBt;v+uKZI;Xt z=9q9qM2B;cS2X6>PGwMU-FvV^@V0j0F!`Dl2qvuX1X=ALc;(}$ag2vKpZj98=EBuo zo0TIqdBBT~aQAFd+G0p4!(#92V($$M*v4Cxj4+t&NPTtxyPW@GFdXed|2jxOp1zT5 zn<}T7bZyJKI%XnAtp<2wlH;#aV=|hgcA);hV`~EbH@3z^;{xt5W}G8b@vMl-X%!6-OHwyRs5VC1(+9yZ;|FS^u@n$NBh|E zp3CogE{xzQlX~yVuYYaSKQ?dWxUaIzNIVR-&ti&c)}ni+u&1s(l5G^U4z7-SR%F$q zy)8^loe2IJjOOHV_<0&Kp3l)8w;D=J_XIcidz$FxnWF@u+dXPT6BOvOxei@;Ga`#l zr7Qiv@J}_r?6fgjG|>f(rq;wXm(oL04OhnZ9&SV-03mS(8799yo=4qj82e3$II=a@;5# zH$Sx%GbSxB5>y+>(lA(xz)cXs2P_-N-MX$sh$Zks-}Xe`$)meKf1wv8UMr@{mzRGy-?TazZ{exB*r{5`s0i;>KesRKwD}{-NcG&hR0@kG5On z=WA{H!&l>4)k}6|$rvQ0N{SYO1@Yp?WbgmbUp%Y-GXpgM>Zx)*WUx25y<@d=93X}K z$d0vjkQO+|b2Md9w#Smvt2ngIGvcPKXs+NXSZwj0!Y%|K|#?MDWZs9QNC}8 zFBd$_(Gi}B&-Ux_R5jw@0FCA%BHP0u!(U&l(sci+tr${gU){?3_8x`rEe9{1HvV=` zJx29({Bm*NilX%;A;9}E`WuLa{YTyhbV|65gX(lI_XQbuU@BGcJ-0xJED7p460DEPRRPl zxtTVB+#ZqOZLO1zedXP1lyNR$TbEnz&N+hEOjj7anar+Z{ReVms6QyeUQ?or^*jz=M*0e=-bQ5*Awrd^6lr z)bn;!=9oN9Tnndfl8?@Mw{vl#8kSsLQHijvJku77_ zyA~!hs=@p)E-X4=5W`XVbNr1M@p&~ltSBrECVd$zCpH19lFFtbVkXk#(HFLsb12FJ z&;Gl36l-C&pMgt)){w6t1YXO_^LJ5$d_6 z-}^zD$mmXS_ATB(TF#jBlByr^S`;*|XZGcKn=f{^-bhkKkiz ziRfh%I8GQg>0c^;>lNHiZV&gTiv9fkwPk3F*^@$i6pD#xXlQ15>H$2iWkac1`@_{a zld=0fim6W(##R9tQ!+~o5v1e{RQH$(oQ4xA41X8lx#dD2S;Vdq$__c?*)&ty@NF%a z5u+CO)45}Hv*ec9^~efP*~mvF>9Dr#nVDBemB&~;E+=4pL;!QyFn;1lqgEc&9aH!o z5_yani0}|)Zu8A?FO>~*_$Y+>{Hj-ip=3|!lC751G$D>uS$3(7{Gl~QJz+>3m+^}Rb8pBv71rpnGX=~s>P?cf;$jTrwV9=OKupPoi-(LgAo&509Y+^YDZVnPQL>#C|4 zIYfS>or9#B*l#eUeXSf$!Usx%Z{8Ip@N&I4Kond|d0zYHrUXU6h`8LvIMVvXXFiWutZr(Tkp zit>R~Y4KZfBf`hxTu|FSj9D^U%&E=e+}zyILz$KdgXwb0Kn9~AZHohvXF#A~+32cV z8RiyNPWqs87yVI^xK0fh-_3fC)$)T8Lz+U-54q#snr`*)vW1kmZReOB{*hgM>jV&9FQ20P;s%T(3*|zx@EKe}jqLR>cRZGKd zpL5++%xl}1V%X`f&T>Tp&^y2arLte5>j%^9!!QAmO9@Y^jNqoM|BiJ zwTI=3);eGy{zUxa= z_X$XbBaV}R1BKen?DAj53Uv#)pYN84wN;y6PNK;V#?1H59Yae}44;e+|Xg z%43Om4>vNtg0#F2A7eW^`L}FX0O6%G>@`ionE0ZhaGFLrAng8ILSbOxc_ev-(=2#< z6<#P+@=V7Y)Dy*+IhFdfN+Ov)c-djY$C+}siDL^H zVv4bS@wuzfm1(HF#GZG2jvb9HaJRq#(1mv;%uwNz zGSyr>GPC+D8>wN-_v31E;MF-WF%k9cn^#5#Zc)*8Baq^Yh>I&JBO}8TNaaULEHhlH zo0$O?*NuaJl+FmD`DB2*;67RWKvzKWS|6*j@;tL2R#(YUP~)fH?c2UY^tc=Mlkuzr zBnCbYt%Lh;>-c3&-VY*$YxH~b=J1A(Bo!tb^#)UV$yNXUsBn7Cg^~!%0TTu6&G1t!?FC^QF zY7}gAcEn;<#fF>aBPLeFYNQd{tUBOGeoj~R&zy+upIQagcpB681F23K%c)^&6vV^s z++nY)9jw+5QIMi*S>lSt7nq4@A4?TtU1EDDc%8T_5HzuxzPJ@JrcbHRJ5BF|Auv27 z3=uv5G`)IT{yL-;Ek#FRR60R(hMeEpF-85Hi==t-{S`Higgf@=q;IHDGOeWtO-Dja z`&Igpyw1#s_TKGIVzWLK7`bT{pL+>`yR-8!Qp9DF;1O2(B^(|}V#^*hEr>b&P=Bc_ z(JjTFbKlFvLtY>7MsU44Y#$o~0kPM9q(z5EN8e@g$N_U;Rx|ze{(agoC0)^#u3{tF zTC|j7WF7 zZjN*w%wrmyR!#F;X1wDQ>a3kKL{raP0!;_TY$nYxot)Occ|@sK3)0u_EXZ-!rb;NE zpf<)2ve6{eF=ZO1k?F>HDpK%QCOrKuBd?D3JeaRBJM4VmYqKJ6fvY_xvma-1poxtQ z0fQ;Y9C{Fwj8S-Pq!?&L+RuM9KF5sykKX=I*?o`rKiB~IL3U$79{FDbV1^_GBpAu> zv!O$SZbAjaF?TBb17yIM&#ynheRW}pDOj$*BTHy=4mslbhgiPX<#F7lK2zXU^-yH>KkA_lTb#d};v+k-eHt#wL|+7vbR|s13>xyQYj_Qbh8h9SgC=Ksw(IZ@ zKt=RHvf*J!)(pW9Qy^?#49@q`6W4^}^h`=PCY6_R;( zpmf@ZFwJ_b?d_N{E$4A<`%5DCylvdMscY(>4Ro0+@_8O-c*WXrbdRz@yJk(o1`nI( zGe2T1NW=44Q5jy-e7{36auV3JZHzs(!k@trL4mebR!)by z97AV{K|TsUMZ`XxoTvz2gVF%eE-NjFyUKu{^3ZlxXv}Dobf2KPVo|S0HSK2Q%0w*j z9-CUhp6GGY&Eq_ zcF@_HA?m*Dul3zDxr}*?VT6%7H8Ui)=0(!woG8Vgyff%H^ay+4@<+{qTGw2i`ixLj zTMM);d`$*-L$Z=>ls-{1Vzg*PmX?~A^`PwC(!BIpe zQY<+Byb%t5@(_^8MGr$glQEXIlxrDABf2K6^XdZ$re{UGb1Cvo(TbLWdcV;-`4W4z z1sJPj) z2H&z&b>v4f#!)f3i2?WFVBqX;neEts0R>Om!kFta#J>1KXTSFrtn}+kTvb(7H8r)M zAQt}+7i{~fzqP$BHmK7uoWwT$Efa3hOt8(k%2wa0Jha*!2%)4mPM%17 zgLj<$BG_eY+!QuKzkIX5-Ix}WvZD(XhnH3XnlfrGq=1T)97%i;s=fsvH?)pUwurFs zpu2CAnEnzQ29Dz9r7xjW$W4ygF!2KPcm;hZ}buTP5Wp*I? zFK!Ja1c!wU&hQKVW%Wh^=!8Fg`jq+7FZBxzh0V&!x^)r5Kz^Bk2EaOhI^d%5HfVLCCLK%1j~)Yx#ZLJJdHJr0l(LxVo(UG^~*h<5bFkes)0 z18)!DNeki~OE{!(=D1m3ejIq(T&!mT#;LDUcpFl6mdCDd_92@#(Eg#W&+1k>TX?M-Cili=`sYjmYjU93`mEC z#l-mK#SN%|-rEa*81h|?>NvgQBc&UtVn%yIEuM{9++e{U%S3nT-=OR^Ov}yYrJ5u? zlk{hlXh2v3x?j%Yn;|j^F#C;Y6*8|>Q*O6PeImr6qtO0DwvLTMT2qJP57j34$Qt_r z_Pt!og%-D~@by=8WxBWuKi=s~CttDPX#Qm1U>oZJk*2JWrnriSCX3eD0)pbd`e*57 zA^_J(2;{;6UYQ|4a^=-WB5Z@>U4Jzr+OQEH@|>oMG}f4ciIxm+Qq$P*X^AXYC6Y```f;k;Y~T8H(R#Y&nN!`K{%lqtWAv+c%7#$8}=cKhAQs zuwwA3jKh4?4DU{9v^l-8+uvjyuX(?WaM*#?L-jtxB;2re`d#`Z>joIybIXT+!@C(X z#GTdKOikPF5ctbHTZc*MRt4K0&|ePx3VTHw(0qih%!td16+4$11diJz>8pXoISllWg@A`nj_q$ zG1`(K=-j2O03(I8?k!6&t<6@?*t@+3q=hlnS-L#L;amF|lmCif<0}3qf|cAsU006# zCj{FnRcbm>Rf&9#@c66nf@!j`U{Yjbwp~hw9Go)&KCxOwp?gPFjx^7Sigl|HR`qIxDC#S}oZb3xdMoZUArm?XZ>32jli*d!X zm>kw}lD0(M;0=Vddhm-Q;tC+0yos!dmuL`DH9#YHR#6+Ah`VAisG{mpj^m-ZB?bAn z`10BPJ1`8!hlfWV@S(S!AFq7JCAl(qz4%KEdbrRl^F_W}k6Tio{c_#*11_bV))eK* z|A;!PZXb7mG5mxm$wf_qC_Xs9G6d*D3RYKx#*N&9A_cy~AyDO=QH@M35*{D~M?wcuxPQ}R23mV}twrv|-MP<1e zsgfsL?DR7O8&sXhS$`@4Z_3BIgw5=*LITlnu9=HS9nd+jr4cmoy1|L)XI6rP*BaNo z6Y#EKK8TYW*ptXgkDAniLqcwIIK4dMQoG@D>@UTy??D-DDO>L3G3rfK|D^OY{Y&ZB z>(C|?_1A-4Z9~pcip@OgX_~uDM-WZLs1vsc8HsvGv~}inD_@xi1f}^nJh>&7GNK{Z`O)KcQ;4L}YcSYQg5>q=&T?qaC58wFmjU@VKgXqyUer ze><$jGOa~anKT4M4FqQ_a4#tt#A7$fN#h)lF3m% zUsii6q4mnDjk%%cNIgBZ`qdG6E}0D@mY{rTVx0Qu1KkSo-$&jY23EMp8q$hC0n^Y5 zc!@f!G3)#r2-9>YkuLA{nG<(BA8|4`4lU-i&~Y!|wHL#z-B$KKS$YCsQn0yW{fRQk zMpR&DHKe@&2Zg?Xd*bfhT4>}i=kuH~JC(w9nFfLhOvkJUzSife{P!at{vWy2nOMG0 z)%8?->~?Q?($K!`E2KqCFwt6S-Kv=?14%#skxM<3DFS3ic+n2g9YU5-A1=8{A6ME; z{TZn}J`Qxx(78?nB&tBge<4vXK=(&0Bnh@jZ|Rt%)yQD1r41NeuwdYFG`=jU)!W*f zyB%i!1}yw*wJ*T&iQCl?$540a+4{kL$iYC{fxbJQVUT7zil+|UlrYyOru3Vz@<6ni z|L1y(=sZnG=^qMfO{e^N(6o z>gOG6ax!(~2Hh>gv%RBaCsN@0{6|`&(M^%qm!0DvUe`LYju3YDgRm)-Yg-6)2OqFF z{By=sZ^npxm}(zs-V$jcR@PF)NS@0!wRk>1@n-ya2*C|xw42Q8kMRb!tR^jIseNRs?W-fBrypIhCBSB<9M+L3Uy`y2jtl1>G@EzfKCw4$&(!7@sXv!| zjiF8vfq~$tuOKEk%&vA5GKO$v(04m6rrH|t{81_SD2ILg{)k|4KcgA|nE))N5jSnc z1?+pj>hR|(RT1}`?G?8eDV4K}uFE{vEjc`v$906_?zCI4NLh@kmMA~HjS99LtKML# zQyD83-psi?xV18YnxzRS<5NKGhx^<>|H0RWE{0xO*Xa2aKg_#$j;5B? zLlG%4jG!ciE;V6!xiiub3%X>^s|u9#-pbanf1W#%DpF#jSX0uruP@`9K}#atvp$== z-LOSepLfE+`U9O8J>rD!EQl96qW%eA;P^e-9K*H!RoutUX#qYXt=GvAr}t&|N$k`i zc%cic9^$Ssvd?KlszUpx;{~?6id~3o3_lsEVucK-4J=O-zo&>&*SZM3qn|m(=fY#{ zIZ`e}QyzWPpgDh5q@|<`A4*}~*xO4wap&i^5C}%Xktt^!F?(R3xZU4p^4fish^myp>PG-C{) zPE>KS21}x&*2RHx<;Zo#cU);-L53UkBB(vwAGkY*j{tdZHk6(*ep90J99^LS*-cB&|Rkp9F9eeeI zL#%EbrA1x=e^COaD-uj*i)c|;sQ$}mKTRkWExqH7yxl(++X^-L6FNm8@# z34KOx*lR5pa z(9Qdnd9GT<977qbIF|luPeJJt975*hNLX#(k~Xde!v=Cn@{PAG8Hk$@s#uz|tSdK{ z75H=Cr*V7Iod^bWgrnh*17CMuHsjOsMV3M4fu5&#+gv z?ee`f&yCH{XP_h9(xf}PMZ);LfSZ36IA0sjdS_V?&5VPv*1y1go^Udku|D7)-eEO- zNqXx@W&sh?J*;hhfhVD6)wytlgi)w3%*Znk75pJ$pyAggnj96{EL>UJgIcur6BnTY zo9uK?v~EQ7@wYoeDFd$PM2a1|N~H=VU;z*wBU7&0q{Pi3yF3^QfsLZXD7XErMcCZqCUZa(%EFj)~&TY*~C8)RU! zlis|?p2#Y?ji@mn#l8Hom+!o~{$sxmzeSSnD<1C(5dVXG3~x1y{TYMcm-nw{Rwn5= zpg$i4-ws)?mr60Y-sjyo4P(3phuHQ&vohNW5K}#eag`+5{=rFciEr1&>^ko8Gyp`Q zJ_!J8N^Y-{vkNfg;b00y2HfjpStv+EEkYZ7t~l-=l;$DlTXb3BJUzU6$L;^ zGd3eq9*g9=RL>|?*rSK4ELGfyZAvngC)1_&k6KKlnO%4WK?&C6rRc*Jr@w6{Vo6mY zikWQ$eWa>0cZul;g;Xk45~o4Fs@ze8N)AimFYk@^5}7MfL}+MSJBI8F-m!2NAdF&< zmu19cJk`FM9y$~^6>x|Bc7>f%PqIF9!^QhFy}^RK%<^X7UYZ|!7{9rS;M-u`(3qb5 zK3_-X;&YHg5Of|N|1Mm82CHqrQj?=DW1k{xQm}VU4grvc;Vf2xf=Ff%^5eNBnXe&$ zp_#%Lp>?+V*0Z*N@Y|*72kF42Aaq4`PZUOYcyzsQ*1rCe{AG$9@k&e_!RRf?Q#s@n z`n!A_^=k|UeGXw**3=|Bjs+9vmS-Z93UzLk!S0?E`^|_@4_|L}p6(z9m~jZ_nlPsZ z3@XjXK5*UGFuPtKZ8d2QKgCtBEQvc&N=lTZc2CBo+}d;me!gANb~g+Y_I4UI>D4m9 zNgMIvfmOn-jbMg^*J{VLN9V|eIR0|SOpSPq#((c0*!%0#>YDZiSzDm7yI`fA?uTN- ztE~ov2_xDP?zMWSl&+;EY-(z1nrf;9@x*C0oNrO-Aqfd+$H&LcII8@)PLWdjiIT&Nh=H*g=vn9+O0Z*U1`3xIKo&ETv+r?OmM;BkaXH1TpewFR3w)^;#D!~1G13*}2 zhdjKz3$vF@gIexY(WnaQt!r>~rV?)@_38cMo2*XzBVlrku{JCH_sx7i&w7 z1!E=PT0=PRn1=?gW2gvi4+G9J9?K7M7%VT8LoHc1p*?Yg1e3tFq3w~#h0Deb+8{p)3>5l2FAZ#E4 zv>o74r6Q9Ikz}!uev~S|^ zAXex3>Cs;U>N61SQZQf}yme6{7Z)2Gq$esOf=5dm1z=gRiNSM3R8&DAx&z2ft%FzR z=ikI77$Mxg`tcJgZK;AGWkC8MZbxj`!G+hO=&)CsOn!1O&3nJPAsd?F+x6AjcKWDNfkIoF+dT4F8O5B>-A zd`=S2E|I>aC(uEtJrHWQTZJCTQ>@I~0K*kfs*4nr64h$SrrtQbEi+^MIYl{rB|cpD zutVK?snQ?Kgyq>i5LT1+u6?=rfZYpYU}597<6|>TBS8!2^=V&>*Op$N<7=O!=e9FrV`H=BW(-Nmkp=XmpxI(1B98AWwU{rlD|OYxJyjm< za>RoVL5YMhAqZ+sEx>~7)9s*c3Qz5k3f1#C_=vos^YxODEA5Pflbv_~WP-Qt`sH_EF-tnueMUJZ9<)evqh{%CFi^8Z#Qsv(c-5 zz#Y%jFtc8+%r)ER!c)7m1$xM1W$}TjZSXqlUFlq1dH6Rrs2!A-1$P(jKl(MWenAXh z%*~Jlm(WfozGdIrVyDyhd2^gLnedGy?x?ummVQr6Hjk?V)0JQu&phl-K^a#?qFH~uMc*6g+Bzx<=u+U6b2uAs= zG{S7Fx}G+}zF3J*891D){l)mJ!P9U)Mn~f8hO-Hl+E*b-3Pasiz5BvsG4;A2t9nta z+jYinF*iT&Erg0j#rKeVhZ##zz1Bj;3?`5i_!K&<_5MkZ8f(R1B8h4Ji`y%aL1}#t z!R3y`s|zJ4Yw?FWY6s(0!OZ6l9z*SeOp~qoGksboS4QKBZM)TxvdLkVv z&i&#Vw=CU?SMAnWO-H(3Kb|S2%w{T|Dt)orSSPe}WXr-N*&*vQyOuVT8{Mff zI91l8N9YIA2J%J3wx2NU_FW)HJTj0Ov#cXTg4g8Zwh?O9HAph`*)g3M@`iKw+-}@p z(}xe%ZAd%Z)!Fo)N4;EIJhTKlBIgiOM0$cKdJyuI7kUSJ+PM8DG!+cb8jsWtFQJL3 zEOf30)ni@uJ>4G89-Ut4{UpJWWtBB*gNX9{R^lX)>hm+xt5}c>8-`T;n!}EETMAyr4)l}H1IIX@W@9#^kocW9F(`U+T2b! z_zuu=d%o*&u;{H}%&54w(`nskk_W2LtoxR^n5%P53DoOz>SSaBeActG3&PKz591?c zV;{?dO6ORDgZ0kxA?!X*1RqN>kZ`Th$8)QuAT=E7vaqabCEorxa7iB#vfJE)o5En< z0mCWU7TSJtzSwV}ooPu;P5VRvOYy~kSea$*Fn2(iP!kIb`mJ9$$@=X>V!O22_++XvhcRsyfgpNl7N{=ln7cx^$8Y5x(iO3b4mHW&*m zJ}=9B_vQgd4^9n|+!~=pJ1~|=rvnvY26dMZahq{bFIkcEH7RRWSj#{+0SJ5{n`PW` zcD_Ftd*OEOyI^UN^QNY<(5=OZu^SG0_pDHgbb*WguanX+$JZK_x_KskO2cPEbzlwD z$fWoQtnxY1V7S8;OM}Z46#Dt@J=;gYH&bnq-`dpchW(f$e%g?ReD6?1eGT;awvDRD zmxMY#r8r7^0wbp{*dS3k;Y26--!SK9{+#slfm9hD+)2OI4B7%q#-rMFIRN7w2(qos zZIpz>H!VYYNWeQ{g2sxKIg8$?25@!Sew27_*p{3gs~t?fLM>+YXrK2VIqRLJ1|4$v zSvc+>HtIV6Dsqmh(8i%xG}j$zbw9jcgWiFaVz#`o+e}(^1WgP2mjgD9Tg}YD zZ0_#bg+XfDJjHmvh0b+2O5 zI9Bk0lYfcPH6AWql0KKP!zGr}Rq^D9=6i`;4%m9Vq;yqQLeW<0U_vH@QkS_W2JpBD zVpdv1{fQ#$m1&PXOaBQ_Tb1?v%o-9_zQU3mRN=c8M|S_$j>g@!R{KR<%}Fp%@n#E{U?nhr%=_p=U>SBW`97D~j_s?R=kOb!$WqCoXg>^Ue>4?o{SAKb-{ zFTDW}X~Txu{~_)z!>icR?cYEkfhaNJ32`OvO5EMu-Q9%*;zHcrU5UH9ySux4{uS80 zclSBH``mNoxqJx^RMx7RYpz+NesjFzohkile>gYG0#$~0>o$y^)N4|E+V3P}B?KLi zM-Jm&{-bEORhHz~mtd1hz(J0fB*d?CeQEMcoVwVi_nPpcTnK0$b7H z2J@_`i38uIqfDgwrnFwv>Vg*V4B!VG&%zR4VK6+$f1*_|+YsaWXy%b25gGWpl` zY^B*b*^YY`F40D07gvP{@CGE1A&8vI#Qet1vMUG_TfHk1cR^%(|0si0K+j;$M(8lnNS^)&f(79ZvIUS4PY#KNs#k*S zHHoW5JS3Z|J_g_KE)pxeEA)>H09yNRW%>6zV?N#hewJnr>I5Q{Z}5_BUJ1i$3->1? zg`DwD>uUc_G`qBs?bHU)Fx(1|6542s!dEp2`ced1p;QLgMI@}~DCV-RAvCE!V*E@r z!%XqpP`GQ_wcTY&&}i!}HyykWDn{xK@~x*KBgEUrq;T!r0e7j`a5c&PtkM1qO+I)v zin;rE1y{&?{7O|^`(I&m2S=7`d!iO3ld{gI=UYSie)U~Hy?)ucOSKLQX8UOQ-{RPf zr4jmG)V6nd0OQ|0ad8bGPZeu^8Baop6t;e z*;o*!Xvo3fL!*F@phK4Jz9P!t~1EcxIQl!H5!?%b)S}>5JypwKZy2SdVMpW-S z!`foEF7uC1tPs;j1&TCvpvhnQSKio9C-)`N>liMyQ{FRuPfgt>?VH)a?*H#v7$qga zZJI?sb8{q)4vJW;cVKyzP6q|Sg-s&ZTV!{~Xw!5c#*|YoBCr1=+>yOMo<>7gtbZs~ z>BB4eYlyZ_Nskq*@XSZV!Tf`oe7bz(+f`H*%Ve&0pWau<#BsZA;yw$;ah_fdy>U(W ziTlupE&HFCv+!arvX~0r%FQT@eR8REwd+wetmycgvxpwmBh}x4S*>)M0M|O9v+Id> zy-Up6Z2iF6&Zv)IO(MB{^{7vFAnjo3t{cW4i1_yQ7pt|N@1m}G`}qiKc58oAm;kyP$s*wU!ZS(?7uec(5ksdJU6(M}erZ-z%1y0?Jc zwo6$+>$GT3apn9C9+{EaWs5?X{gRWz{nniggk}XHG%v9>OM%21yfDDc@N-Yt+G9w zLjJ{s>@t54&3^DmE4mzklR{yg2je^91j+W8UFd%AUfoM3u4^bwj z$t|u}!aRL0k4MMrZG84UQTN$5xpQ2tb~gRuxvpd&CHT4>ZU}khlaGWz z@~#^)=irg}u~Nb99?(H%$R#3oy!P`MDGo!G!o^mV;Js*v!ShaGB;Ms96(fTcat0EiAp%qKQrI| zBvHSsctuawnxZB==*+s}Z%Oztie(pS9(}!o+F|{{zw9pf)T}Tc5mLdSJytH^D1PoW7aB!kpsi>@LthD|M;JqTz;aonb{nF>(TM3imItiwL z<@}6j@)X5RY!*Mu=!IEpumWD#ctYN;vEK`7@Ofb}6ZW`?HCX>`$^0-6)^_nRe_f-dUUo&5bc&wRj$@#*i%wYX~=YxLkW^4lOM%Asq5Y6abV}M|TQ7HTABK znQ+uu_i{5L4$f^eJs@-#tNw=fQt;zZGsJYZN_;G*XOAV_d@Rkx*eZBSdFIc9f3C7C z62lu%Fft+u2iSpE4zfgrc#oGSV1pY-I*5b`{3I_cb%fLNW;}5F@RnZM`RVM2G%WM9# z&*DFsF)Du(V?;#&_V)JnDr+lQZ*Fe`eU=o`O7wI7AzT?4q6Y>dm{0-hvP%k4PFT2&uISbsun>~~Xh-|=;fe5_Z&c=4 zZj!EQMW@eWiIY!>!S$zfFCufSSF&cT;ZeS0<|9|+%jB-nA$DYI>-w#WoUvW(24|Bb zCPRmO){>&6<^QC6fTZYts zddAK9Q}UfRhWhms5lgjs=gSqW9v`Qeu$owt?<0%qwGD*&3DM-;wEUb8EPrmKp!pB5 zxTg3|BZjVJE^MCf6t&F(N?rCc--1BsTF$U%IBe%9e`;m~dY>y;$~Kgmcxah~=gX}@ zrnWkgcBkD#u?NSluPyP=#ONvSU){1=Pdz!=I@zLWJUYjVh(CR!+wY9Lr5w*or;W`~ z;>;#OQ0TVY2_0AMsa(;>LTw}*))U?qu2E&~>BPRZTr*UETp@F3iSns%_;4+2(R;!d zxTDrtcH)7FYk!EFu+=owWbHjuXZQ}9{7VIh`?4LU%{Cvkt`+)1iIIi%UUP-hz96Rh zoh{5`J`=YuSWVaWvz~*_gTqG%JDc4sbv1-wnaTV2Ha4CZXOS1@{w*Y;wCfem|R>^Xx!IqFc1y=V)w+sBq zqCEdMCa=qPB6W;8&qE);B%DHL(+FxS9dL%(p5HDfRgwaATmi3F?RqJwY zE5}5OHN;h)o51K-rCrfCii5dS3q;sUc;J$$)h7Q}qD*=67g0`Z@2zp&9;F+62=3Co z=KN@YOimHKFb_zL^uNW)jq#VEmq(Jp0q79Tq0g1&t)y6pGbb zSFGg|=95=it2X>urDY&DTVO%jGuKS&NpQy{%n5`gQd%rFw<*~$1wIvylA@XZ(T9|F< z>5bT?dforQadKJsx@F8Up3{bH38MA^4*E~JjA0XL>pO999y^;sU0Xl)wpNi`k^IFo zwG;UpE`u@_R{rmtAAb8}!@TAl+5e;UqbDtZR)eCmNONq4xiFZc!yhulv0e=q?^%q+RH34UeOf?}A)&)jZ$guIT!k*yY8K}0 zdgE6%I?n2(L`(bE1(#)i7I|)a!4L{%}sa<{x&?8}AzY%nEPMa;LdWYjKteH4DdrDT0OfA=7D;@qk?CKOX*9|p_m#^WYb%dq5pxtiSHxuL9X397j4P!<2Ala zLy}dz|I0Xtt*SP22)$pK5DJ+=S{fQ+KY9`=J)}U%<4cK>$NeHEvi>Ybw_id)~Y zmU4;|`65c^UYwiLQ9GS7)V{>c=Mb;DKMf9|g|R-Zo80~;*x6b?lEe5-7r0)7f<^(j z4zv9K9V1BHtk4_l91@Q2(*N~oU|>CCx?^X zS{Cf~4<>CG9;_S2?Q+$VllPx$%oCbn^OjCfR)drtH458_PM3a!h>Be2@MCh4`+$aY z;yoge*psj7DE-7uGT@ik7`>*Hq{&3WlW!W@QI6>c6&*NWIXKAQ4-Xqqt$x$pih#1T z3qAH|ah&*ovNoi>3Sv}MF=G=9m`_-H zJM%Z>bepHM`q#ZIy|5`1`lidihVB|VSUa4dDFvZgm)-4o*%&Xt_(&4)Kv%(V9f)c* z8ywix*|<`jCB%f?UmFp4Ph|TBd%O3Vi|QXQ8W`n|z|8Q1r$VrFO*H z;M&^S0GbB85_4woI*#(G&P5PaFg(CZZ-gMg!*i>uVvLTCCbGLsy%?Ui06|_|%_x^I zOER$z{KBu3ugaLj=!`zF7ys?AUWohCR{fvk{{QYEt8zOKzSg_@=bK^lWvSb^?On-_ zYt`hMM^!Wz=<{*)kgUP=7xc8wqHCtllMQo28JhR;r>;+Dg7P;Znx`vs4qYB0Gr^$X zFqW%r-(Mffr1QRSYil!?;RS}oI;VV5a3BlY$-)vcKhnCpv(tO5NPRu0aNZ_=fX5_` z-Qev#yjV-$=vUgd6k2ZdbU3Z*e7<(oATLUx%-g z1`S-o5*$|B-U82ckQc;;V=p5Wj?GXvo5DA6kPM}`J9y%>zhhbb9kEZcca&jtrYH#8 z@$q!X^=bx2dFpDsCwRdgu%FkOKd<^?6KgcFad2?(A%?xoHGk9d+G)1Vkk6cJOXdWW zb|zmt3Ay|Pwfg${(p9R>@d*jf<}I4F%69qyhfCOeJHw{#76FBNd3b3hXtB>%EhhL8 z54>H^xQow$Dds#y5KA>KZ={P3Eu*`$o_<=$v#ID`Z#^07o!FjdnkEg2-W)j4JhLzf zxqLj8Jtw`XS2=kHm9ZFEgs-7sp~rs7oPozfylLKwmr7cc;uiCW{JhXNdhH@p&_H=K zL{Sq1fdh1`!w{^!qsK2bX&tXDMDz45VCC4{Kj{D$sBQ8aaCSv<^J0H@R1u)VD5Vws z(*p}uj?6ozfR=*5D|hti?bJiGm0X}GOoJ-2L3<08YVNZ!Ms*&k1ik{c#{)Iz;{^h{ z^M&NE?@@IWG}#X*S8VkjWCZlppsx|fYD@-?16~4iRClrh*-fXYmkY3`qk%lTi=Et^ z$JtWL;a$lJEF)UQv=Re~eHZ!iSB=a8VWE$ZB^K3GY#ng>qXZYm_fVsgK2(<(R}1ZM zRSon}C1TqqL+07ptH{g&d^*Q%OqBtB>awhE=ljZ2s zCB3!5*dq?z)rmQzOUR5gF7kAGzNVY&B+!FT6Fipd*;WS{LU$Z9IzsZ#pGgC|BudT16GFg5C%15*}dcs;X zGN|zQgFaDFjdZ~QTX#L2YSecNCIs%w93eT_n#PatqRo4^LAD0Q$vwE^W|W+XBAYp@ za+cUmdi6r`{2#eI5?h3oU|g}mwcHF>Ob}NVVBRcF$M$V`+&tw#f7drUUA<%e)=u3pd2IIE(GBXMp^v6;ZGRB`yJ(ba&kPDJe*CAGW3F&N? zGTeP2qF`D(wD%91pvjDH8N`;~$c`eG@=~j}heJZ*J6>u8kk=&f%R-8nrybd$N+Ho& zOC3=~o_RhQ_z4xpG5Ti<7sB{S`g+4>QL`;;JJ3Wo(v={jb`VjMuQ3=5Me>E&U$szl zF%w0ui^pqEN|eYGov=4mhs(@mT?o~UOzF#BcR@$IBjRR#g49`@4|2JC_EV(x%x9$K z`-B(Yo8)qqeXO}f9w*>vB=F>D@dNC zBQu6;Ls2 z7zCQ7B4M!G@@cNxBic9lz4rVX`1=8YAuSZmpX%am1K->ZZYRGIt`m-umE&tlR~fg+ zotrOMI(Tn%*m$T&e%Y`G;_;Brw+3}uz21^5mVU{gdVxBb0aEaw6v5!M4W}+a7z^*4 zBY2!jeK`5sC_6Hz^?G!GoqN;aQeb0@qXJoNq3hhXpwCe+accz!Ii7PdtS=-)*3BAY z@gsXzE{q!!V-+|Y*ATu1XGhk)kN$uSXPMr5Or~Sz>HSr8EX|<`DO*{{(I$o^i-x!4 zI>J)Y6U^kD4}zx9=1N}k6lZ${dwX)8_&Mt0&fW#Q#LwJs0(lm|DlE{KFPKS{u~l!HP|3;eux}be z+F2W$SKirQ4^5QyskMi+OiEDm3pw3lO8}7A@K;5e2;m>Kqxk?fcZ!qYw-!l+( zp1jNzPdtsS-kH{dZLL*d{O(M%U*6Wq87$?1z4t9bOYmI;$M(5VSY|P*Kq$EKSk=7b zR?&S}#q2%=`qw0-he=X?+55P+-Ylau<(Ta1oE>Un2~vaIAQ&elvtC_USJaIvD$136 zZ8jnj(iRUa49!yz7N8gf?A_sv_bc^!P)X>^B8^~Xv1kaqMaFs+&Es9ce*Wk)R7K3& z3TzV9RYi`a*MSYS>oa!!=&X`;Ux0?ebkN+{>Nq*UZ#jt1pRTaXAtr-9TUK1r6&sE1SH+zTTOj@FI(+V#@o}Kc)gH+a8(_6zZQ1zQ@3M;a#U#5Oga(Wb`tu-$J={DM@GdlPJ7p&G<;*a> zfK}jKDt@+LEnQOERAYP30Es`f>ifdNXP6h<8`iKRV*^LSMkd)P{}$NE2i_A`dpcnH zcPSe#u9M_%dG?_l_nXgfo z30c!>wGCL)p!#b6u&?xF*@;Ayli}xz?E_uSP$^qt6YGm}rvdf(??V|DJn#|ju_cVA z?T$0bJ>Vb9iXf3GfWUR-R5!pVVIe~YjEpr=g%o_mf7f2S4ER*I(vE!kb@1i+k0>fN(!3qS~NKpk)NE3Juy0(35BWu~)RaDjzu) zeUyD$lQyhl)I!EEhUf4IB+VPWG+rZVAn@B*mS~9_v2YS%zPcujOsYoj6pl zz@hNhlc)$Wlt8yS@7eGR_(=9GdI@}yQ|7#vjQukRQRsVX!meEwGxk%%W z@*BsR6;l!+$zYg<(mRE*5*Fb*i~ZOe#WzT9s#f-=E!D0l!F5l*>~YLVGJs+LZ0IwV zb~K>NzQ|{4qEOV-oTYZsc!?G6`mK0oD76_-kIOefl$=RI2^$0#M>PIwMj%E9PFbLs z*^i<{VIsF06Wu{2=a3cH_sBNu= z=6u7yP&cG`dncj9VDe6F?~U3yBz;5`b(4$+M+uwvPsn;nhT&y67v3fq7Q=d|X|vs)iBx&lo>QR`7~JyX10!Lz~3_X)Sk zV0^L{!;KVPzLAAumNUm9VR*o76QyZiEIDuU&4R#UZJDWQgBOwup-gj^!Gp`ut39zG zy&(i7OAxdPhS`cVg#KxawOVAaTdQ7G24&W=i{^^mEenv5%nhuirJD0O8wE!{DV=+( z5`+=LaA)!64|fZy94rr1JY>u(@4z!m71U}Di&`Ky3a}Pa$p%wVj!p9Hs&1^-q7tc zrsygP8o4XcMCs)u&}Cr4`j7am}ClujFZHi&i#N8=iE2l{`R5 zQQsBrf7y+vCoxmw8aU2EQCkje-h2n>7xlH*%TKX_-*e(upQ?3qrSKZiM3;NAb%v=^ zUEKxnXm0h!31L_!*YeQBb=~WIk7@U*HalNc7VUs!T4fdj0ZRQvNnR#W?Ra5_QZ7Pf zYMj2KDB!}?$wOmbxhgH5$zj$KZ(%)Emg!AYX-QL|Bdc(>E}2Jaye*!V&4 zHp<+k(^wZr2fGqyg?Xntjgh+N`pfd|hp5p>>fcHdX`o=y_1I}OkTez3nImm=TEM%= z@tk&?@$-awA(wO4hT|HuT_mu3R7d=EocUK-^Ajj~aNEpsJe1$M26xN+8n5*`o7pBGm?D7K6*Xeu9D*koK@vY<$=Ok|WapI$jjS&rUV z{-9%tT6ppuH&|Ft&Gp062VoH(q}6lh_GO!)?I4V8o2K;{q|O3;COuQ6FReJK z6gVL!sLRE0{tlHhh$UavLlvmaHQh`RQwQz`Qe6D6@qEXum!N4Xw`Q1X;cocD4jkrU zrJXjEr{-Wb*Ojox%(ZD0T_kItuPb4kExqY#Dr1Kj8y&_qB_*~dltd_pyK|`LRHqYI ziwq<_mNc|`uG*m7bwQacIm^s0zT?Xv=}Fu9l{WCrQ9rWdriRT9+dpKM$8uA+HoC&f z(rGWXqat&CCpJJ<^+O*v5uwavY3e%McR5>Iy4Kd#VK*R)=mU2{Q#jCa^QWloKypkW zAwL^Ey%Eu}n+FW~GJK6D$zD-WNB~{BO*TOJ2JsIsfWS*2 z)S6NR{?=PzjY-t$lkfN(`A7Y!bl68sUElg790V_>wP#B$G0-K{*6z&S7NkE-9G!wL zyUQL@w{t;;F#@f7-(q^bL^QqmBs2i2Cu;+x0((KUFC`SD?PZgf!-+9Brly3lc$pXT zI}fU(UpPD#S+MM^g>xhWh>UtkWcrXU8S$#%_jr8x{#Q;F_g@WV(*9~FBl%ZDncTk` z+RFYFe?|I7^B5re1nJNCtG{_!ZOM^W4e#zC_irEiA0zkfakTiaY*~jD@k>9JM99@iWxgEH;vk$oxd>~e}CpMPpU2lltTeAVH^7q z%bCAEKwL_QTtfctFcQq&rwC{`T6sA6wpiH(;y4kpwhC*5d7?`64}2@J7K#e}Z#Sdi zl82;ePfMYyLe04LEAW`)x{(7F@?MLuQJa|TW8`3SxS}{XH~>w>H6(d|ljQ}4WK=D5FPGmKX5A)uZvDFtg$Iq)ObMVqcn{~L7w)F?b=qFOosqbsZV$NltIB7 z2O3o`AFw+1+jCbnv32|SLF(mV6!S!h7$jVNuL>G4O}!DxYMe7XS5-Cow` zfr5JBiO8V|igll;JS09ZWg6|lx8+YtOhWhi;~C;c7z07n4p$kcu|*5A&vD7o@xoJa zy|VPul8A5HY1(HJ-sUpSJn}j0McTIvbiA%^vE)!i6WME2IzG_j_hotGIHRMl`z_9! z65CWz+XmYqNchUYWvdZI2UVf>oAxoE)pJgU&N~|Wtou9GAGHyq^dhUKMnP$1^hZd_ zwD#k27uNMnLiN_qFi~tDeKh6}+ldKK13}1K zWsJ2?FDdMk$Kj-v1D$n<=0o&87<_2gs8fy{%U|%D?IamTgMwywWoEVKM&+L`PNAr; zrrF-{6m)QcqbKqC(*lI82Va@12n2M_Qw!^>=!IHy&lL6r#aUneB!!h3Cq&4cTPqVc(p-NVb+83)Ix^G%v#Rq4FE!&w%ouOx#Z#&wjC&+s^p(8Vlg>0KEf zQKI;}Xnq!eqooQnN4c4(l-hQpE@2;Zp5Bfts?Syp|qIW52Ozx!D3M4)m+j9n(fEY_%3}-z&3?g8Gh>2V2 z9uv_bH#|O^{@|d-ou4dF*qyB)0)7~*w0v#h{7D502GueA_NIRDy1) z!#Ygi9Bg)k3KV+PlGlYH18s*;$ zmew>K7Gc=}{!s(liDY7!)47V$(ocZjpIWEu12#70Pb%O_zB#U{=U;c^=0o`41O?m$ zM8m>(Wg3=}YD4A7%67(|pi1#h(Kwfa_*7w`qzg!a$fZ0J%9Do15yAHUSmVynkJ^3S zHLfCXDm9ZKwBy+4uVIM@F{z5!DEGf)>>IDQlA>GCKJ>J7F1j?BndqdW)B);jfNHe{ zis~rO^mK(3IWMcgWt$+LxnO?*T1G}98k%qlwK^*8j(5gWg(QERVw!q-JPI_$t5-EU z5|$fYcuTNN7H#!(<{u%z)W6=SWLn-h;$p!zhPI(g7|67IE8pyINwYJpiEKVP0#ipK zHi+s8;e@e#rYALit-^RLaE!{EDkjc&iMF6pnQh#6O#bLfT@bBDmyKAKZe@u zRCtZFDqHV8^v;w1^H)dHgbM@31m!ok_cBe|B&OD^@ILOc@2J)my6h4M2C@#;i6K&( z6W(p&z{t?bOl;tXXKC|4h~$VTz3hop=ntm5-Cs#VzTC(CkoU6k{iz{#d3bwp!LE$H zK1RVB$^(sQS0zGpWm7fgzd#kmk2P0w+7a5NN9}fB*b^QKyTSSPC2cylJ9M zw6B<_AwDn*_q9tcFTi1}PQm$Hjc(Q71r?P0C72p>*AQJOl=P#jDy=_G|IcrO`$Ccw zF2XXEZx2nN(}rtiXDZeB)ga%V&lMS^yLg{!nB-9xA$8J$9Nk2f5o#Sm5q@UOkP;GM za!FRH?aZkevloQTrpFQqWKeP)EBe?eD#PqZPCc9vVRV2|I&Gmjc|C79m!nEyg|eo? z0g}PIOk?cKeD}t<#<4RV?9h1S0Y0lKUDJ>bEn6&se=wf@)y`sl^+ek*0rlOf)?5JE z)$uq+%yo7y@$-7!NTIr$@@0HaAE<($)GN{VSeLBLp^%$H0jsu#(xLo^9GYGE>kA`` zYTtk^W6Ubiw5bC|`aol!#*r@DSSKHCdJ`$a?zEpl?=Tv|>BA=d6UMzu!7OMTqu>sk z7CX{Ij%tsIJEIuu7&?UwB+je3%Rs0~+EqSdjK(phn(9U9Whj1(MDq_s9jSX|hhC*+8$>_J?QPlH9!6I<7ek#CVCA^* z!M#Q^Oaud>7&DuyqJqqYzbcz+9Oz#2DLznwExE#R64>?dR&R7(U0=uh#|;0aW$8Z# z{HvDz_JJ}5Xl@P|2~+Obetlcr2-KRxP2zT9*(nulXmC3E`}_I=fXW!x;a@88v``5C zJU|j*3t?;OJN<5GyfN=&Z}%)m%Q=AwCou$qhQ_iN4A;xhzy5*06_uvQXrMpvDja*( zfUmVd5bN&p<-dT8*LQ;O=lLUZ(zq>EsTsgOyolY}2;cKxe*?N>{C}@6|GjKQL9_*- z^m=M@=6JT|%(`cwyw85!w1sH3Iz6-bA+ z2@rg_h>`-Y`V0xyp>z&Al0!qEurOwd%N#52YghRwyDf!m(Z?`v|Gh=*30-Y{`)uX+ ztfJ7trM4FR=}ecmg%7o3R!-~Wgp@PANN!x+xaxNQSLNibmofa2>HRl*(DZj0=g|Ac zwW$jtUB1bfF(Zw~W5d zO9QtL+PHjvKyYd*>Z7<+fLwyrH}8lijlsmmDXx4(8S)j;5o%=IPih?4k@N+Dbr}c> zqOv*J^oABPJz5Wu>U}aDZa~%{Unh-|JZL07dbYKN;_524)$jnPaiFQwCrSJqOifqS z)yQqqWnLHX^mG8ZJDSQykVH<+K-ZehD-hjL@P(Af-7mE)1@dE8_DoEEyXW#+kBvw< z#&p56o%kHfZ2CaO(f~d@B2X2eiYw`G0D}n_aVS``?4+Hg}BTa zqPcE+6%37MT8E=XM4{PGlI(2*PF-r|Qh3b|w_3#aeC^6Yzb7IZ=gv!E6hO4?ACKG% zbGb^2EY|CwRQ}-hP?;Lbv}W|9HOzm|h-nQX4SR4UhBR7Lk=B0|bN?*H99CqB)QgZk zI=J6KbL0};-AD#1BT|?Ab>WFvo`2_b{fv{ifS}Rmz2GEM)@ZIoE+~&(V=3VRYdn4A z98nW0x|uhXoq5--`1vfgnsIhtp%LzB%iDb;*?)Rkl8rnfb)J1Dp-a32J$|ETy+x1qjRim>x#;y0^^lQHdcZN-|ai(S~G#-Jf z?3{aQ_uY+vbk5<2>b()O7)G})a_xXn0weII^kZN2Sr0ym?J=5i&tH?$@d?7#mZr$e z>fzy$y6-8u>=2UBj7*sKLiG*|4P-Kxiapv6hx2T`Y4lBuTD!q`sFaZ|xlFxt>Z|-n znc3>9w;~FOtBU1%7VP73w*8C>aZa=A&Qg_gC#hksb~l zCMC@3Xb0YgeKwUq5hUrJKbXM?mbQCMUj)(_Y+R(QJFNCSV9NKlY>&piOCL_1ne|gJ zEm8tqnz;W#AI&45zL*agi!6N}H=r5lGo@yggntRt_rZ*~=4j#1+d5ZYcWa&VF zF6+;}oEZvc+x9tADBZ4H8_I@_H^|vdMxn2F4ji;|{Cjmkeosoa#u&o}Ep`Q1tPSff zYFzKnRMgb19UX51fAl5}_^nGPh7{6*GbOtE7Qtmv_a~cc&nMnam;2SD_$JeZV6wb- z$B`}T*ARFtq;;7|F%=Os{9gYa%2Qk7(3#6n{CM-r*!n&zqVL@|x$leURn`$uSLO>} z{=W85`q`(5R+d2a0%JZUFr%ZMt8f-i78XC^L2{-q3WAe2!zW!lRPJ?tuv3dArW< z{DY+9&*PJ(B4?WO*`j-{Q^A&YgqYK&M7??l`xcQo4(sWLHJx*>RxgzIhfDJUjnuo6 z3o~eUCY;pF$zJjCm2m7zj#S?~zMnGDGz4M6bpDbO^)}#@59OrNZ{85>4*eGrg4&76#S6dxOBPKNjW1pRtN?Y7FdUO17EINM;!k@7?K%sx*Yx1QZ!5 zaf8-1&w6~qnkRfjoyz{m_4E{3q6m?&5b`Yz8|ifrQ?(T)5v>i$J+VaH5_r{UXE%8L zfSc)}-TnL5f82#<9rKd&(7#{c;OfGG-dDCf^?}oo?R$ow5DCzbj+B^v@{t$YU7k;2 ziC`J+yU=l-r2KBD&z3WUB9)`SlvSzhx{NAbKWm1Hc*W4$<NG`6FR|_(!zLTOU0g zFn-z8uGkRU%HT`^BuI+lxQ-v2%57Vh_{1^nStN5_S3iA53>HjH=O{9Yzk4|yPSK4v zw%(NdoQRzY&_)Asc<57%D76QRh)x*I%_K-*Au3(ew7Am`YX57f@i z!(fY!tsd>Il&Dz(kDb^RDrq@JnJ>bmSm*pSamgSM{m{jXIni_fyIt}(M^Z{Tc<-;U z$B*UjD?)*a9#!Nm)q3wRHzCr7M`pFTH`)cU@N9by?m&hak&Vx|mho$)?4ii~u^7*z z)yOfg-#T%f*_6D(xmt5Zj6K_)?=&A(9989eH$56C-fQp@-QZncsH9QvQi-v$t9-6| z3&v9;@au(wLPUk7ow>*~^s2FkB1v+Pf&CMCNP&+Od7{${d=)WTd^Z~eLCzLb74=g( zya54aS?ohgt&VeB3$}y5q~hnclduZZ5ohxEar`qiQJKnyA1G?U008x9fE;D8L&=#D ziqe_xz8chTnQHojy(<9hP((Hd2+;;s26Z!GZCBOgyQD1OWM!@R%Z7L}LC%D)-luS!2EdVQGbHi;yW8DA!77F@>8r$@yk&Q`9r{P zg~uv+-)5|K9|Y|6Vx$kG#{{3nq0$aJILyXULGIT$*{{RI`}?%AG&*_AfWdgwlp=DakgHQrvIlI%Hrtr zDA6@7vpMOs!+_QtiwR4Jp>OpSi8cjq2qr7Pu&{8`R#|Cj%kc0qGao*1k$%&YWXcE; zeaqA)w~s5(69$#M^p{M{luDgxj*ocF&4Pb|&)DuFk@;bVb%XMw;el;*MJ}0=ipza} z*TzfuP*&36t(x7M2CI(|$OjvJft(L-2?+?^Vq>Scx}7Cl{1WsVodrq=5l*pj&nCU` zRjhdmLr+MHf;j4Ie+Mob)9HaGc*tTo zhHZ|x5%_>8xg{jjc6(M?=9WQ%?Z45pAHRt24><{hRa!8x??raC4xrc=aH56I_6_8eJmH@pwA!iL#DqTBNY7)iK}r% zBQmMgVqxX9ur=LXuGi0Gwe|*+#kOT;rUqyp0er1G$O=8fV?{&6VRE=SSVWf-d&VZ4 zPaikjDc|f8eMxSoEgEV#&;7Iok0<^yDgiIrZY)gNj+JGO3l#( z>+z}g^j?f$MVnKPc7~^Wr+DC@7^^*DpbsbF?<;p|;Tyv*kzE}>ye(XA2nzLn(iNV+ z66MVE?k;BC&f0rUmdN3EyOqP{-w3BKOl0-ki#F0-XeLn*TB7sxDcTN~b-)*4G}Gl; zPCt0cC0J~7p{kXvEg}JoFM+p*F>$tb!TPd}Ogh)wm;O#*0y*Ch<}B!aiU`lv>!1sj zXTXQgRc5ToNnqVVtwU)kyXDs9lN`ET`z=(eDvVAwAH+SxbCw`$A6mHjglRLhOLoas zF7Xgl(8{Yk@aBf@A_TSrYcUz4(8=yca?h(f6wtfc{04KDdun<))%YSQwOKHq)YAMYNgi!~K4y z`oXWi`P5i9n(7dW&vRcZT^{MHqY1p`MwhPs2DUfRwmaZ#{8$>#jw+U0078ty>b$o7 zyh1GJ5b>$AE9FkhF3-_|yU#*24#QienfN1Yi8Etzom^?dib%Ir1@nYH-KOdvzu>bg zQx4Zr+QL#xq|hkSmEv%@gbA?9zb6~St4)p(>*YzD`f+YW|GKl7ZE&fkdPEqJwb=_P z!@-((A9Dc;1hJ53Sx5HR2j$fSWb0NqAAHtvZ{kvDq?e({T; zYiORzh>tcToE{M|&>oknjm}ZY>@L!c&URoh81EirAZayQKoXOZAep#dh(*I5L?$8O zzAe;!xM?1ia}NDrQCp$dOU_I^9c+GPoO>8g3|(yv}vv~9?0M< z9!CA;GIuRjbs+)EZKv9Zk0i0pqJJT!sG3FG@lc6p0*y3 z+P)Kw8D}!>n|E#?xy0As$;WQ=#5c4*xqq1pK^)3LY0YlV`bBD#bJgjVl;&?iFT}=d z`B#NcdV>hFO}8KpPe>smw`YixhkEB#3C|C%E{q8dIa=5#yk%^SiX(`$GBt6ri8*K= zKuaJZD};5svB7JWd3eP%_qQH**uCFX-fnLxlz2G$UpJvUzFo5jYg=9ScVxRLhO%Hw z5<7>#z>Afvaz$mT41tcpsvrPsdUK0zdAmY#;(CLLxzu?`?R=q{E96mXuK+f**)Jmt znk_!0xG!$=aFJ1gV%AIJh`y|OU8Tb|I>ar9mrs+P^r>lS>0Ic zMF5?0BZ-Oy$;sSZU*-&T*f$uRyl3Bw?syQ#P#EAgRH&fj+54PsFu z)nW9VGS^(yyV<0|Mbm(yBM27{l;NG9Ta|aktWTjVA+=bH3Ue^Kt(9FmMx%s*5_Oa5 zu|}lep#)7+0Nwt#z`9c?C{*VB>{bHb9qJ!m0JqCNwy1vEMI`#Y3JT*vX_)-Dv^3+b zmAomedX+&*Ik^d9gQkp?h|qubPAkYgdNdW}$J_A*$B*$9ZT=t5 z-a0IfZr%1xLV)1G9RdUo(zru#C%C)2yCt}5;}+cAJp^}mY22-G=XAdBTkEX7&)N4r z_x?jY4^36is%pCC`7A>3%qtySh}YYt+1?2UA&ZrR{}%PP$KpX|GLyr-IcP?Mzx<6LB)H; zN=Xr_+`>p*7|HD62iL3K#j|%wi3cmdBj~~98m*+fT4feby$HE?sD$-}U97zkhxH_{ z#YsSswWuk>!or}`L!~tPK^?yslT5Bi-9l%Z!1T0VxspP3TnU5>RyvWcE18>6s&}9% zNGxVmSk25XDS1h4@emiP_e-j^FfUWC?_;y5mb5ME_T|ej;Je}2by-nTR<4~H_LY`z zouMh`O2sYF5OSRDP*bm0NwS?;XtTbf)xSMmeZ4PNt<))rj*j-&8A{;!hop13k4pVb zDQ)4{ErgAw3dqULg>?J-oL>H!ac^EHI6X^N80Q$? zV0?3AlK*hPQGY?(VB#jfv`Vx!<7ky~jY`X!?IjP=K}(+h{(lr<1#gPsF5w)Hey56c z*iQXZgqePAkG;~JYHD)j%wpge(N&EfU~&0f>VSi;Dstf>k91JVwT0Ia(<3%}gbUp5 zChSlCrY_s~dt`pT5t6O;XUg(MAT=KIvo`5oR0idJEN>6yNcf$?5P)IJ1(WA`7ev6F zBrumdM$!9zPl2x~wf(&6uTo5ygmiQe9;%#<{GAm7SD2~bf7PH={z%HrkNdl(6Wosd zcUap0-$jCpOq@TD^bfJN5GRY3HurdZb{h;Qs!eheTifXRde*hIwQ;BBf21lQUZIhZ z)! z5rc4tI670T?)XD`Y$^^(k0m*SbyJq=qxPR+@ZiVwv>`$gb6*YweCqIjrSt7<|Ky1T zW=J1#N6u7$n9BSq*?a~2Y=g7kn9C;8Mp{alh}M7EXS@*A466WH7g~Pp2M;M$ zRXkE39bq{UK$D$R%8aBj#tq!JhK^VD>=h>!?X9H0PFmfWTSTY#jKCmXnNClczCzg z-89-c1MArP!Ox#vUmkooy4T#$<1^$D=^sNV=BPDsrplNRPFk4l%@xa#eFxV`Hk=xk z`iES@cD5v3Pv^l;Nvh4xmsmm0q@fRG0FyUV$t}RdG}X5lN@%%#wH{_-qAv~s%-EaF z3C4b!_bOMOjbD6U+CpZz4MY1w9q58DXR&#yOp)_W^t@Vsd|7afyPUVlW_Ru@G{GnB zi^?(lzVoaRcU4P^%k74E$xg75ItG6m_N;dP*ssJqM*Wi|iUCi!Q!c6IXhjC`I7|`T z9$1Et6`_CO%HnzubaBn9;#Bqd===-Ubw=+P`d(9<6Ktdm^eRX{4SM=!2&dwvj+U!pI z2TAw^;7^M*h~eZ69Dl8+NCjiz4}jyFx=dj!-Yy^w_Fb6?Zr|g}P;WC5;N1+6%|<6cW`W*Viz9tsqJ{hLT8ASv{1EEA*ANzOIu3PTVAomy;6PE^XmLM zqTw1)nu68zP-pbp*3nE=*#C4NQKKHWKd}~^hs&|9U5aklaR&j~Yab!xN@q|Jb9@K_^jbRU_3XvgUHhyMygHb2 zp3q?|L4`Ss`Lwsh1o^fJ#bOoW_>hL7NSbA?x4juyubhqb;&1_IBI|ydCW)c&rUcOC zz(g@v5*DZSEXjFX7z;{KJk5kuf|4&lOf99tGLJE+muyw9TJVW{*=^h|T*U15zE{wc zjwrg`4Y=`*iOjy@I4T0XmG5tW;YNpH*#STi#K*xFF~WTaGLmOJV0#05K3%(PrsNLJWk+* zqx+!1nx;kHa&2ef=|I%e!^G9HRWe_^zldk8vkQL-4pe=5(?{*J17@~qTUntIBAQBv zlRfGYZsH!wPMMw6DUA=MeuEOk8geP!KgJA;W9D0Xv?{l5P#R4J72SD5;Rcg25%LV0 z9dLF47~lu0pPzTJC>w1zfc=aeuO)1WuCldPTQ7*G^$AtcqUSpTuE1#!^OsHD7GT*> zrrbNm0gnEP>qo}3j_*4gvUXckowCnB*Fyqw<9Q8_q`92F$~;vwOBD_$lb(4)aAV78 zK7N$j?o_9y>mkD0Ni|OQZHIw&ngR~_%OLc$0ncHFxUV|{zKiLU*Np?0yG2vOmLnF^l*Dh}a2VU`AEm9c{_DK}Ey%3yC@gJ&FJURU*#CSukqw5z9395S7rnlMUBi>kri zN6;}Ta$RO^rd$c0pFxDspqN$O3qyxBvE=yk4i2La?zI-$0@o$k!%Q@}^E}tuCELJS zCSt00Gw{_N&*5kWz7HvM9SKTK)@$|sFu_*a=u-9xt}`RBnBPZ2IT(??e=R3(dX?KQ zQqy^j@RANHY~iQYaxXSi@IJ4mR7_!03$da0gjy@-I^B@UxS`QgjVFM zuRaCnsZfCHu`dXfZ8fF>9h-4Yb-b(DA7`O~cg{06qn<_84jtg1LPw8D@deaeb>n%^!%KVC@qu3i2B>~tMu zDi&zRV;fnh_g7R$qQ0Hb)}?@L=#M#FjGTzOrO%n0DbvM0iOV<+n^x@NwS7h3@gc0d z{e~7-cyTCGp)J*)vu{17D_f9U$|_oEiC_a3ht?o7)#6EpakelJmZ2$l?TT~RD|+sX zV!50VF#9Oc$*{3nM5v_*Wd|i7H94fu0$=}L&|Izav+GbtR{-v^o2e+$*f??jW)IBB z5@#BHg**TB_54;-L2ZG}2}}U6@heHldb-I3$GR>zFqNF;+iK+W@H{xh*AcP|^~UgL z&?t!y;(b(+ka}IkrO{(wowU#raPS3$V&8`eXQa=R=LD z=(y1emk`%eTf~5gtEG;hkXEx!0|33yI{UHA!_^kZ8udPaEzbrHX{I=EI^}xiL7&x= zDfJtBp%}3RjcVoRT>Dbt?v?~FOTbU~S80{wA&5}k6GS6_`~K|k%fEc_y$aJQV)7>D5UQeO36gSEiuIR z_8>FA(jEQx%)RipOfPr*-v~3oMGp6z3>N*hPMeKdC~5r_0PAYS??W9cE2m#?$vYi$ z4OIzrr^4%Vwm&YE|6A6`+M1&JA0+KnAMSKTnp_e3+XPOUp$BVJX$jriF_f7I^K?6B z1@VYhNiGwm67{zy=eBoN*35lu;9uvvp!8-Zd{)|wt8_e1s4?V*&nCE=2Jz2`Os!i! z70W5l5K|*SU*Dg3Y$k)$a5^#myPsx6AbwTXgeJ@N^if6gn@taAV4Im` zt>(Ch&Eud;sOGSgS_G9tb9&ubA)*X+_r=<+IIK$P*>_P3m>V zKwUNx73s;~dj|O|oGsE*j_Tmj`Rn!KwQ3v>ArtYlbc%l#tW z)IpYFo^CiO(Zy}yv2&Q?#a_R)em zPgW_p%qe}1W~z#x@En7hFzDC{%#GIUx5f-li5+=iiJa9~D`sp70+{SjU~RyapGC)K z{b8Ha?IQ;C5c>GfJxzj0-r94mb0T%8FT2OOr$38LiHDs*nBAT)>G06^8tmpAh@r9t zwvLLrDY6~MJu)S!q#@h+S|v@3G+_ny)<8?s55`I#tHC!D>vyMkW9{+F3iZH4-stA) zSF#*sxPl{=pVd8cU!dEyozopS1G+U-j90=E(%bD&*GiIC&H2wp2W2x~6d(TFcUntY zT8Qg%rU$3R4OC9>j-6Z{pT~t17*Xa(qEc6O3SmH+V=xh#c-qrUO2fjn)=>>&4GyD% zk({(gz{vFb1EG40Cp$KLkZ2HD zm3M_lZ=v6fRv)z=^frlz5NOX3QMu_A05JakHkmsQij*&%6j0gi6#eMVvs!%6cD?JS zuq3xzDltoC5jRruRR-7HxAd|p@_zr<(JosS?JJ@9_WkCEl{1KS-n@|%jhiG zG+XoKCf`B&qK(G`TCIH28T|ei^cwW^hZ2_z4r(jtnNWD;Fd{p#>;)wye)=DBb2@VJ zul;Myt7zhur>_f}BlcW$ii%3GZeVkx^bzlf54|iiI)J-{JqakuW1C7{n_kOgiN_NxX&O4laG-FK3c8PRmSmdydheV@ zj?t3QHB#VCXQ`5;(9l&DgR6%4v^sq>{&KHfzIJYr_4epa z5y)P5S>W^99I?9dwSR|}leOuKG5pbM{AP+fb=vBl`jSq=nNf+=)x{H~Y#KUnG~ zdSc?X@MY{Jz>*Y|Dmv51DLNCBmn^dAJG&thIH7>!d3Dus-<(-ZMIZy9Gpzq zYwy#k6NH9%l~fDr?Q+!M#B99yBMySYC#vNfAr@bjsVOP?*49WRr>E0m%30m8Ow6Zx z*4B|JDe4u&Sb`z%Ouq#kDqA&IJEOy%Mk=LuWXUX}SEEz}-TD$~;w$jwG4+|U)!whP z7htn5Hc-Q84_YWWk3POGq}zu!!J*<8Tq)4(2@ zc?r<%g=Q>MwU(YxC^!mc$(jv)8EuKb+eiaC&cxiQ(qvkvp`U|Dew|Kd=?zfWAeJ0U{5Y=>Q#R1JfTz} zK*|erRrdlWd?9$HGj0L)58fdcRy;G)wDeKOIEYN&c?^kpnXVEP<}t5OPp8;{g6FN& z>i-`IF;9W5SZ03p?c-v)Q&B{8JZj_XN?)-iybH36nbq*Ru5iEKqM_RH9GiPm>1v({HS;E$>{hOE%1r89!Y8P2UkL0NM~(Iv!b9K=q<3QYgaf`9 z=w)Fo=-z)fF(&+m_OdN53jI)zG*S;pv0uCZ>VMYtdSWH#O8j(Ug?3sF`n*ov_)Z3O z33fY}U3%@BG%ag5RdQR54meS9_9WQ1U zojJgiEfymm3rg6(|4P>p<(5^^k#a?oHc&~{?^xxgI#+Mk^q8eA%2ODfx%;#z^aP%^ z>d+t(TQAbZ#ZJAw92^-z-a&0HE)g&h<)W0!=v!P=kHdjP=jzKo z`*?r2vqxL?#3rlp=ib9cGM?-G6m(M>jvG_`h+e{y5q`8_U3PpBIF0}8?rDYI*UtN) zU+#Am*Aq09uXy3bY8^Z0&&k>+c`#u zbGL%F)(~3P2>ToFc+joz(Mwc! z=su}Qzdf@=r_3ct)kWtxuuBD=hlpHJzHg4Cbq8HaHJZ1vSCfolb&NkXLox{*sOa`q zKM7ezMaJX*u*GQB?6pvfByr^1Rq9M}vb}(Y88f~IT`{IU(}sRkjB`!065zJ_82`X- zDj0^_>!%5Detz$f=nmuk^@^RN(HoAKj&LY&`%A}h10V<@CoE{HNpnzviU;-jNcAYfDY zY*7r7XJI(Z$;Gw4z8=!p=*l<$Dc~b?f726V;%o9(5DA(2r8a!x+BNR^v~wQ`WE zeC=bG(d(Wm?ogic<>U?SSK)AmK|3m`+UicGcFj<)hIB~EPQ|!QZ)wRQ)XJXAs%mqJ z(+?r3PQy8|J&CEb4JDf7D(X(=0=X2iC?Z-N{=XzfOM}AyTVk}m7QP+&bmUF9$(qOi zn034U7tZe0Y;~cF#Am19XN2TP&pT2ac-VADf<~p_&C@XdqO~MFM~AH9226{rMHnzc ztFa?5&y!qE#-r2Di0;M^m~C!xvHx5^)u)tpVTzm^g!!&TQM%0FD)~f_5lMGo3!eZE za+pd6PtQk)Kple#!eXg;Mh7zm4|mYhI}*oo*^~fPg?U-riA*!Sz6WTWuE#us(IZM2 ze-Z3zM)t#@PcK(+^s7EsS>O9waD2$E@=wbDtfMhNQ1E9mGJlAw0?Uai=Oc&1_J_W{ zJ|Mj|*L;n;wXDhj*9|`|GrR6P1Z%N_cX{>s!r!gGih&7mH`Hi8_WL0VL9PIm5hPcR z)BOVCstj0ZvWN6vqB20D0X(g&tS*4Rw)-t(A=9}(#MM+v)D8psfOQ}JZ>eE`Tf+aV z^yoiYEhA(p{-AG?3p$8}a2mVq(8pm?v74J4HBC+a%gN(^wQAm@;p0oGs9-_7w;;jP zm`)s<IhcQJqyaL|b9v6xXGKEb!XqJ~Wyp}rQtcP|bBa8kaXlfF z1qDQVx8$W&HU;B~?yHeoR*OFVXfl@H6vlYKek(^_sYwmXV$nq2yM!ozq+Q@+2}dTd zU(t#dYjVL!0cyO?M0H4E;nq-4*ZbwAY4=%Xj-Q9lF46}A#wfJXqZ#McEHPPQJWVTz zV-+}(wKWxL$;lhvTG&=QB1q&%-K&d>iy;nTn-HV7l9Fa0$P`*!T>KL&>-9mS#0Ri+ ze%52CjZY#FuP%HfxWLFBe2ZH5!LIlB%)9wtDiIpKxT{d)Gn%j|%k31IE+)x~?((I3 z>;$sz;Nd1m3u;s(FFO|Y-XXH&C`DI?t*#@riuvEBwGqMFjZb7r_7FKzyO>+x=0Yl- z;qJPKc)x^Ib7Hs9m}#{RSR?+k=!aMab6;x%lgcF^%4@_kyW=3~h46SY=WFib`6|W< zZ#1S5_I5nwYw5qUc7nmrVP`mKZgHWuZG+oni^WkMkvqI;Q+w=& zOXGJbS%r8acJpHk#2jxFH%QMNO7}sqr=t>1(ptMtS<{kMqkcvKsNngmB&BJIUEg*^Ct^vR zpSX}i(kpdb&l(357Q9K>yA3u&Cr|lE>g-!#S;1!7JkHynVmjtMcA0+vLS;(d8w)g- z0^^M{7s)A)Rp<-_wafQxNSr$wro1lVzlR9JJ(FLC>=K33w*{@xOt=|n*xW<)6)C28 z2TOT^iC}jEQj}~ zz(Jq3`)ho08-X6eA=h%@jp5UUHvu=XSWO2GtJw3e)7z&-=gNaEi`-6kpz&FxLElhD zx!xr*<`xJUX5H$qJ*{( zHf$vQ_uZ8YX8>q|uhADDY$Ok7g$K$SujKEKILp)rBhK1>VOz;`vE;W?mpqZ-%()9PI7*iby&@c_fc7skeUs|pa3H|EJF+_VGq4^8WGFa_TNa&fIHn981P7;wg%)lVCUffPCfD z<_W>7b<<-70jt|~tiajs@y?;HGI)LbX`@HW*yH?&mWQ5$;Uu^*h><+B-EB zZ3WR!fk+PybY}7<1X90|<0tFs+0kr%l;T(Tu5!ENXpF+2Z`l%ze;OwhrG{2h7+B%k z4S#goJlr(sx)2NrM#XgY6OF{%s|i2`bsTZd%LOOm4&K4Jj*jtkWqQw3&()E$o@#xp zEtfDe#NIt!?k*mg1M+?Bvn|0>O!GAH{>|E!8jNN{B&K#p7j&25g$PJRN5_&)_0<&| zS~IUoH=n9Oen{|K#I`Q*FwwErM@Qy&iuhLJd$POwva7X5N)yPBOm7HI6sB>p5!>-h zH|L8x2`iM%My_N7R!EiZ%2Z{;)ld28@6hg<&y{nZJUf3>$OssvJ*ag=siq0pe6tTO zpN=vWB>3@Bqw$NB#VmF*z?Hpyk8St&{InV>D9qgx`@BW&#q!Q;C-;DQ`N??5q+GMH zfYeHa`{t}Op!o+sm!bBWCQJJ0sOW_2y670g{G{)eQ7ZRNhU&xGcyjY{l0GaJScR4L z=Udi2VX$JZ?LFHj)ZrY|L{<4j1ah{_%gfKS%2h{?z77fL$HCN!(%0dco5r)Y0G5l5 z@e=F1^DV-wG;EJn-R!Y&4eRBpwh{jurV;BbBbnLs^HQxqnj4fRIQe}Rx+Fsmb28%5 zd*XUZVL+>@gp)wKo=8uDY?+2e&m}%}zu#w2ENzK7-&&#!oxx#uh)&4_&%EGqJ1$aTsHPR5Zab=H?cXJ8g9;BW zA}7}#^(2vTeJ#Bxz{xS}8t!M-e0Af^Tq7!4Ff$mTAjmNsn0ZuKxjZ9x%3O0hWC{uG z2)eiLVO-rjoW(cO=>-qnU|L>X^hspwM&mFSjPNW8D!&1I60_GEs(mKc?G=6~;Jb6a zi))fbYTTV(U}+$^eQ06Whuq+4^veh;Avk^>PVtc}lk@qvH*W5n+4S0EYtkyH@$tJ$ zeho-gYN)7P#*Ws=#2kJlT!AJ!D8A@S<6dLN*9Wz*A>ap^wB`M5m0p|tr{!_O-M*3HiSB%U%q6f+vAKsB0z?C8o=Z+>%R zhBsG14rqd5yiL+W8)+)#@zwTZ?zd-E+0Yn7fcl>OP8&Pma;Qv= zzCU08i%UiziF5MC{C7Qsu;gX`8@e-Dmy0WPTy(M?vfbltVLiR|Zspj~n?ycFL1c0j>DqG5#7yb}{P-q!9 zH8rG=nBW0}r?SVd2HWXoj=>%aIId@Wl}~h8*rRK~lB`}uO1UI-Pw2%{SBASMctd`P zoM)vNS1!HN2ZPiGM8ZRr0U~+wW$wye&MtW>DBq(SYMEUa^V^AcBpfLUNlBY`UTMf+ zU4JHG#>5I!d(vs5#*1!`tVy>m#FB6DE-r4U!yubWW#lu&JVufdu6_OLme#qmPmZ!y z?it->o%sA^7zDPM`89tc#Q5piA?L9 zafc6ooL_X$UPGfANrTLOf9=LY4l)Y^6H6_~=l_r!lfb<$)}`^sr=iMi1&{U9FL*>}Jk%Z|pEcC}pP4#lR0ghATOn8@0bDoH z)IqG|c(YwqR@}?uX+`onD4%OQH8~1uD&;|9!LmEESQ?63*@oMBm!qJI`<&_Q*sr2} zye7OuknaFdR9XWjjw&;PtbGl%{nUXUfm_MMggwb`N2^5Sc67)NI4WU?w!`0h z?uWec;=U$vqZz5jxbK52HQ>6R&XTX)gJLNA7iUtpig6$QxmZxS|+LhyVB%{^PQ^a}a%f?l=kNE-YhnahP0HyM; z#uVp^O})Saogkni+?}&etOg1wO@N0wJ=1vq7Y$3+qW|7WAK<64dpbmvQj5zdOlI)iD4HpxWg1w7NpV^b?J$Zl|+TygsMB00A?sy+4DL{QG zF?M2<$=c3g1kudqO(5`s7w6{53KCT?D7?JEqR^TQFhPjQP@O7|I0f`hFm^Bk=L_nL zmn_N7aX`q0WLbbGe)GEI=_EXrH|~vrnJ9$aJ)v?M(^gY5YinqI3wT&{orwb>>!pL- z6Z42U!0^m8Y?La4zPXHLk+PeOM0HkFta;MY1>`zr?>h;lg%B zbe3wP$jT}ms9T;i3oKVvGHIA-lr2*t<9bi4OX*)zW3ND7a;-qikOt3}b#-ZR(t4s4 z6YXC!caRY24Nz{=l9|0s10@VPeWkP5D$!d0qJVvBwd5sJej=~6_`WUociiQg$nKr) z>~D6>Qkq`Egvf+?hjaLEGGf*$^P{(8w?1cEm2T|Fr3`C`%9y;zz{rd=8c{jqhIx|ycNk*-XstgoQLVIKa6GzfFWeqvS$5TEk$M@mN z+@@_g7KwJ>U^Lm4vvE_} zuShCU4@%Hn&++U2*#b6Fxo|pkMMOk)Jas>i5cx-RTWDeW)3AG;X=lA6I&bt>mw02! z8P@@5DoY4w?_`o3((#jRg^TXn@k^%0V2thPbMdaVi;yW(i9I8LMVJ>Mjpv$G+~GY=s85J= zxPn~a7N4JU84`m_ea7{5wUrLVEW*#}M)#hWuk?%2DJyCkNoHXCNi3vfj*mVuEzTBu z&FSH-w?fz|w@!w*gzb+xbWz6Es?AOe0aaQ`N*DluYG|}v(-{*H(P>kw=f{s9&vBU1 z1w(Sorwe52AU~?6KZ=Cp_e=lIa_6r<7r5)nnahxd;Gs%`-Y}&ywQ9)g5OO2@)_sEb zJ+&ay0(%aR%F+pC%Ifn}>#f0-KjH`XZv~28YU8_^&1R`V+QGI>ul1Y#MMc{XeHvuX zOfHc<*$7AWxqK(j@0jJqY)-tW+Go$29Juj92;Y-UnYbR$E*d_%a-+GegE4 zM9Ag-;y^AoHa3A7epCkV56RSAA|pbKSe3%{p3fR4*;QS+UaUOu+>tz(2|R!CpC(Rj zZpW*A`4W|KjfLV6()InxTu2}mhS^AAPRO4ICkXkZ{PgL2Jf$2O5z$z^llI<)Czx-xDO;@82{`L z@KGcT1Mv6jA*3Ri|7L77G+_|SE%%<9frPX){IRhy=oc35f4_^ch)8ToibS)+4wY(! zmVO~wrX6Iv0dX#e))){+{hRMykOKg*T+W9WE_gX1R4QdS8yg$Yc#3`h;6SffdI)2K zT%cHPNIxCvXdK^SbP0dOpFzV1L&g+BdL#?!*=}JW;C3nd^|%EOIiVaoan~Bkn1f@l zB{%HC?oAK?s=%^;vB8FYkBPW{4wHTCcsJOZvMI&zfvy3mI7D;(gG)YN6i>i_wu1u` zq!Hvj3``Di=n@kLXW;s19umPze?0ToO^C@0aerqn%J2mBj_Sx*SSa>xiz?H5$ME+w zQUBaDT-sFV7_rq(3+JN~t=1FGw`xCzv4{THV7Jt~OpV?u6-;ndV|THUcFXwMFl)ji z7bO4e^XJb$fs31}f`Wd3&gNab%M14d43~mW41HB7 zqp_wSxkycsF8WhAE&z*GZ2Cpl0kY^54gPd;#-Hzau%~`yBabQv3*}OgUv&N3jZJIB z=eqvWTJ%TX_){afPZ{HmUM-i+Wt|tRaXh3j79F*jyk{6LM4P&MyU1(Spm^J`>#c6( z@lB^~N744>ADObKWoOznXS8%f7vtg@8hFbM*4=}H2iXc(f=}F>91DuY3dWEWKS%Ac zbe!HGcSU#KUCAg4D#^xCPJUhouJKNea3FSJwO0XgCj;?tGPX{S%bBdGay<;@aO%tW z#qP~eM0#s#kOek#@a^8B5<6+$~j|oafe=n9Tc(4i{hu+3u?yk7) zq6J&5u|vXKyQ__uq=q(~RJ6BR-^rE2%NNL;v?)GRm+uz2hdWb9W2(wv+`UG%dH(sX zxq9UF7|HMC2as3V*O)_!RMvrn4X!o=8#Aq~r-~Z=fM|Ol!;t%A#iJl1wP#*48qLz- zcjertL@OdOD51A|xo8WFb>y~m++zv&6p-(~sh5mJuL~1GtvbzP??%3C-2~&cO3VF& zLjSxB#M0$NB$6YzEOb9d8{$6v;r;zpY-{^3maSYXnTahy^vHPHZ$_KBj|@7p+etD& z?9#^DK+Izfhfu^@i#l&h3Stz?h+t&#=Y-~Vu`07$)G;lYca**l&wC$<*||qxNqFy% z&qXQf zDaiWij0}xfOvXQD9d-Nk*gWG9EP7EWFuG)=lO!dw=P} zB`F;Q#fOdeV?#bDA|e?-<#YR*zv%BqCv>}eCdAHTeJSh>I4v6nykcqY zc}eD-3AoVV2=hWs%*0E}A~%lNM>M?S=Pr|&ic8t?#y_UJ1blBaslIx? zL8Okqz%i)X+!bnoq(HOji}t{kS(VjlEU5t7o|v!q0_-`qWF+z74YCH@e+(4Ige8BL z4LVs@8U%dF0wMO#Oqb!Y1LdJyCQsXM@4cFcmkxF!aBtWulgh`tz+ct!vrIvWAC4dC z^krb>np`5BYbmE&?*TTa%RRr~J(N~FU;>;>%01=Qba?`4WE-0tMxlI<#A2d%#q*=O z@ALc_bX)xt`10}-{vjU3bl<-BAyQrL&a~XVy;j#-ta^JI;#;a?4EkN|HBru;8SDa< z6`)BIb=5a8`cT{Xoc?fn@PwKmtS)C>lu3k3fQ&#n+Q~p-e0HViR3I_TwXb?=tXr3o z>eEb&B1t`b+Gpf_ia5uHW986V0#OX6{(%#QaD*M+^Ntl+$aKtcVD!_}40k`?#|p(V zb191za|=Eoa5np)O;lmKvAN#D$Frn*dgkm}_(zmcsE2%WtjJL98CL>&!? zYMw{OmM9(vRp0@twm)eVGp8QjEm0a1(2a@k5W?4Ur(`>6Yp~k-A;}aa+#t~Ea>~`( zteT%y`o|o8w$hQv%J2+P{)z!B5N(^`!h^o~Az-4#SY4sC<5xs;>j7^g0J|fzFYHcb zWV!YGy!VAJ^)@ZBjNIZbgCd*dY6~l-KJ8Mq?<@HT($ggF(ZyQMhLsJQUk4PsgHLnR z3qIxa%E8;SU<=th@=mHZF*qiz0g__KXYp2}*+&)~}E&ISVoqZdg zD3GnABI0EA`p#BLId>wFoGpry6$`)w9eEO9bm&c;d6JVcHVrXzg#k>?GxA5_6udL@ zJsXrB>5D~tofD5v@c+>8ZVG<=d7TQXSp->bBpK)bGdcp<|Ig@Xwz~Jfqa%!U<$#DS z0rR?~gb_dYthf;Q_R@shk_o%WNu&BlBC#b8u&i?8PF6Kk zi!!GHK(6FSGShGV#fp;0S&4gRjlB;T4}AkRiWdU2ZjXvVlaMkW9j@5DU&15>`!=p_N8taf3^4LY%5%ctexQpN7Wl zOwptHV&hpC3v0X$z91)MdLfLms)l#9i|@^v0(}NLrnT=kj@(D?^AKiDXG|aD@ZBgE z=6>@C#}+JZ?VJQGy}|-anwT+Y$SH#GQz`j$+;v@qkkTc2 z4V{?bj+%>N`93^!q14_ilh5t&%I(fAhrRbBp=FRl<1*)eqeVr*R=$oy+rJyV%@$3u zS$AJX;xaMsjxt@pwq<#lki9&I-2h@QX!TLeF%ZhaD#xNqW}(*iAyqctA*F}00}W}M z=1T@HXA=}s3A94J3S<6pE%qC8;j!b~41JtF8p5!2WAP?3V_jg*5BVIxj z7JEpzGez(FMz)?}FK*R+AF3A-lYIiCKu-dN7eoPLp8libi2B>%GIuTUVjZVgxFvHx znb7a7Rt$!m7=(tJ-#>A6{u(~JT7Tt|-Rq?f15~mX00Dvf=t=fxGr<#^x8`0~KJQgr zb{`6wN3;yLQ);5@PTxuR=G(~R<9%KD@)Q>8=;vOW8a1q%VSX+7oNmuTtV1*#9la#I zneWpeeXN7II;bq|Vau+w+F7V+o&k*#A-OTXgTQg16C9Sww!+n#rYM2eyAnP~xLp_& zw1*(%ZY`-<(9ZTQ8hm2Mb%ERc8ZR2o>0KNvg<#BrM-3@<6 zanp6NQgwVj-jb@zT$zvtz zNJvAu<0c7-tkh%u*+FXa@Sfsvlhq z^>rlE=u{LO-jALQ8MXYxs<&6Ju*NDzaAmXZD44sUs<3*uCa*u}ew_6e5h_1y`U0?A z#ix1NN>X+v_B6J(R?_*wP}*C_l%QaK_!Ja+nBZZYkd{$jQ{${mUfP+r`^s>+W`EkY zTDm*~OtL7B8)K8KY*Z1&wpFGm%6-@t?>m83h$^l0oL42!umUEOL+Fl0!2>IqP+%wi zPs9Y#Qh!`m()`l!G`?>Pv-HL3h-^dE!p_$#o<}o_GmmGg2_15gRJPxKoQV`6PLp3- zzqXXrm6lH(l>sNo1X#}+zY$8Z>{a{sbjnE1z_W7Xwb7VZenal4S4w(aF5mpZg9!0p z&UQJz6Y+k1zP(F^Ovj*>^DVX6j<7Ik{%K zW}PHF6XZcEK&l}J0_NtbI{L}YtsJ_;b*E2HpPY|o#vD1ZUC)APN=|9}Z3_nsZ~kHe z@WT+hQ8Nfe(Jv$k`zRHi62O1yF>-&>B78djM`{GAz{7xrKSKufKp_`u=8aiKF1J!= zYvGXx(E!3}YQ8d4$8jwdneK{i_Rlyi%57F#v9rdSF{adI|4C?Z}-NO}$eH>Ui10$vXc zZ0w|Wk?Kxi$-#PGbV4m|AL>U0_5H=}EpDGlB>xOHgi>2T;LRT;6Aw?i<$MLE3)d#x z?(S|-csK$kjS|&FlbpyuL;_w#KtMph@KdINTvzhnowOu9ouQD)&ff|pQjxHKek&YH zD)KMok9aKPU-qxdBA+)fJ^0t@3U@%zTWoSNRkRvbvjc?0K>Uw?WtmX^v(rFQQ4tjr zGc&bn1p%bBmiE}G2mKVXP3Lj15kSEGH%Ea8brc|0xW8qEFi?BLFv)m$+5`-T{ux>< z!&IR3wkqU${s*@?kiD5%^`cZrE9{5L-Rsf=QxrN75SLi}7!&x^^u8>0!iRo4HtW7ui4E6H?( z&K>OgZ*#*TDhq=jzzZo&EzTIu+N=BbhD+VSioP|bDyoM-r{801@si_+d#%iiTLq3b zCuU2k*Iiw6A)bLISI<1@cRcWC+uKHva5p$aL}BWLJz7RwmD#9&X#p5xj^-=l)6<{M z&m=y`|C}206$1%~4Mi`AiAHgzXstc*W%zm7RXP4DT)Kql17QoSq>A#yv92xsd;w&A z20La;uToJfUQYl8xpD)w(lED$1j9gQ28FXIhWdIsAupdADn*$Wz-y6J55 zB@ja^wwgQbYZ{guC1i~#W^9j>Z}lF5xbYT9D$1j*-~;L_gi4jq?G_`D{1US4YBI)f zEc#ge2qwIPwp5iN`TT;=;)*kTTEutbZ@)8E)AaCiMs=i* zjgOC)5EmDquI;T12zU><)oy;t=(%_Lq9}*+J_(g_1uup^9@|ctzV=wAmJ#~*OU7-Q zC!dKs&PiWgSxRT*tIG~vzu3O${TyDIEz6w7eO`Lud7NwMlzkzVib&&jYxI`ukhSQo zQ?^uJ0i{*Ba!*_iBdDLKty$~;=ox5sy=;E+eoeR_6eA~(%cB)K!|ZgpJKtEYgsOGx zqvv17l&k%HAX~7xz}Ui#-oZbVB4z*CcuAqH?-KD)F`>3JsYc%x!`=<;X+>?<_Xeix zMqr4jppz+`9F?;9V_LxCSa_I>j==&j)CtAmsWe8MyD^hGmjZk}m)nyHSr2CoAy4a{ z{PpeE{fe5X*ja~JxS{&;EULoqd|t8g?q60+N%L=@EEA4+TeqC!HB+=CjWsP)dfVkWbWn5*ZD%g+~FD2%P{h= z$5h}&WQz-%@;d2arK{aNf7Z|q_kqqe!@o;0rQL-kvSsOVI7x~_)&65CMW=8K=Et`0 zxR1l&!zax2!+UznYxf;%_v(bi#J&dWWjhCl8aF}`;g|n_W$HsC#I(VeH#B6GbZWBU zaI5M`hA(stWA{LD(2rvc#t;vfpT(3($Sj6qD<_B=3wYg&w#oGm6y;QVqdPiq#?=}UU`(7E-_%N>~Du@~=X{X%h1d(sk1!WRcHPs@T zf}=JPFMnHRo>Zn8V_Oh^isIbB1G`PuxH$U_53)MP`UWcXe70w@f-RJc(!YN);V8&7 znQh9Bz^e*(Icp!^>wcb)WiTD7YIJY6@+odJ`;(?lk%RyiTKHU$`HAqki4>DX6%fe?Jma zUGch`fdw;{E;WZyxyY*Q6YbBVtu&&AXEtH;e0$aIP zO0x+9?DX>V2n?gHUO$7WJ=}dvOArewpV|MQs)?_7CFN(q&eU`W+haw#%$K{rAV$bT z1&}vxon*}h-CyY3yx7RJ!^t!$vh5Go{}f5F{{k^F7M*Wd5pRO7uxWa^=G_ynPsLpC z2XSP6%@m1HfUnAnX@1tHJ>3c|4G)L-@RJaUD$T&*v09`Lqj7mXR_tl<6Ka@z-lAM# z<-UCHd81vZHk~-rIUkR2onOv`E{_EFazOeHK{Z{j>o1%de2=O|cCC}}_z8DHJl}6O*IF;3lcy`O z>=SFWJ7-%gHjghEKB1bFo@q@G5xl@F{ml40kaAgI-;=%<|LG4mXidI`Ul6H_$GDHZ zDVwiw4B=6EYJ&9m+cP2J%c;FJmA^0!(^-;G|E2`+dC#$f*HV{J9+c`7yp+ z@PLVIyuLjHy3q$`p9%3Z1gfp~NipJ>GZ1&Q+4kwlP>!_eBc2+Z;)nq=Mzm*HU6d`U zpG=Zi#0=^KAfsMu>&5s$zYL5kdlO~+lUGh^CC?cUoW3(NIiuT>GQ0=aJBCf`9 z?gct2+1VK;3T%2ank+}ZH`RZ)dql1wA>lZym(lYi_IruKkF@C?;5}}_$fE7tBgO(d zx^o@-wsWL~rVrQ2lgfhCQ^etROAw|!1*^_T{VG&Q)0acEBFapsEnRQ1G1d`esXaRu z0UHnFcc%G9(8Fv*;cxjYgsJDt$mS;)YKjI+VJmGRt^o$swbHc56C8Ws>Ulc*^J>ZK z!=$SJ3i0d6o&G8~^bI8cQ5H=_e@1E)cQ0q9U^$xhql*(~(B^+|VfK93c>kFTBR=zG znEH6(^_%52nO<)ZGJ$Y^y97u6<7XpHgnb^glno4p;a&Y0F9Ex|nY5>(ySry{(EB+R%WdopKhuHG zxyrNr!G%6wFf5|y#&27oj#M(xd9rBDG0`v%qIw)t*Z|u6`rwlhk7@F91U~Kn^T_&8 zEb}v8XNAEc#XA+!vP&;sY5nXz{5M(i!{;aN6QtysdB3IvJyiy&z7>t!aKb-gHY*~+ z>4^7WSW(T1OkAhl!cQObB6`!xC&C(6n>g@fsv6z z+WnI_I&yJ)-b|neL$OcKO~`O+(E9Cv=8+gw_6+5b>@19wU2m()G?ah6 z2}b|&Q$8CqFFaLUA&;bz*A}vU)5dK6=DW4fF1;K>jVrtV&W8WZjAGpSY4x}s-vxPx zZ$sdfO>Qxd6JT8284+p1MTRm;Fw7qoh*exDyP28g|AmJxM~-Em9?{l-Hox;?Uc1xZ zVI-SaYl^bH1`M@ACrO2i`u4GzHhtdgUCp|Zk0(ad_n>kP9HuZL3c<>JUD8F<$REbO z#*CHnR?ymM$rX-vfP$2W>1Dv!Ck_xhcNyBTJ+?4VAT)v?r?zl5KclscF1nig^``*V zcuhh3|0t65%a-NTefj;iu4u*h)zz5)J*Nu~q|MOCnP9%PfdT!UuU>7q>w$>03-9vvPTgj@RsI_;Iy5Acbxz+wWpo zrWiKPsV*Gcu+u;%zd87ieo4OdKeCIa%Ivz&k;wLz<1xX~b3;S*tv&ER@jYtB-s)>t zhH^8N>#^Xd8gE}jvi6Fmu??#a;Z&4BaEtjZH=J4*1+~iN_}2;5!C!*k&NLTev4)kX zgYIK(n%_uqbCP}2-8Lc|v#Zr{BJRiykbsmg4pwFQ9;9Y?0$z z`y)b2YQbWpnb0ChKjM;}h%E>u4V`re8HI`gA()%-$+Auf6uGbAH2Z#Ilt{!kTF&iu zooH~pJT{#o-uQ?IVZPoU!_Tlm1aZ}7-QO$KV}7PA%D`0c771$iY-=h}S&*L9(q_r? z(N;(FRqwPU5@j8RQ||yFFu%VmA#T(H|A!KCfv7?KbBE;S=7vW!$@uzq`=uNmIzko~ zg9FjL4jS>qH_ZRL9tf5{HFvNd&7f()qJP5dB72(;J5QnL+DkdmWmWc&a6_dPnLD6; zwS77aXz^ElE0jGE5;>ej0=F-d-9ATS(NVSg?M>ZWxOj5W%+10(D6#{`*_A!H9t*B* zc0*+Q2*6bq8iE&k%2@s|f}^L6O-+RESh?X$zfB8&zvRH}(rSG%`TELjP^upz?s)Rh zb8q*L)FA#+=$~G*?&)cA-`Dx0To|Dw6O<>S^g}%4VGPUP z$pf!fdLE;kVppgCU&!MB3%=pZAo)~zLwk(wic}?#GsK_W1<+XOLI` z%b`fqpONsq%@>^QNZp=usxO}(YJ%;l5>_MkC7I$d3X ztd_-gHbi36F)$DSfr>^(uVjja41dYIu ztSY9g-?v>F6{4}s%8Q4 zLE9r1Dk?D)u9ApP_^-qgXWzL03p{}t;zZF_u6+;$q!-@Z)%XN`YoOc{#*QccgP2Ag z?K#p|St+T%vk9D~R^|`MFw&8mWiJg1@Co}c^=6;d>zr)}0t$J%6+&FS*pjHMUM|*GQ-}n&HnqK_fmam% znCpYB-g)s%#{8@lSS!@V>k(&Wil@~l^{%{3ZeZSaP@HH|nq%wmk$MV!seqsN?*SgW z)_(u)loVX3qNFrKO8b0?(V}c&AdS{%kn1;0a2zz&Qpid6)ys@hcGtLz}&vHdmD9;C$c}1(3TKb}F;m*jmIrSJ9Q^ zz!8Sc&o)ouX>mlybMC%ARx@oSdi_Mfunm*Vya+j_wFTP~`_5sbe4%!y^dcuH%fWIhEZ{vnRnhCdz-o_=SSs?H-~)M| zH{0L=S7txBEOjDMz9Qnx49#XoPIPbyf66z(&TwAK(e~?${@N7Ds=WqPy>Mw-eS~=0 zH9N;vBJQ?!AEnX)-z9%=7|g`4*IdO`_z0I28qfjg%tB;AnB7Ey=Q;iH{fL_OJBw^U zw*66w3ooUK$~+ch0h%mU+B`(e38<*2uSfaX@Xt4(tjzFfqR5F&HucJM({ahbe539$ z)QaPW;I1>Qqi>2o=f$?*SAA>@Q`&Q44} zCpri~wNkGA+OI?S{b2qEX^-c~!a=-UH}~36uKE5BdP-uf$l54NJ~e$rqOl;Kx-$+G z&fCLr##P$?%zPqHzt#ugCjvYObNA*q4Q(6RHQ#k{^u^QIhR-(8J-qRk)f(?2r~sHQ z?hTyU`$uQo@NiUprX+00en6F_y}OinL9F;Aa-6k7@1#?-5*rgY;a~D&gs@3q9VhB3 zc-KIzt*mV%-NcPRi-=zwudZZ#D}rI^c~$o>R7EjEg!%@85?>#b}bYthn7S zlu2LOVWP%JjJ;zHC54UcqX~EjurG99UQc}|b!4}B=8**{X*@f{^@;%cUUPWOh9$`h z_i7^I^5iqn{bx@exb0oqO0=MG4wn$RHhqEQ z`N-k>9GW-n)k%{8p_Oa=WJL6ShTRQGBVBSkpB#jjc3kI z(biUDyIkzAs#(~`0H3b&s>c)$34_x`pH*)CKu?3<6ZuC``7@kE^OxI)XX7^RT0A_~fm?fOj zME#c`gIBxT+>u;34W9UJvbl8m^+V3BgTjl2e!WX?GL|)64hO_TgNp$q+UI{EWY!;~ zn0B7Xt*?npDc*N5)W@~j=*dx^KU{?f{^n&rJvXAyV(2QL!_$#`!m4v5_f>MJ5XgBF z)>XEbIjJt`(p~9HSTchIAPuIQxItK4Save3ET3MIKtmB2rk;iDm9?yCT(o@UtgRzK zqM)q18*6$jNUb=byn8S{KSI?|VN(>dJgvdOeyYQcap++}vFeSfMFsg_LuKQVff`&) z>7$KK(x});p=b82_&Cyb#mHM%=Qb}8nqhHD|Eda$4w1RS;5Q=Waxb-ysSG<%e*sWp z*TSzZCMccCHa;vW0~u_$7ERW(4nfUIaUv(b-(h>)7s(f6+i2f(8Tdo@{H}16LB?(S$sm z_=&7wQc6GQ!npNpHeZwo-fbqJ$!PhM~I0cH}cv z)YPA$TXPg5j_B%cSVOIp_v;CkB03btE|Q!_yZaO^o>@3~G;?m8bgEdjGwU!l?E3~+if1V8eC5%QhRf z_9RCxC+aOSEKM#~PcNTbXgGt2beG?CL!spGMqRuQCP_zB@sNIv^T^6tA!&4WeS)yE z=EqLV8yJ_`{>!nKL`wi^y5p76fZO>wHG8q~wecCpL-e#tu11KrMiMq-)PeWwfGwXd zYi;J){IGcP;!F~a2kVD|s)U6&1R^PU94<(yi^9y?FxxH8DDPS0Ou^T9olLdI z$#sXx0P>5$&MDDAl{pQQ%CxRupBt9!B*L7AifTHr{jS@^5X9E2=3diqhW@Yhb)BxL znd8kxR24807E_ec9@0NM5YgG$85Zl(GTU;4Edlkr*ALht>`~$Q0S*`9#fCLBzNpTQ z3w@qzm_e%->)YdEuhJj>`Qjf#cKCk}*-7m!`CmU~{0l0czXG=1Zsj#Lm$3sR@8W(H z1Hl3lK*+|ToWIjk%Noc>QAkBq6iF7Kv3XP1{dP9;{D8G3Ic$3pU313yEYXQxlO`q%2GL4*6SA|Z zo*Q`$`x?yECQ@zF}3!bVEI^d_mjRcdvcIcN?+D@>`FS8 z$}gyhOwH6O!&COog85&`dLv8N0_yum-b6ntoRa}=s31Dmr`?E`1>}7 zx5!vpjQ$V!E@DDm2Se;=aY{*koU-jG^36(~c5p1jY2mQ(uBAZxcXH~-k$$_E?Kg`N zK;U&?H?H+jjO?q%Z}x@H3J^y*cf%5W*>Qx9?k;^@8t>JI(w2Z>v&m`Q!G?yC?)O~c z86Va|73)uw-#_GCO6>l5Uo8PSqoMzKgu?l5J>TA|O;0pT9CEPrp2uAGdFXcBEVs5y zhcvdYTGGa}G&h-)78hO}xbTU_{uaIMgtV0KF$iTI<`@{CKDWK2wJUvXazjjnp=j7W zVN8q%AXG$#Z^a?>om@f?CDwCK_d)))wG+vfGaoIs?t^o?AEO709e><0gnv|d{I(k4P0%TWm|h;%K|{kp zmzEGyOMRLNTz)rf8vL=1?`cD*{$9DTQ=M)DXUcY(&qsqBkN(hWPTxtpRvu@b69&ER z^y+)Ndu>4juBh78$FOvswC}Jc9Ne;vEJAE;eJ^}mSo=%j*u{N^%P;U0k#I$ohl{)J zDvl0Bf+KD5o6_whfi7~?@n&CB?rEu}!<245aJSJbysD@1c>GJLHDQ*&<+q;=hF`~W z-*6Th2HS%+Ow1^%1<$m)v=O{3ki^4GTvZ)^lWP;%cN9m$HePSxlv7^x1WI?LVKfe} ziY-qRt6unzi_=#&hk6lgjX7Ru4GZ2@6~kx{ak*w~MIIzLf?=W$SPhPd=L2o;ZE?hXHm2o$7Iv3zl3 z!o$S*S=(Koy`5R>XGsk@QK5mmt48&&Vi+I?n)HFUs@I&7cz%h>W|Z7hPH!X$FUrqu zCU2YEVAw0!kBaygWH}AkaTQ2h<1#zBZ++0wTY;3L?nob~(7{>AkkR&;9^VuCghwJ6 z*~c#2``J|0TI+L#x%`ii=*MXr3o4!WJ;Ugda<43VNlXM?cj&Oic*}BEPSl7j4SIe( zsw1h$zI2PT=;c2kkv9ap-#g1-u*2C0q#i{$?)qx+Dx-cg3vSuMhaqL|MfTVX&b}_YLwK!;7P4=U3ienuE1+GPd)#f+22>;Z`OiyX@ezU-&ze3iS7@>!Kd7cCv?FE_yt%(Lk)mnq!k* zs6BSm@MwMLjpyNpH;fkhNE)gL;&P*7(%f3bBk4(!sx*-sNfhmSN-;u^kxgT|beFH_ z9W4_^NgJ&I|F-9)=PFqt^{m-2rDyQM!NRFug01r8)kXprHN&$)WWu!PkR?^r31X{x zL55845hmbG)J)RUdmM#ety^(6b&stC@6GoUTmq{HfX*wwO1|^PrQp#Ec%lwZoD?XU zcD#{0a?3WW#~;Xga^Hc8N+-;aj=$vpjvBC3xs6tTS_Do4dNsVHF))Q)>+kW`}<`>t5&GtbDSCk6O3;?YHBvdXa9$mFeK5Di zQHNrCZed@($qMC5v7!!s$QM+%Zl_Qy-W8RWqN+|6f@SduW~xn}pdxms+vA``yM5Ge zO-;}psJ~na-HEk4d6p^j5I3f$0}SPx*J{IzI6K&5mXMI}C+_aoY%qAg`0Ga0g}MLpj{nWZalkg3Q&7twG6D^?Nq + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +1.5788 0 0 1.5788 217.31 507.975 e + + +104 536 m +208 600 +320 540 +354.398 522.111 +399.131 505.566 +446.928 544.171 c + +e_y + +3.43377 0 0 3.43377 321.717 544.259 e + +CP +GP + +320 256 m +320 256 l +320 256 l + + +128 256 m +128 448 l + + +128 256 m +320 256 l + +x +y + +256 528 m +278.584 530.034 l + + +256.154 530.461 m +267.689 566.776 l + + +268.117 566.348 m +272.175 529.82 l + + +271.75 533.428 m +267.365 532.958 l +267.835 529.278 l + + +384 272 m +352 304 l +400 352 l +432 320 l +h + + +160 320 m +176 336 l +168 344 l +152 328 l +h + + +192 288 m +208 304 l +216 296 l +200 280 l +h + + + +232 676 m +292 736 l + + +171.483 519.383 m +165.505 562.989 l + +l +BL + + diff --git a/doc/figures/tracking.png b/doc/figures/tracking.png new file mode 100644 index 0000000000000000000000000000000000000000..835100389474517f935f0d1f404f650146e535be GIT binary patch literal 20201 zcmeIac{r7Q`#!o7k|ar{goK2UDI`Oo3?XwYLz0k?DN~dlNs=jwJ49Fe^iEoKS^|NvQ~ivJ z9)YlxpFr5sOhbWpY}A)_;s>?0=4lneCi#E=R%ApI2;2m96=egjq>0|^erA^|6#t}M zReyBpNTVrd)Z&L%J-p3!6TBbqoSRDDsy99o0jgI@q^gBSeas?Euis=T*=sgB zI0yTRnyZf$+Zz=oT&nYJ{94o5x3Yfi-k8jTi)M1{og?ePmc@Q^zsBqH6?|IUDt8^> z4Bs<46O-pRtP`-(Uf8Oz8sF;2e{~mm>=D;qy;2pau&^{TVh*p9VsC!;PQ{0l3BTdq z_pyE9jA-Q&58H44Lx+NcgWDHg<9z~KY-RoY+a0333IU_(v7i3u&1ZW;eJU!;%jKCv zBQ0|+t2W++?6LPE;sZAwBc7eljkWMO99cpWDC05qxxW644<|eMB7>ipNE;&o80B&!rEug#>B)lrwsGBIPJ!pGwO1-Bnz*BAX)EF?z_>4bbgeo z;_do9_Vg+qVkjgX#x-`^D%Kj!(V5=;gIky^Txj+s26EX18tOUM42t z-&-vFrfW4@1*85umUqFCPHu*x^*RT*23VB0Avy#94h zbw;wErE016-4T4oywEmGFO%5!D|Gbw%1ooEsIv=h_a^b$xgojhQyj_+SFc_jid9Jc zU9s3_ws2lGugiC+K2nIYIAC*QIBXaGHbO|`9x*H|j8eI&sfmGdcWCVtX@s+?ziOhs zc5vEr>Kp0LQDNpQ-Gk&s8@d-Ny%rxI-(7e)yxXy_+`F)D@F;s&Ud2K}x8t*R{r00V zjlNMQMhX}H_WLI}Q)?I3nXyvUeUm45VoVVPRGW6bo-*G$NV9Kam z>B!49E&4Dx9bu_3|5+ce{*}%Waf82W_hI_N(9G((&WZVlCm&lCc2g>sPru)%xVhZg z?>(|V-z2GRhI|#Hz+F1RuB306dU?i>HO!=k42O08(wF>;@|>cDwb(j@Y&x!-;nLZb z;}Ry@@Xt52chaZHpM7|0h+0UWN8THpmWgh-ua@RYDu4Fuy+aW* zGxLm>{ES#$*D6xPYobVAmkuN8FioW6dIm6-TS8bRp}IV?1_ zPI_`F_{G%?7QK4K#KIDN>_K=qm#ix`Q}!Iv)-{(P*hnBq-6Z{(UWz<;nkLYs%yTlm-%x-_Fvejz zEkuPUkXwsiUy}W-VGtq0jU3*7Cd9?X#mAq^>KJ<0kuJ&J{NuDHZ;qFIO5hIy_p!hC zBz5$tGJ}e`dc!b%MB{L%CB34Qlxkm4!@HUDH`jtTHwgqc*<|fNZblYf3x}gS2GZJ| zHx3t=2#>U;iV6xkT)CooDTEMl1y_s0Y~QZ75@V$ct{HHVVWQj*IM&;)GT%-Pvm_lmn;Ix-KXKJgZMO;0X zqx5kGnuzKvnF>GzJ17Gm#!?bUNFCo#O?!R&z340Yb8+=zK5Jt0&uK3zDk>WrWL^&X zjDP>VSu^l7qlzy~?O@k6(ad~(Vdfpww{2*4P*UIuHsZ^E3VB_c+D{`7IC?S(2?+c< z5nRv56IpL3xw5_?Tf%kR)6vgQe)^KP_p+g}c}(9Y0=Mx2S7n8Jb%Xf)q^GYuXKdpz zd7wJ-oFlHJwSb5Q8ykj(-dSbLY~wI8sCU&q`rrzSJj~LvAARsC?9+-qCGI!FuA#OA z(_Y?Q5A@R7Si{t!8w`w$q*oWRa!Va1zZQwaW@Tkv8GIk6kuI*kmqMvUc_=VOlry}n zcM6apKAuh7D(K_^g;Yxu69ERAAy;n?52@7F)?T|Jd129_V)1-vvt<_yo}qG8BRVH>-WD*;AXo^ zd`Ypq?w`>fUvKhQ_ihs{R_L*Agl4w5`NajEkp*1v$0IATU#=vgNNk70es*>o^Pr$0 zp2#!8%%Lhz-oAZHXQYXZP<(CheMz6sIN)uf^woqtIoP7lXt&<{7F16qbgORgnp|>w zd%Jc<`>-o;<6RY=$GXqkNB=oFFkE{6EzdO>j(9Ep=my4;*S$``H5W>Hg74g^4@+uT z;$seFvCEDyqoh#w^<5pP2?l&g{`vE#N_eiFF{xmHfv78PgTv!$81Dm z_t8)~0qu;W1}Ff|3q|$+K4dwwpFX&5aA0=1wzd|7=VjcxcW=&8yTy4v%r#LYqa8aE zf71V(B}9dfk54c2)$7+XU8$ia!qRxj%Cd#1AgTq|JZ>K~&5>pgYq2;qGVi0f^nEKf z#=OrCTH1e3Uch9ls;a78*>-axu%3!^fH5Q@@0Vv;@2I=03`f2RW{i(8uVp35kPDv<+)ZnFcxEdwSJ15A>s_Lb^hyT&(wGs(=(*D z3U5u;j@3xV*m844IKy>xbj)%(D=QT;+7C0()GzTxo?)mWaQAb&Dtk^Z`x#|*I23(# z#8}lTy^M{G^WyfN^bS%>i@fZ6#le9l5OM4P%Xe&jEndc$M~?)VXypqD5r-H^+_Mz7 zZq{03yhd3u&9<89dg*%IpFRa%`YlA~-DYQKXeiHF(ld$3S|523!)~(B%XIIKMy2x2 zp$Jzi={cL$hA?6v7-J%=eRZ8cXn12PQh#4p;Nji#lI%H8$uAZD=*KrsRc@!gjb*nH zKc8b%5Q^~deResN3o$QA&g0&Hj(qZt~R#vpmHKye`8rG=2qf z`tae%#$n)a&9CpMjacb}^)1LR&hWZq?TiK+IiEQ@c~0X@3`9eKNyKk)YHF&wx>|GJ zrO2MMobp~T)6-Qm+Rqs&5Z+iLb`J+u|66N)l$DM-H2wMWGa|7i#l_5f_g*R$sTV=C zEvGGd{P@;4`kA%mS*Mb+SFa4x+M14^my(y4cka`m39cDwNk%NA4GR45bqgVvE^>!} zvPdU@KBinPtzmfP!-o%@okqkA>d@-J=~t#XMp-+lZv(Znh!FpH$_U@}j+()J&2CUD&{;-!Iz9Vvn-eJa&I4yp}*do)=(9pfKwU3UN{d7FH?WGsa zeUk-tm7OI0&Yj6J*G^^{Jc^ASpIM&(*o)?=NIz+U_97-Pu&urSP%lI;Q4>KRh66QW($ zxV>ZoYN;ZJj`Dr2Tb?pRH)KC2zkW?j5N(EdXxzeK*yxEJ-L?Q82SU8KZesL_; zNdNlEeoyK7lL_XzWz7<;@@}h>C6+#`KWf+r1XTu-RsMwM>d?^BYAb9Bhaw-0&g3ao zyep}jTYajj4*ch+1*1Pp4-J4WSZkj;sm+0u|!NI}EGf&IQ z{bqkQ6Ge`-t8NkgM=kAYUpXl&ONfa!S>AglSaEHjyv{`?=}KP3qnZH>;PKgUa=;68GzcuU9q~N~*R~25Rnm zC>F0J5?kgmZZwl5gREQRVCd#H`|jO46xiL4#YS0%Ssk%vA-f0>rgw?n9|m0RhllT@ zqmI`yI(M$Ev(vMdCFNM3*F_f>m)4XMqnQj=N9$GR%C7$w5fF&$-5ywN^%;BI(6G7t zA|^EYYK5b8&&Q9t*RPitW#wdNzf(OxP?GvO{Iy5-*8ijC?YaHdvD(VT&(F`%@qW#K zfq}tugBP`VSC)qkDUz=Lwuo&E@|cS-{QC84itm(cj?p-15r0Kx20w*>$ci5nx8D5u z^Ji(v%iiAp?Ae_y(i8*_vBnq{+f*fktPbR(AG6Dv`)%CaB^g8Hr{C?dT>aYhT!7Jv z?b)+uz$~PAoeO_oSFVhQ>^a2jy%-o6`2PL-m6a7N_QBH+{&7%z85(m}f>lY~TpP*A$k-kzaO4PxmUrU} zHn$}%w`5yXB;1Q!_uu^f{rk|64YC>VhOOroZuL1KE(#JiXE?CesbKp3a_)C)1|~og z^p<)Yo>+4E@osx;qpJVZv{#Dv&v=6{x(K>^Gt9~iM3Iv8^h;g2=8n>-O(pF%&@?cKY7wD^&K8AJZLcY5^KuU}(h8h!}cb#-+|+o-&G*ux%xIijb3 z^zh;5j*cUPV|xgMa+{xGh28R!l0%+l9&)`c$;ZzNGkb)0^!A=u)hy{*kF8o8Ug&Yo znA*LiVB78L=PzD-awyufXAed$C?b-Zm1Xvc_6C8_KVUh)c3QXM@#W+ok zF&e{CUBu|(!hM%pQz|4)Nxf5mARBI}t3)3Mf=M}24|dT#>Wnr!ha!Z3%p{^a6BCo9 zl$3dCF9YEX$6X?++D3dQJw0G4PIvf?iHr6rW^!&n@ApimxTW<>R{pMBb*fzcDZB3l z$UwxIckd`W!g+u}TuByrivDY_bp;&l>=Tn8|I_)JVx=V`Ip* zY9G8nZJm7lF}DO&`6QiStg@qj*9Nyk%AvE1OpPP+@Nxf~`PU=${rSA`kvWJ-pKWe;j z-N$Do#@J%H>9Rs}OB8o2u!wWOY7g>j*|#mDt!9+UAMGRogLDPveAWR8gHB#>bCa+w zCvb0x6bg-4En8mSz?~Xq8C`1l+=iW;#3IHYecr%8C9Uo4z)K}=RxYkqgR2p#^jr$% zKJ#M}jt^AR+K}Q+b8t+sMHoX=WIAXFyehd(p>>1UHtXx_ehO8^#nSpaibXQ=@=y}6 zu&`+IG8S}<)z{Yttak+fKc?%&kB)J3d3bvExzy~8&$T`f@?AZquwl@%Y( z@YvB(0(bPwM?%q0+J51aA3sJI2)~<#UioY))Nzcjy6^VoLW5?o`Zi z<>tof;cuG+KmcO3HEp(BCZzubK; zT_Tr2U%r?a8^@aFMGd^m-SXYeZ1sXImZPGIxyK8R%~H}m6%;g zPngJ-gWh~?UWR#21)v@{J>-XJ6i5M6GqYZ&>v&mOTDrNpu@^tzc9ZVp&=cBj8^8-pz$HV8pxKeu z@UXDR%j>qOwC*w-n(P@bU%D*V#HjEbP}JaM%r_}tikIh)KFmamgt41sb<}n7{#rRD zMfIJX`;gT{h_63l;y!)iiP4ZSXCmlQL<-FdGdBk9V4(@Lv$GR8e7M0~mc{$~t!R(5n6&CkWLKJ1-?B_%NFR_v(oT{+s~#XREW1!cn%*vd|>b)Mt&%lmoelFZ^HEg zLFQ0IH$}}0hA30)>}Vo)%n<*B(f%VVU@R9e9xTnE$mYFE%)F3qQd(AaUbqu0hTlAE zBxS!l+!D;?Z<5wlZ&Pqd()MZlC>`}dR=Q^|G7qxp^g5L^4!=$Gt#?C_k!e6Cuw+~a zz4}{>7*%p@fT&VZQc$RV`<8Uocb7I>9l%4<+e{?2f)K@m&}>Vr?{((0<1~^P$EiT3FUh9$a(1|Hk!aS!ZawPk7)Hj?%vP1YT`+l0cSkW-dIsB0J&f>+I|-z~tcQ z*pxF~S}5Vl_eqe4Yw9L2@!PyD=1};Um_arn%Yoh#`AI?-IpCQ(H^$+w{KiKHuMADm(_Cqx~JF%aHzY z8W0IKH`bR@DI!AgT_!elC-b^`K7TeaHBA_GeRTL}R zxVFwH!ZUc7yspGG;n+4iEki@>9}fCMtaR3`sj4$R6@Che?S@Zvkf(r35=I3JRDo}w zolWGRuUnoS@%8mJGc{!<)yWfVn0dNRf)%l}Otr|%18^vDGS?2yp-k74m63_F8T#QO z15%4Qv~4s~_Sg~TP*6~|B*wsM?&V$LQc{=1SbsWlaMDo+{JG5~Klkxeyokj9w*!A3 zp9)U#n^AwkRsVZ&b0)SbDLx+D#3AjZ*9cn9q+#Y4T<+cPtfvv_nr&w@^)r_I{Pp#t zmALEO&kB~LoDy8Cy%{%%RkxR`|RC2HPd0%dSb{3)g(`i}Xg^5Fo%MIi-yV!f3OPK+s zUwS%U;z1U55rB&sR7*9%yEM~3y??JJ5}P>sgka@=m*|Gfj6vWvGc#je=6N(;XfpLK zv1Qm@4#a@C;(k(~f?;H6xm}x$I5KPulS@Nk`jAaWlDm|Y)Lt4&W<{pk)nG|c-&tF~ zwGqcLN6?BnI$Qd(rKM$`)9YKGf)N8!*M<&_kB?Vv%;!aykR?I0V#mjG%KB%(opy|Y z3dXIR&N1rh=s1_GyTyvl$Ta25Cn&Z$I^icD+c`U1X6{dIs0W+6pZ>J?73YTfBHQMK zhX$&-|AwojfeX#@++s;r)$c6Hep*cp1ZJBZuVR#CLi0}(;o_yow6wGjHPTN$KH~>a zG;|0l(ne+DYggq$V*eUH1C0#F0~O7}7(|)L-m*nZ;`a1#SIDS3T3WA)i~9mL*F|E5 zIOs8lWby5xf}I17BT}A)1u*Aar%k&zMDneyxR39>sk4=CCcb_34V4Z9w5?jxF* z#ErTNNR~+%i${Wp*|}?1#`EW)BBo7m>-W-8PkOE<$*B6v%F2#dYAl~iCunj2x z(YvShJEGgYaN_T(O@B?z#u=l--W7dUa!EhuM%#_$XTbJy)BFfIlu|$M@r#Z+t+?1I zqkZSjowx_%jP{PtpPxEZ^AUKrM~1qPh$l{*@UN=Eb~wKHL_3Gc^VA8iKURW*Ja%;zcDrp50h?8I12O--ZN1Xq!jMg0bLG?pPYSPqGTGXT97+h|s` zx&9lhDq;*O|5JSh-m^bXSL9h#K-&E41>m~jj;+}!bl<^d@iP+k#>NIp-pC%%G8 z()Hu@s-6KadnPep_AvhT;iu}J(*Nq{T*;;4&*JCjm*;GaQri=vGOMj`Uu3FrNYl2} z_1vPs`5&npqp4A()y<9Hn|6`n|tNbbLM znOkaGss~(fQvl`2cVeg(5Dydsh6x4wDQY4!f$y*jQJn5=U%wEkx0eK{jm)TbZnyGN zN!Q^IHXQ}ShC>>kz7}0!ar=^T!gs249LT1!v{WvT8>+mH|4el9@#~C~yTj|qyCmSZ zZ$IM9{`jZ6IAp+R09YT+kvH^`?^~H4Zy>GRx^?T(qen?!b~0P3ekddFZ1@m>@bP!o zp*=^5JTT)8I;ot|elg!9Dl(Lt=CQ8KU!E~K5Bc?z-77Eo$~e}@xa@>N>k~!MhH}Zd zV3J9S3_%6Ft`NO_VTN!7ZLBL`lW!u)J_$*EWvNba${q6fH}4oyY^Dqj*8Rn-foHAz}CDLYBxywq{? zc&%r+;ic-!I!4Xv#%xL1rqZ{RxEW}^IbDCo6Wi-l1#KG>NQQ|UQ`f5w_Lux~@-b5W z5{wKR-*4~IPLWq<_{S*g#$-`feWPHi?-U8O1)mx#8ymSsnf>x2yjJZj|AG+%w=3ad_-k zTNzAWb2>opkW zQ{n%+uVQvVZyBj_?MJBSRDadxy}^jA?1ib`@!4fBd5f_fXOkBe7Ebssd;{$vDD`Xc4d^QOYnFcSWrkq?0?%mg?53X@2iWl>Mdtl5`@tIE;PPySsCkx6`s`azRy11hbr43+OrN38Lia(R}CrO1_+I zsC|I7q;F52@PLER6%Z5@#I`|6a?J6|%`gq-+Y`DB;dNnZ7lD_)axS7$b3Xl3?2 z4#&DOKY#k(DHOPjl=;{e-aSdr97NM}1>75tl+4Y|@k*Qlg|@N23ToOCzM$#p>5No= zx#hQiIhd@hc84PDOgM1(_W2BVsD2}44$m$>;E>hR)BE)GtK3M{C))c&2?>b|omAU= z6T}N*_l=?0tDdQMXJ%YyW}zLXY(a%msK`!7oz+qLxj|4a*+N$rNBN!gHFb4NjlGjo zZcdJQmH&+&Z5O(2Enmg8)O&!Ng5FvsF&lM;x3{-9b~U&E_iq4-v9VHA*7%P-&8j0_ z$cFWrG||GZQT=3AmByCNcbZvx567Sa4dh1Mh?pM5A+{sGdjhc+L{34h3hIVO7YM=t z4DvTt<`CUAH8n-or|#V&4!FEVQD-Mv{OT3pzE*5>1*7N}MAZoZ+0!DgOvhY3*u&lr zxIiVlbO}_(XFEv*VF0Q8{QS(!kiZuM2jj=~DCce-G=3+#z5Dk)0}zdtmYaiIn@dw* z0>9DCHDYI_ZZ4-b=DHOlKk}ZgZyVne3tAN}v%c|p90t6QFW*GX&Cj3J)^_SGop#8`os^?rZp>M}MnE33bK+13{2xn9vlIpMv?_*MQD z?W1HZRWv}@ia2q+y6i3thVq!>-E%tP&rgPQ{SCL%=Z1?kXHzi*IIUlbO)SzBA* zx@evM<$Bd>uWSy_qoEFC%L<^y{3L-wNYD^UP?f@j)$Qo1ufON(IH9R=z=h+uXEp@3 zjP@gbHcqE=mAG9NCeFhS2U^JQOQf9PkgIHX9V!V>+}Z^~jBF%5LssD^qR3@2R^Yhi zLSjk3zb~o?u=B-ljL7uCml9`7@$gUG-QJ|!rxCUYzda?+#}bg?+@rOG(KWmtD2i~Gqlzk{0$12A*qmhGfh>C8CyyAXpU`>LI>lto)JY3ee$=S z)=ynA0PDB8-oF`k|2}alZtFIBaB;$`2arb^q7NN&`u2KaH}~+6>+*yB$3HvFgB}a# zJW=}DBb6YeC&QuOGxr=k+^=6IL$qB<_lf+`_NWoTEudZm9U!wMpKd5VbI9`LZjv~k zVe5Cj6Ps(ZkV-9=-m?6hv^g#(w?Y;qP|BMuEMWo1fBmW_na9oCQcLSDx?*f^KMF(P z^_96%1i#UH1o8Ky?w%eV>gk`(CQ2Wv40Cy)DpJ)S0NxlB%+7t%pqtqzqhiDSs*6p8 zVP=TG1_%R(iJtx*VO1X2BSnWzqj(t`kRVa)nwj-jZmx87A7Le%9rOI>vjWxzxh|KS zbCFAam;FA&_%r6bX6Lb|FTX=LCwmGHbBLuvh5>*?ae}0qo15!4(t-r0#X=R~1j3Ar zj;~y~0s=E?Do(9RR;Ny#lH0dC$6#oOr!X_jD-FbLEdwqPj%7I1*3K5b zW2Vr~aAPyy7X<-e4X#fPP_LsIsT-5d0h$&B2+&|Z7r;UBt|;8Ue}AI8Fr%a-misP- zfy$@Xv*&6F{_pK==dZ6aD=JO|b%@6|k*l2R->#jet!r{~``Q`JB0eZe7`lCi*L7y7 z;SBGmzP`%)?|VtRN!Z2l5|?enyRpK5xQ_GLTFyMO8BZ=KD1cfaWRbZgQ#JSfK^kiZ z2f^mZQ0Osu6~Ebn6psU8KKAl@uZJf@fTUlA2cwo z5=p-r4y87^Z$~n!bpUX4c3$+7C%ZKF?>{>m?{ssHm6a7=VS+4CFv}c?<1_6A&1dax z&<B37wj_<`~}Ffag3=iR9DO%Bzm_%C0+ zy#CWy5c)7ZGPGs@mD-c;qaIAORLR=mwbqA2pt+7)>g(+lws^+=ajS6l%a?z~vhq^m z<432aDqRLnBJz8d2N9XV)LN5|kGRXRe37`8S>S$zB~0y9-xH?LT2$baovy>#$g>5z zWNb;?JFx$72#Sh}AB2SgG7p4p8DfrYoQFyw!FvB{PjR}S^X~KeNZdR1Iy4KPKew~B zrM_a@9xi|7c0JX3O#!B;6HG8g03hto>PW!Z$_3B=>(4+g&72WuYL^ zFVJ~V)GlAMQWx*InL>+g{#mLcDIsCy{jo~rw(l;gj3G9SkB&4qH>0rIM$dslDn6IS z${%1H{0=nHw6xpuyB|XTTMqTBUht_vMpmZ}3T2iE^rMFKM*GEQn4EYr8y}o}{0H>O z6FV0Z-b{m1+yrJAbRsCQHv=vjRH3R0x8WbK5lV|vN?X~IffHj=jE8_9MV|rw-0EoOCqde0hhsCL0EY#t{d-YC| zNf6hw^eQpHYJItX_4sbsYNR+0@jwMpFDX2T4jcZcykUsKEThyvL7f8 z^?^$vJCue#SlTJ2jvJ{RGXQ1=NtSe(j|QKxc6Vp;78-fp;Si&d-ZdW1Ag$Xlg96!%aJ6<%&FCFX)Y;BVfOg?$0#17@hy*4VWj(qU?QXehBo zmW1N9VScE#g37`XM*M3O^YrxGsM=iIjN-~EC~$Rj6kfi@t&e#on;$x;Zx6sN1dbfl zqjkPayGQjsYcbZABw3{Ut~|@sjjKw#Ni$dz4t92ph8@M)8T;7S&KKIAojFy_bCmUd zR1{-cTbx-=1>68+3;&@MFSrdpj?Rsa+6Qv`&!utkEjONOi4=BG5@33JCr9>H`O1N7 zh8Lj~1+wspGSGmH-WGYr289j0&IhHedQVwsUJyMiJo4dg()b-YF)>|z{n`41;lJir z@Rh+eFq>Jxi-9r@zMtC-_nTmha`)k8h+8Z}B}^7=qS&}TL0-S^?hd6gS>XozjdE^v z96;x)bx-6*KoIem5*nzEvNKdYj?$-jcJoVQT2iaz-q+1EIHZ%(lv3}}e`B$GasiSj z^5aAJ{Fif^ZF>qX0UnUteV>nxj@EJdYH>68jhpqGE~LfA#)4@A*F#)65~fz@zwT3T zKK8G92Sz-|jKYtku5|h>mi3!2+@?sc&tktS-9vI4v6D0fov6aofJ37Rz66Kni=(Hn z@A2T82FIgN{=+83;RFF7L;-3mkgUD^ zv{!j%<|s;qvY!c8I`b@Bhh52HWpK?k(xi;JNLM&k#LS7L`dC+Z@bS8Tz^PL;!_v|5 zDJlNwmjGj2f%Sw(R8J3TW@2LECGqnwOkYxXFzz%!b<=1g4(tau^I2%1BTH!Q%F4?7 zy1K6H^ZI`YWEyA=t<9A~N0#e@&gaf0uG86`8sa%#;6t{kXfnJ5-HI}Uhb)0n2iN3x zI}W&b-714YuYY~&`Y!oDZ{CI%pgu}F9gYcm_t21=0j?7uxBktI#gU3o)10aCakHN~ zRNc3drs35@5rMnS&2{n~<(RnzjtBy+UMAUS5;fwYCtx;q^ZuC&vA0k$AR|8&3QUh* zr$eQDshBm88>z(@{_Vh}G`yOvoxPPih=q@vFQPj(z|?g;Y2)^75*_9fj-FbYbbGI2^c^9vz09 z;Sf0UN=iyzywJ$%5NEv)c`P0o?G7lCJ9khLz?~p&-XhOHnlYtaTsyGQ#+nIbbosGW zq&O!D9;#laW9(t5tZ}~A7yHAp$>>ZV>eWuXJOk(8cvs%yvL6IhL_%8>nec0b)_!m| zkJ!Q-I@}a5As(NdmUi$;z&3n8&OX!@7}0`m@Gy`*9X`<`jH>u{wURVJ^Qll;+b?%H zdr4D!`%J`b0Va)f*ZHvyG*O_EFzwrlRc^AX*26;K1VAp`MNdD~Q<8_!Sz5||;>%2M z*Ej;PqZn)X*o&-!Nv$7H-@U_XtszT=)&_PGW#Jivt=UIDg*UZYiTk^YjBQ<}N-6x0 zxZ2|h%e{}Nk^Q^2vS5kcL4gsbRm6g?1sPx$51+#xgvaAi;otW!>85eGHCV!|g z{N~LY>y+wR9itqxk_SzO;Gs23zmTOm?Oil~y@dd?8)nWG9kzv1$Bw}n=>6Nwz`z~h zgikw$LU=2jS-H8L4n?>=nHz!ZoSE4$6Ti#k4vY;jv5AR+Ub;T{HAbj$33H#Ukq)zl zJ7^nyof9!@GB0_N8F!Ol4Xn0`OU`<9?0gIzXZSG*iBnExa0kwyGWCLub*KcT2MW}l zwzdIxxsg{PEVMzE%O8xVKr1pCQFw<|TLCXZEKh6mni*1O4+Hld`?cS#p1a^wqOQO< z=t|1U%Fi0lQ>f(noz@nF<=Qi{Q6wGe^shl8dQkl;$m=nuXA#aoY;FZ z1?=jOXP8k|QB_qHA`zzl@3)%O|Kri_g@vu4RZVy*Pp-lB}j_gXmTsc_jQkiyn}|acN_*QZW?jzBen&CtaMRkN5H;bIyD5`Z7~#y2y%P1@nq$0 z8cJB_V1^!A@qvlU%nWc@+?<7Yt97KzWgxKEi7cqi7ndNZixh<=*M~CiCY{-#V+32D zED3_zL7ap!kne)AOp?Z!a{=9 zGsI@N?1$aI@EmX*J}$`91raNnOVIeKLxDp#eC?0!Ra@B=eMQ?D_5kQ=UylUeq`$F-YL{O54FIx@vyv^ z;qB~#8x|+r^ZIpTBO}Dp3DZZ)5G|^z`pHSUlw)k;0wbn;XWGhk0-X37{u38%W?~VFI;iB1$hE00bInJZnd$v z=AYHo%2oZa0Q92}q+qTWg*R1!NM|FR2$e4C@(1({gxC4Pf0F#P6T7@LwKA)YUqBwy30pg_*ez5Q3b8V;6Jz6c1%R3iInk zr$PHDB4WJAHmhMoU7$nEkWaG90-HM7SS(5!~#|uMr^nv&I|eYY<5;wyQ^0{NJJDawuO9EMv~KX>~weVcTaav-dslIh!3LQq99r@ zwvli1LC*KR{pvHzen6IZ!O;#pEfSlzL}iA0Aux1d?fe2aHUK!QXKi+%QP|8g$jgp6j;jm9N zbBGH3t(3~}wqf-`YJZTa8z@V^cZilpwEngiImAS}AEXJGq%#e6*jH=^P{-)m^D@RK zC6V<4Nd14k5RE-23J0fOcQ_=+#lgD@P=*|;mkCG1rQIqb+z&O-^Z}DrL%CHylsobj zlCSrXZmfFdo5+ic&-_XYCo6jOX0tx9#Ze)$qtBHS<_qw;A6`IM@(jfPVCLVLv9*8nS5E2u+Sx*&l4Q^&Q6qxRr z2!m8-?AFXN5{mh-_mA;WCR#Rdt8I3L-8j>Fncx#}ck#JG+qhk~C^P&60O0mhYew|T zagaX1wi+6y)qp7gfn-p_^Z=*}w^KlytPPBc7mTJh3s4A?l(=y@un-itxZ9~jl(E;k z9YF|L=c6ACF1pgy;_Gtl+BJkrjr3A7ih*0?;hIYK1j>CdUdVKZIikxaus(NA9w_gn ztQW-1|E=8vxzSn-r0zZQwEs7nD6o&jeo_u;xUt{6EYPkoKVDgWvZR2rkWCQ)6QZ$?uKIjV*5! zfxT%?kit;>s4wQnHQ0dE)EuZMM4bHFdD79}_p-LUhE6otcb!(A~<7jlOVEJbuXjbRhSWHniN}C<6g<`TbFb2$m3pgI=DAhK^m7 z6eyKowIcT&v4z*2N^3*iZz7xv`mfCM2=m(vwD~fqn3mFIo0mPBP)Pk%o{+jQ*~0=v z(80;GwnT-c2t)m9NZwzJZ` zpSXSvJxAPkasOzTBJWY!;2JpI7-$N(>o^Dm`a9(30xbC{RQ2mHs3C>#K__OiY*!qr5IS>@wtCFFe{|rD~1I_I|I$LeYpuXdGL(no)JF zZx~cqDQ+wcipSsQhA01HPft&8FF3}{QdB#9w?|->R9jH>TON*8TqL!oJD3EW#1Nmf z3BVSnoC(j$nN->fMFra%$yuPMJ_I`fVJs5=hD$!uk%R|R1 zr^Co<4JfmqvhbtvTJc)8;_*1Hgn~UhEPiMH78lJ%78V-_kicv`&Q-~^*1fKcM-B(v z<(SD2zmZ*4m3lAIP;f7K0G%HegQOyqM5y5)8nSMBST>U4OB1-%w^&)SR5sCD7JE;_ zNN|Qt1r6c#+Q|3^W~h#{hb1)I3ch!?v*V_*_VfE@TXal4gW-3 zlWGq4TC=nNZGH>QKm|Mqp_>eA9=>34bd_!#nQg9|hSg=4!qUGQ=_`mTH)?OA)+W~m zn{@$er-8A=V}|yT`#&A;2r&-K)qa}VXYbLS%G!edVVon$uhBxf4@$7(Ub;)pV3$~JoD z_JyM_M|KgEq-#P{mlzV8`H#CfRp6s_C@B_k|#ctmXZ0M+s918kIpdeE?c zcF%ok^6;WYrzoy(=n1i})HL_3v|XZ(!@vnLIE zZ1%w^AjS&dR9{cC+%OS7bd>t`V>IOzdQcFQD0oM*FHw#ofGzgT1NOA9+oC3%PlD<%6Ja4@MpnWrH{TU5m%}T zaM@+Oq}KwguF-}tb!$%2-)j)WK=-PM#0j-+>97)mYGhj+=iQu=oe-V-=)`%c%e8WU zzoH0Vk=va^65mULjS-y0^kCh#ZQGE@VEcx%K!yXvFxn%VnuP6N$wnI%w#S@&j3S$9 zo&5Ba?XZa&e=U8ChyzM_VQvoM7#%%*f)+oD4YbwzZ|WWEuz>yJ$&3?k zZX^bNH~Qe!)m5D&nWHG`<>WN9jk+DB^SaP=3)N2iJ;)8Rb@b?YJbi*h6}jW5sl!$u zkdGT{%dp@=S%;1fdwNMvL&FOfnewN>wuRjkx0HAyk#!nszc?;H$k1STH`QClk-Hh#Lm?16U2!1oRbvHvleR4V3JPKC(--K@hGYa>72me9f~I zX(~1`8x6!-{1S{I+4WO<351tFu*T%>GX7|Y8f2Hv$g~&2Jagy~drBlyaG&6##p7W- z0~XFS;E89Vl+_05V%j{dh)P%I7;tRX26-fukZE=O_{c0toFHZf-zEFkYq*toD& z_rhVuTq&CTU^|j%J>OlT%Xyn2osS>wY5bKKGKr+Wt+Qr?O*MPUHhB9&Q4l3nZH419p z?bQ{vZ(I|Jhd#u2&-+_SsY@R}#84kzD3RErc?(Uc%c+H+0UsHXHDwNt><$O}glG1q zSslerz3j7iu0#Vb%at)l61=;mmDgY}Jg+?o?H!%ApoYdA(1y0Y?mtdg>YuI)mjbbn zGoS8wK4$ncS`>|pyQkoKx!PTjUN5`l-&J*yPBf%c+7>R)3^!_6;CV>tPssxJQSk3F&bV+AkEn^Ygv&R}ooIpytsQ)Q=DE$NF03pcg4vIJP% zRGgk4@3K_j@A18qm4`#~9~U}{*uIbJEAX{lIMnmX9*rk@7c#aA7^C|tC53a}J|SO* zfBk|gUw&~Yf|jU1#I{9aj@;tHrT7TFeQZ`AP+-fTt$ENvw)Xd`z16OGU|zzY;E(?IqA_N{SIU&2j` zg4eH?AocUASNbg%faI1us~E6R4pQ5xgzJQl!GgQeBF5&OlbegDRn5YU4vF+iktsAC z^uqn8W8K8mRM1!M->mtfp2>pCgkL++UW5inJP|-W3hbVFxz{yV@htLUPq2L4>PRBY zub2 zh|o@wnTAfwtB#gFfD|}oKBUn9a+orqtRIIZ3d9p6DKJVVL}GcsrhkP+|H}(CXn5^T m+tT}Y(r4fD{CC2qxcpv~xSXo|rH;5qP*>Gf$vtTm_ + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +1.5788 0 0 1.5788 217.31 507.975 e + + +104 536 m +208 600 +320 540 +354.398 522.111 +399.131 505.566 +446.928 544.171 c + +e_y +PGP + +320 256 m +320 256 l +320 256 l + + +128 256 m +128 448 l + + +128 256 m +320 256 l + +x +y +l + +127.401 549.628 m +163.302 564.548 +209.927 583.664 +259.581 598.241 +278.064 578.107 +278.724 560.944 +318.096 541.702 +352.131 523.518 +381.971 508.598 +409.013 506.733 +422.535 521.187 +429.994 528.647 +446.313 543.567 +461.233 555.223 c + + +3.43377 0 0 3.43377 234.921 624.227 e + +l + +176.458 569.069 m +250.756 592.027 l + + +167.817 566.773 m +147.35 615.654 l + +CP + +253.32 593.723 m +234.713 624.019 l + + +3.43377 0 0 3.43377 321.717 544.259 e + +GP +BL + +384 272 m +352 304 l +400 352 l +432 320 l +h + + +160 320 m +176 336 l +168 344 l +152 328 l +h + + +192 288 m +208 304 l +216 296 l +200 280 l +h + + + +171.483 519.383 m +165.505 562.989 l + + +384 272 m +352 304 l +400 352 l +432 320 l +h + + +160 320 m +176 336 l +168 344 l +152 328 l +h + + +192 288 m +208 304 l +216 296 l +200 280 l +h + + + +171.337 574.494 m +234.092 624.434 l + + +171.483 519.383 m +165.505 562.989 l + + +235.956 625.055 m +255.017 639.765 l + + +236.785 624.227 m +223.319 641.215 l + + +237.368 623.557 m +261.174 631.179 l + +e_\theta + +244.229 631.232 m +3.49671 0 0 -3.49671 243.901 627.751 247.257 626.77 a + + + diff --git a/doc/figures/tracking_base_link.png b/doc/figures/tracking_base_link.png new file mode 100644 index 0000000000000000000000000000000000000000..9fb3df337bf3ae279a764b8eb14e01a37174b5d9 GIT binary patch literal 51732 zcmYg&2RxQx+rG93zTN|Mh*p_wAMEe(w9a&g(pn<2aA=4%gLIp&(@-B_blCP*YXXCnDN?nuutpG4W3P zWZ7&)9{;u1N<&47Xq)hV4;%C1iHMF6sVSW{^iKOR;$>p^l40K$$@6wQNpEm#QIq8p za~tW=^0h};8H=_bw9@#Int$>6D5GcFXc;4|a@(2xN;yNPa)?RURnJ#{lGk>fzjeV= z@_O^{PPdE`ZkKK4Xi67^qvBLKvfGno{i>s*`>yZUb(dMXeavNuav$!6h{%DdaB4#6 z`Sa(Vo}Meq1qOMyC>YzX$o%^^-Cb`V*519jk4mKP@9&?s!ZiPWnqThlTINo8cxS!W z&ly`Iqqk3)$8E4755eU!_SJ zT&DQT3p;u5nb7t?o_#K3g zoMIDW50ea7cQ5~CqP_RuMM4K}S6M3`ihEO*M*i;yq?SP}$3#RbhIZe&D0%qbk7%C< zQW~t^5dPn*|0v~j9I5kgcXwC2TK?b9Wz(gHg-1vKURaI{NQ%PnE1@FddVX4d_i3`~JRE-h1Jql2YrR#jiP^ z+uPem>pY6G8~G9q(sQi-=YH>OPY8A26SyyNHNB$3)!LeajpM`#gLQ$%#wNGvS0YSX zpK9G~swIQA8crRF8GHEe{&oE}<-+#*{+TbRWdR%N5}6mG1R6}=Ii0s{i33xaslfh1D*fw z>tXU=%Ne!g*rO_h-440XYqUw$X=l+K*ybwo?-!1=O$n{Ub~XC`ihGRA)Ju_`uc{wE|MYz<^ z$Y>uG3xCU-f1&sA9`9v=6chdnc$nFNyz@`5loS>^I6DVy{8{w-JwH9Ug&2mjs3pnltgP^ka*mbrm}ftHp4509Fjo?c+)k+6SPzb>+A zE3fJ6yB z$B&n;|N8Fd?OnG2KhOLuXv0<~=-j!~8#i9O2qjBQOq^xFFAWY3B0|5<&X)iG-{T6K zeP5b!{TD zEh{r`jtUE3>q!=lh={=HUteE;^X3f&gXl%RlSJ41ZES5n*Lie5d-kO@{6N6pRU2t0 zd@`TXCPU+_vy0$iB16L~SFYULOR;4t!<2;7IT2~#c>tM2-PF{l z_4a-yN)>L}3k}|HT3aVNVvjoQGy2}v+KR8fgr$$?(qTDqxo4s?exSd9i16_*+c7Of zr{=^hU+iXyRw3@}>)T$P`a z5u?Lw&#GRI$fBINA=hPAJKMR&cXG_kOUXZ=i zey3~G+G1Cu|Jo8(8^^M*s|zIro9y0*2gu8_y)W*}`+g2jnIRY8aQX7o_&7UFctu6U zXj1?q14A9sSWi#S_3PIa=1R@$#XWxvov?j{6lGEG>D&>+Ha0evo11I2{sBAPm3P#mIMLyKgi!G=!2J`tb2%v+u9M^MU|w=!5I*&uCDI*uGn(^ePJ37zQXOB zHx*L^bkb!GGcrnF8&@(iGOC;U_5FQFOkak)F()4F`jR3gzACu7+VlNmbDjv^9IY!? z=DU-Q3kV1p7n`shj4TqUj$n{-&#$gtePTbrBJ0Z$tLp0JreWVRz1-W|o5B;T?)bKl zjx8I}($=JoSf;zWzE3A zrs!xYbD!_;1Qb*q9C(wubGEAd*IX4;qgiANaYl$v`CQbICmP(3g;xxtl)W~-+wGk3 zo4t4M-nD&|X8-!(X|b1Q+5F|Uk!Gi+rVz!NXY`y-@bMLW4*aWi>?SWa_wGG=e0+Rr z>*@-Nik8>>BeKp_IaIk#YkPQjnUG92 zIvjeRx)EI$(W4bZ#+`kzx#AHOw6XXS8$om6fa#r*EM$fYP5zyeu92^ZWZV)^=j zJJZkGU;(U&|9t_F=ZX=R49ivgyK(6SBV*${ZinryO;c0T9_^LXp7-zHx3}lo_N7Z) z9d&wJ`1Wm)agH0kvT_?j^)LfN1s*nFO|DYKJ%X>>fGMHwMZu1E-ye#ItXU-s28#ksnsb*l*@$R`1!{-;Y9fkBUWxd-X~A z=HK7vO-%m$`E!JTObE9if3JCKy}P~r@}GqiVYL)2q|EiJe`b#{3g#l`6rN|1O)}9z2?cu$Zo#|InaEaMQtrS zDvDXgTPXS^86~B-h)A4T2G9f#4^IenM3#5M_D0F8NmsG}b7jYqOxggDqyg0U?qe+y z0z`kVT|1eER6jg4#Bb5`X<)!$v}J8=Es9ls6e0fk^XLA4SHuCKc!h>4)79fpM%=#r zWzpNJJo$0v)o0n<5e1gCbqDTTxd>|F=y<+O2+vbUu?i5>n|P0q5UWQU{Un?}JV6TY zy)W@|r2cnjJl8C1Yje={m8(~Eo`MD$>g#81wM{Rhv^zLCIayl+gb!oOPv6|T_U8|+ ztd6OH!Scr1GNZ&58L#=YjV9_$-I|NfZ{U)1!-YU#$*gTqP>_?yoRI?Z@zx8@xfQ*F1zCT+n^C_Nw;rH*Lr3tmo^ zwT!sgZ`bwIoQDrD0t7w1GLm}o+IX(k{fH6A(Nc?M`+;1eC(|ScjxBgMw6?YaX&nrx z$F{adG1k}DcYl+RrU|zOy9FFh5p#T3e(9xpm(Cymt>-uQ85tU4qx5FWvE&g~0(eN2 z^?>YNy?Uj3k#MpI#ab*fFK-$JzN)GU(P5!piHAOM;^|~|W@ctijxupAO5vNQGAus_ z3k-mdMn^~KzEzu`kSg9HA8rcx`#{=jsxQOB_Y(3h@W!9@bveJ)V%L_pxN{j2lCTy% zL&F~+DFpNou#Ld4+E0k4(PD7`NuX%Z0MVaOis5N_BROG1qH*eac$$w4%$9ZPv z2PnI~K0fBnfsI+R8VHl86&2vT#1s@BE-u`CD(9Zq31fQz?~UB~!%)REsb z%zPCe7dI0if<@x2DzB{k2+)OWh>~zV^VMOb$=Oe}mG|#|#5sjJ(Y5`2|Isrj;E&Sy zhLza$P>;R6|KL7u-ng-Q=V=}Tz7@D**`%aW=WcKP#6zqo`drZ}ud4dBxX2SUjfFc9*;ZFqhm*~pGn~LUy>6NTE|=%B(69)JCD>{ zyR?3>9FTouV?%*;%(WK$5O67{&k;v`kR9nW@Q79U(D3jYfQF`)re+UtH~_~pE5rVo zVJaIN8!@pe5k|L@X=?Gu5{K_-ldsN?_@Sf$NGnc;O{>omJV@wE{vAmOaxKSRWM-wCYaOLsAV5jiVtr zXNEcPV0x;hh2t(K0g3=Aa1X2!SD2Ez1uDa^1i`!Z30gFz^-T}Fw*oG`+?yIjFZ^O; zq|q+Ib#2MU%jUp^(I8#u}@UH78D5#4l2a*(o%BjL4k7(ft#s;x!m87vca;gc^}G znjiyV{nIl(oH%jf?c29DV!og+ApSs%#~%Dpd-NS8ItUpS)$zQtvdv(=-p|j?K`S$Q zxOm~o^&;a^oIeOIpBw$&msu+A+O=!nzA)0>Z@RQOMMZys4uIJ3XW+gyR`MGQ&alTP zpq6T<;TN=P{!JMf7C4zCYyezh;^JE%<7`UYv>>#?Y;31*@to#d`S}?I<8Md|(17*> zsl-u}Bh>VV4}-vUTib;zi9lY0P?VVPOlWCo302tk(xsM3SHQiSH*XeIm~UY%Nk~X= z);c;nYu#t#!o$M>-Qwc(Cw)&yNJt0^^Vl>(!aT)%ooVmGSG;vMEzHd~QM{36-D6wE zLc+p8xG&q<9#8U+P0+lq!}=5S6CIsC*JDC=z^I{`B{nyj@C&*ZMCRm7g4{vzi@I~CF#Q=eW#b6c zoWauw3}A-E#YN4i$Vd%MP0jM=%%@MD$oQ^s8?5~PogwCUR&2Lv9Dv8b@UWzq*e}#n z?wny9Cn{zsL*M&LA5o1ta}7YUfFMdm?uRR7WoKt|>7<)AcrPM}>pK8n z5Y8um(c@vW78(ycd@SiU)q@qCIS@?3Myo7vp<$@IyC@<04wIyajEp})M!tr`aPZ(k zf`H>!+%~qnHrss0T=_y|8DVJ`!tT~z$-@$P>Ce#s7y^X<-k;Nr3TAC% z1MCvIn{cSV5pK_`rmpTnjY}|3VL^ec|DUprmz>+1zds9{d*c0jo|*7nP`ktg1t04@ zI^!B%#cw9)1(gMvK^Er-3!lVq8kdxmS6p0-c!Bx|+y^;RnQK7sA}`f7H76FmHOGzha9%=U($dlZT%uX!b8~XA$cO3aO-xJ@roHX3g8AHp zGkuznm{;dgq(6|bAyFZiQd3hWe}=S-$@s1IJbQM1R5K$!egJsm&mZrD5>9pw4%*PR zpuyQxXCJRCNc-`_NigYrEmJOl5x@%cM0;BHd!a~S1_9(KaPw!yk0O&;FLmsFge>(N%-MfRtyXn@6mGzFExl>kI2>}yj z1Z{%{QXUYp5)49c^&=F2Er}c;3O0*7e^BKTf)-0fPF}pid>o|+P@JjK`71IIux0$| zhhqBf?km_apj5**mxyLoL8(?37KHFeT>Q*V0ylCd949Njv&AkR9*LvRgC;umU%mR1 z*Yjg@P>_y}PHqjfY;dcy<$VmCva%yUoS^2=HUMP-hYOUrz^71aGEMkBU0h;(2)C$9 zTFkjCzl3z|i}BgBXky%b@Zf=_ltqQLnyM;sC0ih5NZhNtt81Lvv=ijm-}tk*SR1%m zmz8Cutxd_M#6e?Jr^v~fBYde{NFoFv=F~5+O5MK`las?AKF9@ar4_t+|DKA9>V1EI z+!*(v;}@Lpz=npjgz!Su`wP(vhgO6s>qAIrDDz2|FThj^nL_gN7W@YXNHpK#`!$bk z;$Ba!fg9%AK&f=RbZPn5cjMgl(NO|-SUM@o&%x39?AZ}n*$32d85y556~jV8Am8js zq2iE7oVoEUPx~U&hb0K)lFUKH zSy?npOwiJ1$B`3|*d~QzHn_RDAvu%ozOlq1B*PNjJ$Y~XB)%w=AiM)|v!B6@=F=l^ ztJ(o=3Bp0z&Ym%sY6O?x?<=*G4pvsKv;CY?$rUj%F~}la9!kUCK0f^!$Eksu+2vI5 z@ZoFd_DFsbcCQ~MtkNnYk)y7bm{y+2jU_Cv4`CIU+tCUP+VY1Xry!{1^w@=kF~!TBgeMuaHqqFt>ox); zF*IZ%6?|CO3W~#SJ~X~KPM$PTQ3(sgafWEaEzX&rf;8Vd<`OYd3SbFgF5+LltR@7t zBGBGT)F^rR3_*R`DkPKxT-s&yalr51-X|-SIUsMJHFoGqRANVc?nN&5f?jiVv_XOK z5_T6b_$`uo^$R@S^@tanLPTP3R%Qo~Z(aehY=OQZYqeOZJ~^h9iZyVMx@2738-n zuR)paC(%TY2JJob12psm8USq}c^sw7@rU^@R0)($z$G#YimK}BV<9dl({LF(M@P3> zvpVmMKiAd?%~%IA#I+dI*21QyEi@}q7*3q11U?yjQnE}S_b6yDXrW7OZfuy`$umh3 zG_QF2locl$yH1dh2OaI~AV@{M{JQAf0PI?Yo#TB3iX5So#k&uroQRHqm2!#6$WDlh zBPuRCp!NAEE6dQ((Bw`b2hy93c#A3rjfBM0PoV^`1sK2MCKoC@NwvP~E|Jz&>W!mGywpWs!~R^%4cS zx%08BE0wz3Pbw?BdwQ%TI22TUeSNVu=%f8cV!(IDrPe|ssP~HQn>M!L2fHQlb#nP1 zF94oZ(6nL#%*$w&(5C0z4Vmn3AnI!P0j~b~2@->)#zbOmcc`LQ}qfIHWvc72}RF|>dG{>;)+R#w)&!IP4bZSCzSq7{b>Iin7rm;$Q1 z7=b;4`d(1|rrCe39~BNA65v2Ph#POk1@UZUdvG^ouhM0a;^QAY&^p8uajwS2^5VrK zMW>BA0WRv@XBFf%IO2dv8od{@7}_bo{maYs^!0s_Oz3pf0B){-*eOX~c8r~U9CX`B z<@pW#7ZszpCO2)rvS8Bp@89bk-tu}rDy)M_3B>=VPa*R)DhV>BEA!$|`6XaB7CHYv zgvOPq-I4c%HGequOwRD47J5Qw&fLn)y&Qi`Yv^vy`v2mm!a_OZRzw;9Ee<{=N-i0h zPiUZGA3|b)KhRVF+3CLI6`{**R$<)<3gjOUu#bX)V|k0fSP2ZP?3SG5HwZI0qBhd^ zzAZqg1>^&2An@GJ&83VaTC^Vzu;|=`%8*+1xMe21bf^`wx zCA7PC?#$K65Nz3MdGVsSq~z`3;EkI%k(;fDDm793a9X#1y|-}Mhf<`Qgi`blot*yW zvP0oY$aVXPcRQ(Y948ccU~70k48D$y?FXncjwT{3Dez6oI1m^W)5#<8>CsTeQ0t&y z{aITpC@SjC))~&?c0f-7fKLO*UmE@Q>rzEmwutvsp?$eiSucdT+$1e2$!mKnU@B|9 z&Klqr9+%6W3lJS}e4x)i7|EY-hWrU2bU6asL%Z34T883c<`!I3#Kz64Kzi;*{c}iL!tY)r$D0{BD5n zVyIsU>E8Pz+fd@r^RpG^INa6UU0GHZqkR#g99C=LCVDz$WkOROUq5{^*gTFUzfCw4|4;tbmZZ&#G3?96FSjmv`v!;m$!6&(@bO z9qsMQ>5YCF=;@JAQcf+exeN2~;L8gXWC_yik70176hOp$W z3S~W-@ClR)JXdiD#{s}4@K}QNq8I&{qwMV74XoE@j|HTORt8Lhn*(qO^zB<-UJhED zOhT%M;$sixf9cMCJio(XLRhjy8~=Fa1Z@sUb=IK4Ar{AO_1oJ>$xBgL`N*R4!|Cy0ZM?RjS>4?jOZU$#m#iz_7r`TtxrH!k-W#AxJZ$B`Ui z^to7ASbC1J0(hWybgp_PoO1wyAw*0B1VrFnZM7&IuB%v3G)>?bp=V??*0_$#>E4IC zLmV(AVMYB2HGDmJ4%(|^e9a}s0ni8N+rrr0b-=z=T6#Qy%)vS7!2@SoTcBR)>RmYa`O?5R_am-^TdH!zAwA=^`?2ep3#n$wo=@bL9(T`euGb>NR?Ap8D9_knh_l7%c6$J>k_ zn-yp~Q4z#oI8sj#4bE+(Qv_@!Gny)=n1h-24jE2S zt+U5+OG>UG_q;uzqdYfK|9*5-PC_DyfRk{L5D(>yMp~DhoB$*H#x|BE`cNTNq8L2> zgJKK}2C%+RwksGjWuRd|TLAj_?^i4yWmzQ^3yX`R2}pP4afV1pI6CR+=zv!3IZLpR z3{^Wb8cZy&0qSF6Y=pDVAwukzr+V>k44Zx4U@6q^KZMl)YlJcV{yvL%IdD&K3io9#e>iQvhk(~&kow& z0&)}MAh{DA?d9otFkA^x9&-PsZ%F2l{ge*w0@5U60KvIW`k+Kpj}~2WZgmTbARr3# zBE1KDX0%UW`OP5aguFSy9^hONg3NQdF*^j+vi1(9*Lfj9J zjQrT>_tYKjMkHDdK#L(#f`5EQ=fYPDSM+n>$rvMRo2gVrNSWN96NRlMb21H4>bvC18$s~IXN~_A6n%B zKp@iyDa&O9A>jcDR?zF0ETi6{5CM$LpgsW&hs69Z3J=$|v1s(2oSO2S>N$MyCPBDj zN{wd%K1X}()Hr&NG@nX)KK@96*dDd%C)21^HVu$@wRb z%h2n~Y56jAjd5 zd;W4T_7M1e|6ZuL{(oG2<+9hS9NtDy>x;h0;^g8Qph5!*r*IJ|pO|fByMO1-o#W%< zT^k4Q++)K;L<4KQIMhShE*b+PQ0&DM_vj$P2I;s=1|tD$XUe*5SLiA;qCxP3VO{ zGlura-FQt`HzqTaZIhptl$nVM^4=utB|H?x(GYA~^-K{o00Hy5gg`myYzQ`_Z-`t< zKj$*{?A_~HYlef?uY$a2*63RWf$YalaF0L1N5X=G(xf~h0#()3U0qzts;X=PC;8?7 zO9|uUAr66A(4+fpo}kJB^}uI)tFhrIPiuF#lUb zY}r6MPA?T;Pn-MkqrboZKYjczClKZrEiEXDDu*EYV;zo9(?<%sO=&VR-bXm9bI{bC zVUH#BL=T^UO!@ic%dp33C$D9}z=X3(!Uh%Kl3XAv0x~E}HivIb%&5YBrM$UJm7$X= z&e`7H+Hu>?A?}_)=fl&jFT)aSd1mfQSg*NS*{ZmEga_}iA4&E0jxjFX@{#thVG08m zh~ZR=Vrz77{rov&&LvO@58NLl&KT}X@;h=)06ayr z6eP$I;F5_3^vc4AX|+sEB~PBL`EaH*3p#Z?H9^DiY^iad)yE-)63tId%)9a&ga%tN zWf6#rOwwBNEG;iXA{Bv<15&A#c5*SvBG43FT!7AqYJ!UizP(l}VRKCljYr%La94{^ zsd2=Ggs2k7F094%^lUe%YbJR6s{ip1Pe}p#qVfB)Wxn*il!iq|#4cuHD?sf+I$Ayc z8{ESc%2_#_l7VKflM&fNSBZ^S;j}$9g(J2NAYNp5uP8si=F&^bl;~eDR7^?Chfq_7 zoj}j}qdqtxdw`=KZpE5KQ4c!|w?XXI(yw2?hZl>%SONrDWosa@M?)eeIIk&An~YLPku4TV{cqWvdiGNLfN}VtN{~ zmYuylguG6NhYCr`h57mLM@2D7>eod@M0BAspE9zHo_|Z^WB1qsEGrsXXc5O1ls|nM z9T^E#Htu_k2(SVR1A~K&jS&R=vpYL_dYELq7hXM(hMhtJ<*v&^#9=TW^gu}Y>eZ{h z@j4VMYsv%TrqACOz*iC!=A78Z|vV4b0(% zW6Em|xU9Wilzj4<6_9@7t^`?W>3TG3*4JQeL(Z|{3m$xZ#Qm}P3h|viWUAgL?cd!| z<-8S2!J=8`m9+U9$XNljZ5zWAjnUUsA7#QK;YpTmGVLX0lybM@xynfs zURqjOSojTl%{S=$rkfoo7En)j%A60toJH7b_ldgu_s>K62)!jZ0_s1@e8^o-O>Gvc zvJJ~_vlFOZ%iawR7rHrU!eMX{!ifQU@b&Z4_}NiPx}GdDwK4_}iF`FhG|%t?4UqTm zyyO@U1c=7S}W`b3-KbC`Y|GMvx0?Ci^OCe3Fl~+HD_|6Q3=_c_1 z%T}xtLb^lmO3oKl1m4S(=tPq|JMF$Tsrf-Qy#%-ZCc#hkNPD4KB%KQG1$Y&grQs!k(ag50^rbk}-7p&t;Y^=t}4O zJX#pz{PDJlrn^UHd(?J8v*%n!*mk;+-kpO2s+04fy!M3t&Dv?oU7>_ti`$77WJZ7y znY;v})sPtWBxP7U1uU8x6q+CBay!6Lx3cG3NDROL@0F)VLeN8Jq^C!F=T1R+Ss6-K z|G+?zRiD!uL}j&KqvQwn?*~GKqeoFe;X}1EUo;#3({F3T=U4$HIr+xi)6Wyax}1bc zh`JRL15+}Db_RNSCRyJn{fz)W-@xnXMl_b^7=1R9HPckz6|)N#t zYW-3cy|aS#5(QsLtc64YD>qO{t+1mx8&Mw<*2zZc_|B?Y92~}a5J09?d3;vZIE=N) zM_>F$b-Nzn`Jx8x6j&KxPBl1z9{laww?9shwWeGkqHZyY)9cG}fq}n58yg(gL z#9VWcqwnIdbzZGmLgVQpVhx-~90og($kAFimGznhb@WG6Qk;?Iwxi-{l|{umXWnpd zhW_}hbD@btnJc(|fP_uySRT2d`@c=dEUbw%yahapnyn@O_wQeTt@sh|Ko@8imnqG7 zVf?}72-@t4Eh_pB^LDZoXcPR!ZwMA=73eEPaF0w)PY<-=A2EtinDyWXnvyLoH-}A6 z>kXpwY1^A>G@I`L^S2D9K*~)v(3*j%srD`>Eh)-h1#%#y5ZGwDJowL`)A{&9qq;ow z8en?Fu2J7>v3<(>1xkX%GZNw-3#w@!>+0g*%B%a&dFW|5wRrIO1vB4Wus!0*<7krc zJ%ss*-R+=wSR^C0i^Obb5}B1<+)-K4@$uu|o|F@G-^Qjhg3`sE8RDNowIbMXstMM5 z6c+fyVaMu}4u*GB1GEf=`~}2=g9a`G77ZcxQO4}@qwb#aU}}ZW2kzh3sHlkWVWeKj zD4<(sb)Iv$3k~Dpix}J(gtEs3m&$oeQlW2=v=ZsEUeN)@_gXn3>&#uIwf7c5iT##q z@U93aajE}PEroX_YX9r260p6d7D1GUE$pg&{@hHcbx%PTguR!K1ZVfxvP_PiYu5VG&8U@L>@=~ze|Ekt3X#*9pEqCn`GygvPwbE@ zJ@*@|n1-(h(t0kZ(-#4EQ;v+jJgQNF<>JaEFbXC&xkM=4PoExNo{B-eKoEo3?$LNrDrfzZOIEa1ca}%R*GsX~}$mj0Mlej$L~j(8Hcl zAtbt=Uy|0|9gE1Cdi9{YXP33DZBn3{D#{ufDnPuggXGLoqP)E75TdGs%Wum)O_gJU z-uUBoiD!?M*u9FsRi#ng>%Ll*4#^1Svxq)aXLWN$Fb6WiTs_LXE^3AQ{?E7Qe z5;d^9Z_!mqxQyuUSwPO^$6a4z;u>-eSC{z6jETWjkToh1yJ@+6uR!w0U~o8zp-5`0<5Dfz+jI{{H@W znktOQ5oQPaf_h2q-#;D!YM8Y}mu6^aNUE|vd=-ZsmuU9;tqn1a|Jc}A>xa~ZwTU&i z{tC{5nV^`TpF7hetv}0IUD2NE#d2z8-{ zEi70>Nm)7Zi9X+vBM;#?r&Si_*&Y4zFMJe#0kt3OPX{UkELu4c;HC!fRCWo61!xVWLj7j5M` z6u_c^jyez@%p%W|n=WMM!p0#gENnS1Q&m=m!gf)D1KXLucZh0NqLRhIP7Wc28v%F{ zrYcQ3J?dJz6 zI>U6R{p%tkWR{FIQxjPj$~i0Uvfe+G>!>1bIP0Yky#8}Vx_!&t{N+Pre{yfJ6x~4$g zUa^TqLlM-V?CH~_D=lL#*y5f^S7<~^Z5Oh+95MO{%>=$MYX_Jjl44_@e_brJT#n31f6qeh#Z`putiBS(Vg#;BTwMMmu$oZ*P>oS?Kx8}kjla6qf(}EXsO))J7!L&sbG2yR&7;R*XV>9* zvO)sREi_V2w1I&EQ{HRyw(h`veg52BUAhigE?){Mq5*6b*;42LGY|+)<)nOJ4dgrg z4TcsHQqnj}LyQanC&p_>B_t-ojPVY4fB3GxG2{zCv6b)d`?|Zo!^4cv6B>fR8Zg>t zXK>PV;)fOu-p~J??IVgEmJjK19Y%4dWL@s;$PXVrU}$L`#XD50{K*pxNzG%h024?& z@%gZYBUo_KUJ<$?=$mgKS&GayU-^q{4yy%{l^-qzb-a@K?k9LXl2UAFNJE*WJrVBc z<6}`{=$Jn54h0hZXL^HZ{Gq#BLs9V&jk%<>G%o0aMhakZ$#_K$+HSBp78mp9UPcjw z#EnbE#>S$4qYOS)-}d;38-+Uo1q#iP`=a(QJWsw2hOIL^M+2Y&Js_YSni!rc#lkj5H0deLrt{GnA}!b@qhW`SVw&dgPI2 z+C4EwOyn8@WndA<155-zyoZj?C520v?rX6h2Mr;9Z&5PW;W%JO9{|0KSK>^4@NF_sB3HA zjqH91AP<$zFm}ex(^FOZRpO5yj6Ob{!^6}5{^`|rZ^TcYOr8@0vYdcGRS2x(rL=^YHDu8`t23GPvFeQV+LLb#D;25faN zUUUfDB2!qW&N}uhjy?PQh0(buPcS|^M|bdG`?&-x=n@vR17S@V>kq$+9lioy31T9` zbmQe$u;LBm4v5NdRZp&|!o?mI_7djPWClgW7Dx@C8d*_@y{*-`^W`S|mA4&hK|Q2T zo`fw8{zx#Ya%ReI^ie)OQlZNIgXuQ((#q&{=t|6Lr8qd)j{lTm+Y|DHE!!?jA-s)j zdFe9wVG)L9yvstDi!E&J4I15^I0l**EUl~v9&_})I{lCQr%OgKZ6Ia$w8J>RvasO2 zy(MdK!tV8vSt~Z=f7l+7e8tP(x&Ck0z~I!W>v!)`ejFPl4%*&?PeXW<|6pXj$M<{b z>94M+CdQ|x*11j=HZ*L8400(YQBx`8(y;S>&}U!c6EiB8+4dr2B#)Sujn}9f(-1dk zAEc!LgOBFx9hIaY*uvpJA()T@r@s*RY+rgWztd5|n*mO*X9s6-N1z-$e--Gvs|QRS zMdXQXAB$W@pQn=8p0Jj1;1+Q&jJxZ=kv0;;I}Nhr-$cPixqX(Ov$bT1=@#{cEAzzy z=U2~Kz}`djc8@LJb`8?*Uzje|Q?l~G!*!B{P9H-Zc>}i{ZUq|RAesw+fuW_#UkqhPNO)a04CQ58R=+ZOTyWktO@@+)$b)LiGSCd?-8Nm4=`P~4;Q)Oq8aPmh z#IJlZv1_>ow>?BO&Iix9d3a!IPg=gBgpdVcpICC+2lQ_i0%$%5zkRP<@ErYN9 z8f0&0_r&f^7C;M_Om}xT-)1GPvb}=?=Eczmtmb7)oQ3ZQARpF%CIrpKy`As$Ln$e^ zLWVG6j&2rwi*5O&JlscE<>SJrI#W(qzZV>z(>ln1#2`0j|54Qpxz^{wCg-=8bzGM| zhZufopokccP-=TDN{Fkrd{-74BZ~hcjT|z!+r4!0>ecOu&I9YU7-9jW$*-<5ftFc= z<-zgCB*yl}vd*mRJ7n%g_y;)G@qD?!a0GW04PL}RXj4W-QDpS#XlP)1AwG~Gp`iHz z$KE*=m1(?F0fIc=Y$e!o(#u)mek4Q0Hh@DdmH`c)PLJXb9Vo{^+gttiB{=)|R#TE= ze0<->T2%JzF$|RBl1S?69GjYOadXWt=AVChl_@i(?0 z7M@qE`NvgK2wSt4g;98Qfk40%3r5Rb zzFe=b_p92ouqo}ccthz-Tt4^Gy4czZ&(*Ds?vR8?!F_oA zLIYebEasS`SVMs-bapp4A3;*Xn+cTl&{ZuY^tOdXMQMq5!yF+(TBa0v==iY^RDwCo z{SNJ28a1OmeAxcdB}F|$PzRdiEckKj?cT5-3)VY(HgMrfu-O6n@Q?-{#a9oGcHi4? zY~0@kh^M{&(n?$fWGEM#L=x4M93u zsapf+qKEm%&r<*N>F225SoF?83~@v46-C#BcLcqvOP9W4M7rbe1%hvLolQhU|M4!m z@1WXnPT`#uCbh1dhq`ZW@f|B)4UeK&TeIzd%q%Rrq4PI6ITveM{CY1P<}W8*=jP^slN5;UM;q1_#*X%7WTYi| zJu0?PVE($?V%xhuz2LrQPk)oF{pa+Jb-w+PodbCsN3NSSX-@SX_4Ks<<1#*VCRMcH z%X@-tkp|vYwh$9jQ?Rw$1aFXvuU}&i#vAZb2?HsxPEkB=1POU>YbyujH}pfknJ2R2 z4LX6`Pgu{40yCpSi?>YpW4;be9}c0=*0whInbf$1Fbx7CmSnUuK3;MD6I;>Q*ylIH z(^-Utjx0~bQn8}*Gx0$zE>SrvtE0fcdQ73e)M9M+E$XbIB8^f~J}$weB8!!>MjQNsYu11bXjR=i#N|dH#B-4 zO197|A#{-_IT?NIm}KInCYKp%L!;B4;!M(eHIfuA9+~;xoXH1A>!!p7(9G64PY@3r zbabQ~w~oRL6B2UY(^JjYH<;+RMO~GG#&<{Ox$u2d^ACh9Uoheo7ti~1Tz;}lOeyG6 z^Pk(TtzG89XJ}M)Of!U9LG^@C*y(M;%>V9v`wqN>0Q5QF_k*sY{sRXNK&$R=xL`B( zA`~t@@LvdO;8Ji9&HOO{WQIs%)RbMU%dM%VHeBs|ABhUZ6f>PjfallLKU_ofz<&fF z1Zfg9<(rS4LAW7x!RUDRf4~L#6N$QKR`!dgHkMxXb#Umn%?NmxJH*6vaqip~SIh5bYg~2=IhoW@tE+z)+^0ay zmVK;7@bEe-Y3PgL;ad0&?eKa5H&_vH>%6lYP|v~g*tR?LxD&wI(YB6lxJana=v3Ca zPGag#`cLu9j2%P?%CLcXmUG-)tf-Tya`1g$_$~5bDfRN43to`}Y9fd^Ty)p3`tJ(d zrPBB} z&0jA1CB|F@bhsYr7cf!gL9i%E;WmK~hMj^JBqG9L*RIXc#u1Pt?XBo1NA>lO$S_^` zbbSxm#Yy$DCqKNCh}lA7x-L@Qyud+CJmlo5sJQD5IVI%<3}Wpw9**m@CYaY@0_!{> z%7E7keZ+8iC?5pi1|Um7xL5ZjN{{OJul_h3{gP4Y2}S*mrZ0&;x6K5D;2}Y~66AXt zc5`esMmo-pev}hQaYE6D=yKlLdg9Mw$tSuAX7*TBR8Wi@{!HK{Ba0jyG!(x-lKPR6 z;ma%L+fkuICf{d{?*g;L#P<23KViC`tp_8MZqIv9oFKA*DN~R%iz}otu zuHW@HT9r&H_tMaODJsQd&Zz&r!5%{Gn`eoq90M=~VLCc(MR>yw+(L8nCCJsfhYRRu3J;AKSAFr%;jdhN>MigW|z5B_|Je0cHJ zliAUCMa9K3qe?C2y*Q1@OTetaR+ z@%<4wmg3^A_@^Nur@qrSGM2rK`smz%z-r%Z#QW+ncU4`=#}fS%U~-D& z4qkJFlSyXt4liLq`a;&4>!@ghA_>pigD3nuh~Pk>IjRim<|s$}TD?TAPv5`*pTPUr zATkIC`wS!70IS~T5@HQl(Zlt>yIwEkJsH3-)w_#w-(K?Tj1m`HiEF&5A|k#lSSHIg zJ|fY?91I+d0RJq^%;qL0c9xdHR;{-&6$M>SLgymU_1?=88Puu?KYz~-OiwOt3bLQ1 zkBdX3UVatwTKA7vArIkX3MaQbG`=t6vxM2*+m~|m*OvR_Z7*N0u8N0;?s(H5FvM6} zxYh3QL6Cngc2&w+L{_zsUX03{(dcD)uNQ5>4~=(RgY@iOK*OKh=ARE zW$`6z23|ZfG*t6|t*H^8Wu>NmJvgW&%0t^l7)PX|`?Zv4Z%@raxnsvgX9~ynJ0DQ~ z{jF$3XVy&lIgm_F?scqvoO%uhRz(B^=I{mvs7}!QuX%b>VwFiq@QRm9LiD8#MQc;a zSNT8xWY0HD7NUvf%)mZM=6?56*uPQdnwOluB``T~9y@lS)^!gNUQ2{Pgz;}= zJwO~{EygO}_+Kl7Cl7|Mda>UQ_VzZHFT0*hg8+fhU^v|9(6D}v-33BFJ9qiDIp$B)s&@bL7Ux4V6nIaFak4UOyHRp+HUDJiB94&Xr~8XUyU zxopXpHIn-JQZtZBd%ai~yEy)mNRQ#>N0ltz#;>UVAO@ zduV8IvwTnTj)1=}7QYJF#pLcngs#j#XWH9%8mc*vz^Nld*VEF{(8~+O8@Cqv&p4&U zrKYN=9Z1_~~brrP()d4VEL`X>Y^zExj5fP-oXvl@>2rvybHJ;;nDoW2n$y}x>YwBL? zV}16v=ykJvz5~TK2#Edt`f?c-Mn*PPRxhl|;j0;FDnCsV*5Z@-|7iN|c&z*O{Y!&F zRD_C1g=A%gBvF*T$=)+5NysW$NhKMP?3GZm_o%F_jLaw_n=-Qfj_ZDY-~M^JU(bDW zU7yeUeU9Tij^pG^xo`4es8^^vf1k_~ZzC_?)deA@{Zf14?!Y<39O4U$yE7IL< z1_nRJRgwRWU3mC}vO8xtDdo8J4r$tAA}}T^BIR4wYK(m)Nn7%h)hplIDZ=>F2KYKn4CUO zgt&l7^&{tZyXGGP28P)McG5fSO`?9?>U4E^?l_0lC2dY?X51sq`6Z!LBN1>PLxYk5Ci3(TB=DeL6f9%_3M z5P+I#s;_|8wF=C2Xcf%)54&1oT!fxZtG=B`Ur%V z(MyRZVRw^bBIk`(C55g8M9&YZ@ zKQ2ua22ePbtg8gN6&6ZL?NE5VN7|2g#Ba0-;TxuX96A<}whwkVw=nMg)m>L-jUdTI zCm*i@>n>)DM%i5thsl-Qn=`~s)?8m~kyxKbLykv|rp~rK7%vq;2vR@$(HvD<8hZM3 zU>U)yic#d2_fx&NillPG1GIv|7dFXV{gkXuo*MW!Z(Z|iKR2>AxV1<&IXU0z>H?Z9 z@m1CJZ88i#nXW9VyKFF+nrh^49VG?{F{H8l378cjB?g*N`7c1kapm%5T<&M3lCrYI za^cgbB!M@_7#QF{;)H+*$`_*BXu~1gGajMV4*)}ukQ{-Pz>Y05#cV?NkyN4k;jN92 zvm34s$^5b6Ae(|0bbduvK3zjgQxh^-JZL{ZQov5a(NJ0VFMJYH;_(-~GxTNO%5UbC zdp|+!kxElX9oU=l;X_E1rM9*E96S`n`;@elfMva{#NnBDmx z5hNocQ!v(eDz)13M*R436|IyBYebK-GW1IJW8>p4L>Yq;5v5*D36JqtO0_aKSK)XG zEj)yHT0hWqfBt!^x2!JbEy(G$-@g~26h;e0MMV|0Pv+wzDUrM6Y>GQ5C|3T;8-Ez; z;^yGmMQXNGd^fcI^Komz-dL}#BFyWBoq4{(NdpBN#vG_H;2(xZ4_pO;y++dy@6{^Q zRXN_@<8^WUDQAHyLQM5L=WUsR&lvq+JOkdo>fa|~jicOLTrWaHfo6cXUH|L#Imf$q zg98I65PM6`WlXSec|X{W;rn7$5R^P+k3A8qdk0VEa zpBr=^o|s6_e;5i`D&iO5_tNdHbtpA5i#{Vx_V<^@#Z{IK_XXqv;*YxM<%!W4wEv(3 zb#5$XN%HukAF4Yba*yikSL1$>Lg#%y?o-#5-2%}!JKJI5Nk?-t;zUz*^)JN>#0Dv_ z;|gb(uAW{=QBhuED+G?UwY80-j)XWBj=8db$C?GF_AoK6O!2zRU5LEM3>~Qxe0LxH zAYRes5|=-wU}T}2CrGAiV0>U{VUcb>lMRUz(u*?GrKnwqT*fAD&;bA(Il+?Ux5Nto z$sny0yhldNkq+}N6r54vJbs)+>9x0dDjw1^iHB3)e~tR#U}os`2^r?&jAIR1@QjE;5Ly zw70RK9}7aX5xXoee+F-?zCN(~b$Nb%S8eTYv`)4Z#!%0INQ5rag8vnObDLFDa^GLj z%0x$J_%X_)YiuEjf(ZsytpnwBrmETwug%@5$~BwoeCB%|`PcX}KYUPSmkqbxse6MS zvRU&=$!btm^5I9(^JHXZBA2*J7_sc%@5_U)gTkEiS*(s=X{hr#w{yG6YSxzcEXb3R zA3T1ch`_k#4nv0tfJ4CBEpQ5=CErcJL-qoe1Lm@-9sv`qhWh$f7ri7zEtEKDPP33jOi@#1d^yP``w9Ym3k3AYNv8xI>{G8d4?s&61=g6dJHjF-HL!A@Kk$h3kJyGSAF{%hTnn48BSO+^a>b= zfDM5$AL7H{?5>pby-G0;-}AlC_d_Xo$%loP`qwYxiErml^5ImWn|0k`W@7_4Yj9CH zLasgz3Iaw1^(Z|vv$DP@08~^4VxppVZFigqlS7Qcspu@K`%&U8_2A-`cdGJnR;l)Q z;u+Ax8PG5T+X9u}sc7JN!-HPinaza%ujGU{EI}l7H}03OuW!@6-x@H@wMNB^jEG$B zUvKXG<%FP8ymz5Ug{1)Xw65yq=my%(KyJ|t-BeT)bX&-t&Ueqn_H1qzTebU_ev z5m}93j>9PplO~V(Dl+s^Rv&vNuW{lwzLYyfSL1|C8SvzhW(La%>HmGuGsF){G+TiY zCd<<>IimHZK*vgTLUMB0t5@l8gPWcGfhw^Y)`Z~*7Kj3G_aSazihCFCcNH6d8$p}c z($bnwO|H^5dXUNegqd1bcR$lUeV)sgC4t+_`~-Z26b5?e$SV{@L_Z(AGK_u%goHd$ zR@Xi`JUlaFZ(?cah+Q$zFnVs?PnI7sutMK5`}3!X zxp~Lw*!V*MrjbhAFWqYG2gpZ9=e@R5Cq#%Pw^w8hi)MmaI?y}POBet6{~s4%o98n8 zz`kcM8MS5Xh;D)#2oK<-?q70Q$dh2euUbpW;=+MQ2T>UtJ3E-V8A&Z!aX6)~y;T#v zYaT58HmN_z!R;H$Zt|7Ex_0(7ph+E&USH{7+rnBv2S>-<(BPA}N-h%fW#LU2u)=H8#VdhJ27d{r}*R@S3(kK-qH&i?Qfy%WbY zbvN+yuZ<0NOhl1JLfJ7(LREHgefndT`h{m1oEf#P@qCt)?wffj!vF+2nuss-|1(C+ z7oh4Og0f;e;pP_{5CG}sG%9AJd%sX;ax+i^uzJ5&9Fh* zMxHe_6=VWF8bA9k!+r1LAN)ua*K~DzQHM$iqT2!8N*L=4@^%xQB~Xx|8A0mOw&cfE zz=c4NAyOY?x1n4@*{gi*8fqB`e;xdXJCmZ01U!D2D0}?$-XC7f`8OWi9*$$WQxZVT zD4u|e&CC*E5DEwh0-+vpRcNA!)*PCL%Q!h`$sb5cCaVeQ1TW0K&-3lf_&JsR^QUxW z;C*EAUb0#f6|2?N-?5;=*NqV=&(Iz=6F}zUgBcuH>ZP;Ea7I4`SYdQ>TN}Y|a{tz|U}WRZ-a=(Fo3#10Hy*5qrK^Qx|*7-)oqJdYqVF~NwVblWqUePNHU6wnEb6ErnH8}sQ9%LtW) zQjK^Oq)zOnh!!_9H-8B}?ATb!Tj~VIrXUjx!{I-D)_>wgtOsdde$|@`^J`JWMsPe2 zS2(8ezX4YfYAdyLT7rwXAv}5^tLdOa@8GdAf84+3&+;Vd+$XMsZ!vTJQ>|@3^&y%l<5V5-%^5>X9M`dUN+a#!ua|3uvv8k;BcKN>A+C zjMm5vP*YU2jJ!gD3(+_OKH#rJC%TZUy9l46h9H4eU{h~DP_~(-p_4~mzD7>ROKM=C zDC>jntE9?}p<@EY`fW&Q9b1-&8bRIMt1%?46N}i90kz@mEKkVQsO_tWl_+#kkB=UM zbHw=X-?E8qXz+?vcZ7!RKBK)OdLIRyx6jU`BuRZn_bpdD8oUHUL&G=W;Y9ih2r^wA z9dL3*PW65C2tFOQ~YJ+mTN|IW-4%h4&Nd=R{#(qjl z)Pah%VE?Bbt!5-mm&QmO_C7uRxZcS59f!RkItD_Fw}KwSXeA^lh$5oV<9LD1ED|OW zLV;`t6aV7Ry(M{hw?_6*GE*5E&M%&x?%zvHgx;xE88=}Hk52}EC#o32|`e$ir z9|grxkz256s+kIZj!UZi5A zQivuMuzDD#Y3IOBP7GpKuIyGF2i~SzM5sXrB%PJGk05wt1|Ta=nN4k~21X{6 z?>sJ>ez^!;t!IocP^!fOgK=|lf%|b~Mc5#U;mQ6!Pq@K=YKIsKVxjZG0~UzKa5m7b zJ60bhAUi6(;)h)o+sTt)o}_WtIS>6ATDgtv_BP{Z;9qoqR&gj)r?283@E_O^C*P*Y-b^P`p3X((@(f22X`tg(XSgUNa5OFof8ZmFMS+b2UNA1I5nXp4*j8 zL_|bbINxsTX*^Hkr(KpUeoW`_!I05Ob*Oy=S;WNL091~|mVu&(5!2QcjE@vnKQ!+ ztB5WL^#I|TEK%#K|8V&|1#%EX_}BPfJrVzi0TBK%zeIYG=l(LK_=WiQ`*pY!{U72k zM^`#1gpd-wqQDY0U%<)S{)eiqnE+r1XN|;LDqg?H5lAO39bIuzQT^y&oDz6}mC2vt za}?Dt-D6^6>_CkUOg^G|;rFdcTFRqcYCF{R^k8#(=;+b=prS*Wgz+@7A_4x0+<{RA z(=}u8&IDnv&;KC4%^Eb}s$HacdM$7EGwk`lMEUQ*o%-jN?&~uJhYuefnQw%5H(+Z4 zr*V}f@lAhb&;3E4QPLt{ql*2C#ss@-tgYb-2tW?^WNT|{GBC+vb@kRf3!~UB^4$ut zR2MgQ@n*&>UwkQN0mK&h``WcY9WHc2F#dz@+6}vSc?uk5w|6~HkKu8G0x~^Vu|1WA zs030-yb3HYe5HOcf+f6MvRv|uNVPqL%QVR0Sg7G}oB*4PcXJBIRMIsdb0xHP zb)~df3m#EKE*8N*bbb3)Jz;M!*kAPCP3WEe-jzXe6a;od`us`Ph$?fqmEL}bWkoSD za9Zn_??=Z7qy&6kq9ssUn<_kdPaY&%-oc0-n;?+f%r%Z2r>pU$c<(UiMGoG)?|VL% zrgH4G;eQ36zJ$WU$d=#g9JFxGq6@;%M!K{o<1tuj|Nc2_orh889boWcF~}^?acXd$ z@;pTyHCHS>a?m^~9(!KmRqA&6$R&TX7PJn3RZ11IhlCJvq^e?-I^+Nz0lsi(eO*$( zI7Ifcz_@=KjeJ0njaPy{_zNZ-vVN&5vH_nfEo<5m`Ai@EuT5bZ5~2K-1c*d9+CWv6 zgostt*7^ghxYasz7g?Q9NfdW|<$_NLfLi!!5N+uc6etPLj|#mt(xSU8Q(ff~mq8Kh zNxuFMIl+#XF6cABHrNhwIf0syo0BuS5>$u;9e5eF%BO6w834YcD@e=EX3xHT_wFV% z?EnY^cS+)7YimbT89zU~WA#k1jJmyWr`+Xy>65}u>RhbD;5P4gAc;qFnH6mh<<|b# zD~Ng9MowZeA}Te^jrU$<@hHULQYIDT&YxKKTCM%op5t5iE;!#=a^Q5Y!O5k>~9cYFzR;_y8z?Ae)EsT#l3I|hiU{< zezfyusicx}*B*GR4}IGE{-e)f0pl*N_qpz$EiT7X#J2FBK5cX325g*+Kj?tTh|iJf zt^#5p=FRxCkaUHMPCZ@YZ}4lS)X*9#Jn_i>|16IH*{+tp0~wbKMDHsVBm+Jgoc0@F zFYo+Q^1Zc{B}^_aHy1WRhbjF4V1|So@ToH9W8hZym(E`Jai7mqoXaTde;ui-2$hc@ zFPt6m-A3$g!;b=~PzwrAS7o@gP64HdEC2NLw9K<(1IW2VU)^%|>zrZ4f%#0IHpGgP z|E6}3ycXMy`qnCJ1~w&c*Bbb*>0%-STon z&M;_`4z51=Q*o;>MF=DEnZC2c#Ik6KFVp)jKK=omH^ak`)(#%(Mtg@#NJ+Yr04GFJ zN`}^rHT=-YNn6KAU#a-P1NZlLcpn+&fV1i?Li)LRS-Lv zK)gP8abjl115XMT$+$F-mxNu^i1)t#Z)jXq#EsT%>0!|wlLlI2KEu*!UnzPVcDkUY z;wBCY{_AjFM^gM_hlUfp&+u@%#KWd>af&`WvAPZ35vy}BtGVLl24=o}iNxA@G%r}O zaNzpBRin$en_sPMLXz+ynI2 zOxGzBvB&^oxMrZxm;^(0Mg6Q6KN|x#P#LHRw~G2PFMM`bz>ni8J9$5eq_T4M!06xP zWR$eigVdUu^KG(#A6#6^T!K|N;J$=09qunBsSj~9^p2h!SN7DajQx+K;&D81LjQPT zIR*Z7tchA?2j+M5^_7&Cz7{7cYS2b#b%n>#!v||@yB_j4eI86b_<^XOJ$u~q^Muu- zboHvbJD1iiI&_x5NJnnMVN-RqT6dYy0CHi@yw(x~lU;$R~e`xWiD5x&zkm zMxwFKzkY5$2s<(*7-9Wzq&(PE?EuTP`bQ_M;UaN(M>lE#}iaI z@C_tcz>FaN$;{N`47;2q<(WO$V5~cVI|a{RWFKo@wa>qZd-gdJ?m4& zw;log(fN{8#T#50{kcmFiQU3i*7aWEOVdOPt*`%Dl4X8z(Onc9Tu6O>4iD2XGY^(; z!8Hbr5(ixnm~(h&#H{im>L_yZ&`usutK@Y!Se#pbi=L)Oam8S zq(#<`|!67rUG|i0_vV?>7TO@3fSZFO`)iBNJ>S69R5;rmu=-&HO;xW*Thm;d!z5UrXCxQr% zTba-m5Xo~jNf2lRZ2S}2C&IVYw%;d3siyOpkur*Si&6q~3J3W50~VD);z+y`6U`b= z5P*Vvh*H7>mW-4XVmzYhkjNcO(b0E+NDzNwApeCvPX&1KI$Q{ZH;>!1Oe&+UAiDJe z|6%Psefoh}t#Qk-=U0u_oVq`f)&1d2X!V6@sfGq010`M1q5Uws|1{W_{lrP72(k(5m0fomDgEKURt43b3)@u-9RhJN_Eo*vD4 z4cl8`Z^0@7ya2k$-zVU5VuR#)A>Z^yl%FOT#nVv`Q2gQ!zk3J2a{Q-gfmTkKo0weU zdLKVwtXcDgROXrA<~nQ!gqX?Y zkOk7cm&g3jC4GHH0!LT_jI@AHKxJLR6jSg*l7)kZM_5S6e)AJI2tf%6F9HH4hm29+ zRJI+!t+%i^72ARhJt!g}XM+)-kx>Pn!P~bD*bRyA31LEYbwbb$aLQi54Y;Gb`|6v+ zLbe)CPPwGgQrQUg7U-~W{>R4HA{sL?GN5E5!X+Ugm*Q2RCk4kawBDWKMT_gg(8rdc zTILTq%xKaxfXot4~_LK|I3l3!VrH zl;m6>TGtLE*W_)|s(*^zzZdE~(|C)mP>?HcAoJTSO!X#v)1 z!pDF+puh&ppPXo_<|hpBr@S>r>Asf!zheB<-OO{G9YSch2`cqlhfF`LKsRrWpWHg( zgn$Xpd^Gg*q>^bGxYL~eHISwdyiu%S$W2d2=lsPESAm0@+g|qgV+lv*X#kE@Jy%&rqh#uyf<^}3TPSgIa~HwW z$CYbncwBGZ+ue&B*f%?PJU^4T~Io|3ej@KbO4O z`ly%}Q5Zv8{2`?MS9TNknJOzIg(8y@|9to`VLSXFdvWzQc!1Q*QiJ7DRJ+l>eQ)2l zbxQh(m9_QNcWzvv^@g77r+IlD%HJG{&n%Loz_&t;w^q*{yT1v0(Ri=tm}AVxIq@2O zMen9*YG&rl@UZmkQX5g!53n4P=jT>H0z+QB_}j(XyH3U0bq-Sk%m6`9L7R^|2V@*? zw7L1|XVY}^TX~^6*Q-)vtMlz|jRgGftu7IR=2xtG%gH%NK+Xj)M{yDH`i-CsD4pu4 z#E>n)8s;|-Kq4mJ3A9-bh!KnK^$@I+s zVgXNze2dE^gGXg$7yft}A>}nt1mw`6H~nR2fYvrlS!)%Y8Ii9njLfz~rM+KAHJcnx? zRx$x<#zGgYMS~I)NeLh04FW8d@lA|xnRRk@m(Vvf{BNYf16Zwt6Rkgf?Sa6{$fRxq zLLickQl<{0YOyvom0~<6EDU3%g8TE2(3R;GJpB04arwrKAF12aIya!|s)^Hrw*m~FTOoP}PXTuspv%JF=khi-r&q%_#2iZFEbu#V20`Y( zoP!0J{*;Fu!JNupPSmKrxLuiLkkc4p15f#<%&|gF)-fRm5_59Yu=7VT@fvGLT}@45 zCP^ATq)$P?y_+|AT>Y9X;m2EGbCh}UX=`WaT}R>{uxUYsz$6tqcpR+ZTOv1v_0&sp z{QPU4AAf)2aCfim$SScSVrBHdl$i{3?Slt@O;5j$iSa-@Oh{mL?2uv&0XZ0`)!O=c z#b3i@HSBxaNyJ5b?#$w@BM3>S90mpia{}wlnXs^i`FT=V**`0P)Y5Z@hN?!?Q{Omr z4~aTYuLHY2sTFe5!{L*yCFarg>y(q2mE+ul2@U=R0PD!gxNsXgJIGCmOAjD7?dwB$ zgq->(@)-Q*eNM#AfQtEqac_oJ`sxSb2N#qhtDx782ZHN(|1nU3vEBV?^6A;xza5Kk z%KsT`#tVCI(}8oESb*N6*>CEh_*#_*FZJqt5P%rKZ4 z85O3(esmIfIevb^u}lS6gokFzn(t(XN9XtN^oI`JF47OVie-Fu4h}8m{GIt;cAgS0 zE|mi%LsJ=V$Uw56oCN+cpsAzNjfH4|W}HMOEzH_Cs)Zbj#LpFM3|+dW@zTb%sH!eR z$jJQ?lUPTtufAG)!BI}MTx{) zgA-^^n4dIgF;wWqEWcX5ByWrBoYmo4xbOP$go;1~OSAv}U zsO%T+6{w}7j}1*bd3-DyK_^-5bja2J4c6eRAhr~o=>fN2pkvY5Otq#T0BWNd5o9>z z7IZE78(=>W#Cm(debNImhh67NZe0`Yd{$KE=7gZmYs$pYvOO|HaRG7@*rOqu%qb}z zw2w9J+-cZY?RX_nRh=Lmb0(|-GZ-u{hD917u6+V5U$iCM3Rl|WU^$ML#Gkm+8@zMO zNpk(&FO%hm!w3J%G0&fVVO$7q0>p)ggL% zqf&-48}S(4i2vgPv_Bh(9eMs-*`Z%#IZH7LbNhAFh65Yf3z$k_^qyVyy!HH5{O2!U z!UGZII&^L*(y>qFE#Gh6(Dw0xiA`?|0yGvRcm#VKlJt zH0I#Aie?yAaj+0_b{6L28+X~x&%ZT90JiMu*_tR@yDp7~Yw%Nqa#9#`F!Xb(sv&CE zPIKR_aO>Qf)EtOoi9ApL<(WT-PDD4?!WTf-K_OyVJu!mlg%vFu3Lw&BP$@`DCAFQb z!HK*GQZY^#vKq#t8fx4ER#Zu;((d9;WiJE~vuz)WQch0dt^tpYXgoyjyS7GkZ*Ez{ z^~G`8>UY=q?(v9-ZvK-)Ne`aJ!CIhKuw4MxBx>WJY6jePW+TJlkhYXI9Fc}x|r&GObF=i^5N9E_3>uW@zyzH440Sy`|763llm%hv2N9`yxG z%O-V!=Ob01jK6kmZ;aA8p}QR1OBB@Kxj7#`yXg1n@_762ynmD9<5&nr?CHQ+PJ{U& zw#%T6=%6nv#+z_~DT;ZhOd_{_As-jlG&Xi&4v%H=YN8m6;laHaq+m(9ZJvj_#JuUL-1mhxSP+X3t&4CiX~CF)?&oI7azf%NC789j60=c2cD- zm=a&jC_;>&U!*ejK3z_22MlxaU@8r})AAgdynLi6&Dd8GsHyHrK#vM;>whmFEGa4q z3U$T)BY;=nV~^2Xy#R&@7r~@&JhU58-^l&XXCM`UV;EBrHMHfD2W%lOhyqNDE6x@= zF*(`WaOmqxxs{DO0YN^6BJXo1rx}Q$=MeZuckOI#;fRF!Fc!@c!8rI3j@|z!<;brP zNm%$Tt@7OVn4oc`H}FoHS_Ek9-*N`AQ}MfP?WiLz!hZyLjIcoY>HOw)_nMxPQZpvn zfyFEYD1wO+z|I34QcNdk$>9`*>%&0l2W}3L7nuG;gPev<(p40szm$~8y);nX`JspG zcocb74O9n1!*~KhQWsKJclB4KOd>=~7>zo~ zJl}Jd)K{wNKl`lcs3??ZxHoz_oCd59POGz=*txYzro(mDFAyr!ov0vUt^oF5-N1)I zz`z@wp5}_^V-%4Th1Z1f@{LFVvgv8D3r})$1#$3;^pynBP2C5zLCNdwWBA#@$U0OJ zU^Z^3n21R3U*}{sB8wV`?|wzG7_2Pjp0~5@7(fO>dGijGeb5J*nu3*5EB^AQ)21l- z<_ZP`Rvq*LL9tO)h>74vfwg+buF!6zCMBUxgT;w12jk*rdM+y~IjJ50kry8MXPf}) z2r-T$*A4=NT8rRcWeis_o%jzXjAG|Qt&H~crpJz*1f&MhirnF`9+yQ0jM8^ukig#ut$qwbj_64Qv_4 z!Zhot*~5=;eXuMD)ztnbv~QtD>sUHwpalcv`9Na?7-5(vSGYHgHNC%m0>qsG_p?$6 zZ`r=0lxiecIsz(2Y%FhRJ(2|8Eb~R-{*Pozv7|;uI`0W=O1lgU_K}IgKn#Qynp%S* zkr7ah6J>%~w@uYJUcyiP?t8}A#!={D1Kv?!qs>qFTZkRukcuMSp`09Y3JO$gaBqKS zs7rm4liafDA+y(`?24|d-YB8P=y7nRflHuY01Xw98Fy4caYpG)(rnMpqN4eURN(N7 zGZ_>6w}B<&yuiq-$uZSCxl{O*UK@&hGHsW-?U2^dEr-AYPuoR6LegBgFt_!Cj*J z0Z`-H$%~wvlbD&aKTX%LMez^In73lGSJh7*yasZWvvWRrO@Du~jGnjVr%v(RX2r#R zB``Gou8J7$(si_5?@JHtR?TK+V%o&QFSbxox5e%p{)%g%3K-UahRaEwlQY!kP-2QB ziP<7ZFC#KE;>WV5k{VFJLD@x5AAuGygYN-}H#mjZ(ikoRn|vG`1QpI{=HtPwZEeK0 z>L3*(xT7J;HYm};3)raZqJw&Q$awrab@Q9f8ojGrLtdIS{m@b=U#t7E+rxn`@WpmzXv6X4i{Ssl}MQQ~!aslT2 zJ#du;Wib%GPp6*M+6}6a?74dhy|pYoWzPcv)a{9~H8t?ZGNCB~#OcwVH110fpPft4cT=;|V#!4lO1E6fO)8 z-kbQqJiiStqFb0Xip|~LlO7Ai5{Nq(#W17;8icn7XFaziHMUcij9FGzD$6~hs1D#E z)nZQXhH7#R$rX74431+L+%&X$x0a9kuKabZ+OaZ1!^hX^6z~>Qn>X-9UU?1c=N0UvxONS8cKN3=A9?CO* z$Md(V=L-fU^^RivmU37}@?Qgff|ceq-oi0nagRz<=HEN zy#9gXXDE1#{LY%iHQw0n4#Y^k3~wXH71aW4BJO?hfqB*J+?+DoUX)!(Op%}#q-xC< zJdsKC0RLyCw*kIDgV+q8^6=8t3jqq2kZ?m?D*hug_~=}7ROd<+8D>TrTL1ennF~~< z2?-H!t;J!%Dv%;fyef_BKA6?)*fG7e6`gaFiI%>mD|2WhX<~opRLNX zQH`ee$|}4OFLG@wg*PMyb~3hZY@VpjfJ6YFa3zA(N1l)!&ui=K#Alxa(g$1G`%A@+ zE4MgfdGFluk@k;k{Q6!7Q+Za$@;zNOrFsxML%9XY2{*T>@|#5or>hqQXe3s5m9Ok5 zEfZpTo#N`|CgJ+`5fQ0?9>(Nj59dTrXQvPr1>JzU8-w|t?(Vdt1!s&b(Xihgtr!$n z=Xg0kKMyjSwJ_(Sj%<8ZP#-ZKKHqo$w^Hhx3#ae@dsy7HhggYo9^C7`DWsAv$#pg9 z>8VH+ZI$cSU*Q~TY01PF{wVoZ6>>t%(h`0feD{STqwT~BvworyKo$TZO<z8QWN4d4Ne$k50-?i}o#ZwDRPCJD( zG#1NU&hoMOiMjBB!ev(baE$zHF}#w>kRah*L*uZFn1DP85PN2J9$5;VguUUZi!gJ9 zD?^&q>KGlI9$gMY+_7d>={XTnAHX`^Z!ToSyyk~l-enmo5f9zDf2WUn5jeAE~)I-FJ2tvCO_-%YImCUF_0B3Z<(~6S#-*7Pvt&&uwilHy%qJ) z-=b_YJG-MaGy?F5VEuDdUOt-N0@hMnJx{Z+CrucuYjf~i%2$2`!4flrtIIh=E*ZsH zP+;Kk7NSi=axx}xHhi3qim5QiKs8car0M2*3Tt*HJWFfG+g=WooVM-Xro-gPYo?FP z$aC*hSJmS~+Y6)T$M_tMzfu5Q`Z<0?OjD`Hc@efREPVStw+?@KMrLoXNcMdc{Dv81 zX$YGyFMuTkJsGN52X%JfX&2%4+>_Kk0r+qN40z6+_9f7Y!-4>Q#R1|psOpX$mG__z zkCje?f{+V-H`5c>kI~Xrn91U)atqrhe2tNhZxduXjJb?JRFv_Sd!gfQT=nD9h8w55 z7ArI{UdBL8R~IXIo4;q5ybQNhjv+U4C)wUAPv}}hHv>}XRYsg<0OjR_lY37Aza{RX zkLIGYL+^QH@O(~OoH2O5Kt)GhL2~8{gNh@Rd*ic|(I~i-4tn0-s%4%X8^iW>8~;bv zVpx6PMn+4AEeUbSyTLO(6QNVhIL3XPE(oB8!;g{L+JA+PzV>F)F&CL7wiyKt#C`;? zg2VZl58oWR^a(WIjRhdafWZPsyT3%=40t;Q23}()?Ij294X7lMNIwRT7iOhRsl{L+ zU|`_rSXi;aD(>0O&8LDruWQZCQBU_mQD|6jvudQC&ve(u)_GX?tQ~nR{cQ$9kj2F( zof*tZ(Uu=?^zaqEUifa*@q|*Rn3g7z>Y9QArmUYn0QLk^4?OFiZm>_v&K?wha2PKJ zK}qmUB~JDjET-9M7(6^S;U=J9yx;Z$XISsY)M1B;B7-vBtBGO%CX*x27o=w?wVUAV z=*gx(%G85OW@YQXh~r4!UP|K&3I$!5ve8F)Epq6uZ!gb^XT25M9$xvKqc7ZZY1ifF zOsXA%pTZaqL9k!DAsf*CE^KU3fLf z`q;S#;!8q;)MZeie!$rgD4)>R7+sB*`cWK<|??|ZQeBAWF850w) zTYUdaG}4oj7R&xM%U-4pW;pOV?9l#07U5wQxh`jSr#wK@4D`Ohvu+)LHtMB?3jYgG z=AENwfKv_ToScTnPGSR4LOJm80eiw=j+S9a$vUrB_Sm2Ej-O1Ir{%FK^P!LYy}2wE zF(ZTGh#OP}%%#IFsG)E(!QIoLf(;0&Q_A91B2(7c}@}1h{npPk2B)uM$gCpvFjB z`6ejYXl!92dOyD#sas7n zhS%pBWrI2A|MK@0ciJ>Fwx7{{a&b$i@sXLLF^I&77TJUDGkM>Iw{x$)12q5ymrXq^ zE?FD$p@y6_O6*q)(*-v?<9ec1T%RC6zb^JmK}#KXgEMZ_rZ2xqmf7{>$?+#oIHjeY zY;XO0SY3T2S(?N7FE-6{xLKMiT$TQ_*IOaAE>`Nq53yrFt*H=JyaI7+QaL88qvfYjIfqUX4 zW-vgkMDb5=JeIFpsFQH(6x_rGVziNW zw34J~z-s=LUeRi?i>ky*9Qf@%MXsMc{u#Lbxk6@S6t$*CXG6KX6 zTEn$=Hw`c5*H0|1J(F0o#&~AwKH5+8P-C;RS;m}knyZr;b{{QYO*(xMuzcD4dcxW? zVWedj!MeFQHli&t*1-(=0+fmCbQ?b z;se!ige5Rc;H<{GjPK72;Tr;916?F$0*DX&`o^>6>$6ysNlw}N@=NeXWyQRxbJ_0o zFNDj>lbPLB(wtZc)=v`>leKv#292EOwgh&|jPo_gUGe48J;`+b$T81CnUM{v*i)ZA zzbgGJ#||Z4a^j`SpRi+Mdu#LDy+ar;KxA8h27^51Uc}o`w60c>)i5$z^ESz2mWlNTdKK5z)UeJx+xXZ4QP;mCL^-B_z?87i zktURK>)BqL@;w@oXRPe)bIk+6jrOaQteOAmj2*Jql*+T5I0z~_*8;^?;@nr_Kh-ue zW#u=yL|VpU+JkEG)Tz&HZD#?=9uki#`rk~od^YdZs~!~CSOH6W?cmX)>s@*dnwpzd zYTD?DhzSY1h8{m6na^mB$%s0e=rm^-eby|C%kaj`Xs67?WkMWCy@xO8zRMg%eR+4s z=wOuxgD;K-MynXX;gR`cg6QwBZX8tsWY4@c?$X_C2-q=w@dA^Utr=VriPe!%j6%xF zD@DY_uwhiq*n5nPPo>1n-$P&ju3NEYs?wP9g|L_i?G$`JYqk6T8V2skXB!?o+D9P1 zP;!0-O7x-dBfE+%K0tdfUw(w+@-6;W25zyK^R&*t;`i@wfkcIZO3`@<<1B4M*U?aA ziF7!T&|-4{Sb!~TjfA26#TmOd&y7D?$d9%q3VV98!646LMz@`~EopbAA@>Q{h9KUC z$p_67y8D=LOQA;p#dBN9D^ z&47EQH{ZY_ikRWNek<&3?Wa6!dUraqi9J?hy4PZA^i+GQQP;%VFsj{z;N%=NHl>%Y z3+C(2uY@v=wHR|7MSJ-dl(S*S$}1Sen~rZN}f~LK4!(t%ul*|(1XpXzPbtY5_Pe6+RRR66*Jg@}f zviMj=Wkjd?o~Yku6=J1NqZDbCf8 z9=k3~@7UoK>)Bw+Ptb7KbPEi4KG8nJ;u! zVG}&cTehg?5rHv_8y4LHl=$ZT8`ejLSknoM>&e$M?r6C}CE3Zv^~CUg(`kWr z^G^W{sj>O6)mZ6sxUN9z%{j5eZx=59@AZv-4go8<;eC;DHG<&)EQws)?hlEu{=gn4 z;B(-SBf}nPEEx3E8|1+0Ma^%Z)x~!w6z^iIy8~46gVI@Xy2M zPspK?Y2pKQbalZDDfCQIH#PkQG<9y|5#(~1O*gf)WcLXQ3lrTD23>}O&+j9T>$!8c zvwXW8^h(84rO}X(Uc)j__RWYTz|gtPmBhvA(*q^)H{Rvi_t9!6)d)u741*&Mk~cU& z5C-2D`a(--{R8kLT)z9(Y#N9?3|1~~7OBHE+6Fkv5_J;x)w7*GJ<%r1Y#$j%=%}oz zF&!v&PF0RrTc4UOszulJ;?pN$qOauj*ebd#)9~bQj^{hY~cAd&3+R#kO&O@{Xz|8N_BSCW& z0_V>uyM;Ng5@u1ynx@P`FR!M`2d8SEeu$*>wv5l$vTO-YjSb&-^oYye#*3e{XU?ms zs@}%UzQPI^w5mkd z_w^4C^G+@K#+*MY^Ms@1)};vrg6=Df2{7u<72Hb^n!qaRqC$Ip>cO0h*B7WP(E^7c z+JQq3hi0G$95H*En(;>awgl5=FtoyX1)8%NGYn}FKawawzG6kFVA1+v1^~fC11p}^J`e%#gl8QrD zefa;l0E{9k+mc5J8xKQ5+MXY0`t?h{d`nC~D6hLkdAmb>w5Uj~c1LRh&|B~l*iP z9YzHPh7AxmuPI_+jqV$AppLl-+-D5Y`%+$AXOawx>$GKdsq~sNtW}l}jd!{ml&A2j=3ha zXBp(5(R!_@z%{?zwLQ!%bma=3eViiyUEDz;dHXs-ePxKnhcAK)cMy775$voHgS<3 zu~t8ZgNRW;YOJSjkvBp2DUO=g^7ZAnnmpI9FAG@KQSdVK^>W7S^(Ke?ArX~MT3Beo z2Dj5w--D8qzrFI5@?Sj!Nf`1PN&!%`fDz%G$ep?rgk8eJKYxb&vwL*xQZvF z7)vfm;XK+9c6Z8gYl*j|RJ`w%KyZpuXRb}66B~gu{`(YacV{G~$B*YzW1U^b4!aKT zv(d7dwUyy1(4cq>Z*cHxi_v*oW?Mn=&f*FpXJ4 z!iKdUX!;x>??oG^^YP%mMpzgK3onoDK7(@SO-#(prJPW*oF`E_0^q!`Sz|fY$vmIHOGEy zqYA&K82qiCASi%3cCf%9Hq|QibI^VJA2$|Iiu~#w9bZegIDitX>WF|-^sZ7V+1b^mSOO=OTZ!tS_Z*=@T$R1?kkw%K6>P& zTb$dtJd^H2#tb~1pCiqA)2(ePOo>!d^eznt22}T5pgpW^tC3!ww&~g}qigSfnv=5^ z&kjfV4ncALZ`0-LBU9PQtIxR5+Ediwg879TfS!zBgBPar^xi+c9unBsqjtlBl~ZSeWR~iTT-+CzwVKgeo`uj%~cta++x>JTIL05AGCTFxGDc4o39!^vQ_Xtxqtxk-7Y6 z;I4L?H1(;C^}*#tncY}cPsjc3z009iR)bQPvtVis4qoI)!)mrD-into=K((?n+-e- zA2h-82e{Va1T%r~O-pN@^tjKAKoAtgu=>D>wMV{(V8S;as{H!I36J%q_R`gq6j|!z zP!ekvJOOBjVb?I%p86~=Z)zN72&`n(k7HxM)!$T?-;#3RW5AFL@kZ6a-~tv#{&zPC zgo+k)upMdTa}^Hje`+sAMp-USm@%AdzBTp)cclLV_yW_wev*<9&ZIuwtt1rPmOgl} zL)$oM_n`l2@WABcpm_mX#f-^`K)4wc6x4X)S>fVTMBcr*@)n5&EUDS7p|&&hyzcIS z6E&m^Dmh^3$Y;-#!46(sh>Dj!C8*JrjfK$3p_`|<-`7CEf9#^CI2u1FMmad@h^@nA ztYdhAT?dc$&de3D3$R%Z`t93-l?@G>wV~TJVkbp96pkAN*kuu%UL-AKFMq1~fCGP5 zhI5?K#Zp+-HQ(;rIttAg!qasvEn&>!QJbn869|PgaItA@oJvn6bzfBd{NmKQQf%|T z1%a=T*!Rd0Ls`j2K5}xn zUV(OXy+8>eC?NsT(lDxfwYpONXBjAQ#(=4zcLK&_+KFu&M|@yB4vySNy`c|jvirY* z9^Q)SBZn+tpc7Se=kphtiCcu5YPz~^Xvi^gfx}ZzUtdsNv_r#GHsB(D?3w*}L`r_8 zIJ?anpPBW<3Hz13@p4ARR!#$BBReRQF&iQ zUIq&n)e!FgO;b}fy))`5bzPY`4zc}rBk*&t%8&E0!ud1J3Fhp&2Y}}N3+w185HQEJTt*R=A3x-N+lkm7 zYurg7&?3ck<=NFuW&7Xw;qKXyX14HKn{@X?#fsFT8}2}+GI~e_g|F~OK+>Ng$6xS8 zM@DjUcbsryAP{c(BaOQ)PcL`rT|y8!BFN#|@bYbQ%#xAEDN>(r_1<0aH%56;_wvj7!fr_H4E8_^S+YuDy}pq%D*PT40$NkH?B z&$yGc|IndMllGJm=NX>MGLvNczA>Lq!Oz$YA}`$8n6d8-tA)BSRapCcg^D?R{+8?X z+0~WdaPcZqCTu_#BnL~VuI|`WS6*M?pU5ItU7g6S2V}M_u}H;nos`42pCjP(nuKTU2bn8aPGFAp`lmSMXG(|9gf)thBK{`@Y znn)2TN>RAiN)u@!C`Ba{X(L6X1k^#Lmw+IGpj3eoq(ndnx$9)^AMt(9bI%Vx2FS_D zTlTy6+H0>(d>;)kq7~gtqg$1Iehx-Brc9Y69=Q3AFIV4?xQhYM6f{Wz)~^-Us^}JWv%z2J=$IQt5Hy^VH5?)3IH*RU$%?VYMF6zUp?8-Csz3QI%?@8#-b}5U zN9UbWH3b-#p4@^0WjNkA2GDZ259Q%Nu^$Xvp_?Vi8$NzAQoS6%`9mk`VYGp#E6p|8 zN=x5-6A@R#c>bZI_y;A?%})WRPO};Kk1!#~(SZ0r$e1e?-?ky(%X;|UVG(iPFHqKs zhi)g33Xdhu8INs@!Hhxs;{OVF`a`t2n+TAkV zEPn<4?fDhumU~?2tE(eWS)}-irsu%zge~cS{4AXQ8cv2uR<_p7t?Yt=VPWhWZ)LK5 zkZe1RMiz$#CngrVY^t*iEi^RjQpnmnI7~QrZ2?cFivbw}lmjJq1v0q=>1LQ7W{{p; zDjSYB#s_QUAPiu3zcXHBu)PcD7m02QtuX z!2jFF_t1<&kw7`~?c8ZDE?Nz`Jis2#&h;eN@Urfbso-CN&O(f^9h5a|gxT4h8OwY2 z30*MUuwNgC3M#NsX9+J4&IDqEtMosMT__a2m}z87n}R)WL<{PCbq=o>BNPBJ%ZIW z9T=EU@pT(*ygp6L4n>@>C<T zNU3miIL1W|LUNoCOOA^2yKn(;+I@7DoaSdXgAS4*X@B65Gg(`iPS`w>8TY}Ljz?(;wzGA>s-wp8mDL!kMW^mgfwgQQ8oI0>L5F7cy zrcxAGs+TJHK(785Hu5o5`Xl0L{5o76`wllR&1D)Jj}Bbz9ZwDFLt%s8&-rpl15lnk zDn-8nlo;b_g23xSHP-4aw&?dupMSw7UmE4%lFN&k%h^qs(2{i;8nd6TC3l*}S=h@C zl;q6aX6N%6<5Tn=baZl}(AwL$-|x5Kg7Ck!H8-T|fq19{U(N#zSqF@`Y(Pg%?W~LG zGn=1JaAX>-KY22VtUXt|$hHAzTku2o!N5m0bufVvQC_jfk66T?IzGx z(1{RQST@YhINk@}aO@DI2Fcr1++*1uYi?cLBsqN%19EtH(bAa8vPXX&kNngHwd+YW zH8r4E(#G3jaXP@^I+fMoH`NC}!Nx~%vRu;g4+-Sa4enxDGnkLElo}HKYpOCWDPJ~k1N5e!4PX1 z91<4Fh*|qwGp3 z1w9@P53M~t5N<9SE`eo%CiQSFZGdQcP7ZCj24BrScyG`Tt+AG<_-7Piffl&6Czp*#sxY=u{*AP>;MMYn%KHne-ITlsmDrRmmHqz?i`Y{J{0%yz{S8lfRe7m) zA&8BUAcU@>AG#Hw(X`>iT8F!1sd3+Wg%2162Xh}itE)lSh|B~s+}xCQ zjCD6OhT`Cn2>MZ16)<|fYyP1!^+BSA8)$Jb!!jv7Ej8F`(495}n*%JI&`=Uo zL{%6no7RtRm*yjuu|MAb@JMXT-b2ov{Xt&V=H}tI7g04B6m-7Sn$q#es;;$FC1BPO z4k>sRIAT(oHk>|K*O^n~ms|TLtkCNhXLt1%Y#1-=%G8f? z`f28f(?@bo(GA@0@;#h?pIzL}kIc0?eL0?!6SFio^5vF9aS;tnOm#>F_qMi1Zv?@t z!gv`ET9B8A2c^yhNzt8~?iN(Q2knpHDQ83b>v zO-*Z0HSIV16j={l-1bR2K$mTmnK{<4^D^{L&Gcd0=X&FGs--jnLYvU(xxr|&<6SgME!4RKoZEZ#0AnAglibuC8YvRwuB&bKgyxiB+Zf z?)@Bl_9Us;zm3|#P6Uzq`e1u`qr`q@#{1GGm#`-1Txj_qnJFt&`Is`4nTKf$9v~B^45P%oU-HB)xqkKk-Z#zklw{*Qgn6|5 z2f!1yatjF{V}yJJdjAkw^ngbA`S}t0zhy{h6?Tt-+=)Ef$OuF;z!`*|Y{4_8hMZ@7 z4MQJm<@8_NbcOt##DVeo&d%KpMNSwY-ziC?hz3G@p#xM7we7&b3iDh#NXYZ^WO;cD z^0{0zcfwgOgR$V&$2p7C<%i4BQJ>uBoC8+8msWhZlqxnAbaAuR0C%XtzG><{FRCyf zpNv;(B(H=Yv(26*K?Y4;b5qk!oQk*y$`@p;GmYOfl2CC9sw!S%lgqz~N(@x=ulw+9 zwq0}f`(OG#uexp%PDGo7j?VCtXvdi|-v0MOR!;B*=}mNNb&mT2AjTC)?eFL)2Dp+J z+w&)oV6+Yf^*=r(#lzWo6hl8U^1gPV`plW()nzBTTfpDXGOOHkS&ovcgP%_#bt&`k z;ku?ImGd$HY^9{6AP*!j4|PX01jnCkx6U_juX<`>X-VkJqvI}S^vjoaxbGUtqDu?k zdIUjJ3YxB8oY4=N>AJysUeSgIM|{$<2Xy?)eMVw5hex&lL?nz+gb;$CLJEqLVG=&-_dJxy6w?5pcp2t<8R)e8FA5>8%LKi1iHO!S+*!D`|GGJa1FA6}WgkF*9NbX@Rnz#c@u zG`O*{6Kh4pO>AvbuzGR(#Kf%bNra1#U!H>N;^y|$my9F`9QKe^s4E&P=;X2v30HwY zYVbmeq@84zVIu_b&fa`c&Y0!IgTk7FIs>^8Qo%Amv?d@%s6 zP!S#n*Ay2Lmcdgeg@qu*I6K=oyE3Pvc}|}-KKdR5%2rjDgM-6i`(XNK=9cMyp(Fw7 zD+f@AfhPy?7~TrL189r#X9%W<bIb(%n}2=> zxs)yUH@tK>_|>-piM@(`q`iz6c>aE$n~3jR%-H7HE5fEE9^H*N$}4D|#+ zc9~Y!tB=J)=?TfMUAvHE%g@e+oq#!0JaPmIqaCo5OVMk84sHpU)wZHq^M#xm7tQBr zjfLqIo&M2+ibO0r5$UBW zK>V-5I}VlyVI`zY6jy3nS_+cXsJn748t2Dn$FpRdBWcFg5`;&m>SJss47aKAp0;(e z-b2xoS>cJ@`B$Gq02b*0zcN{fm_OTZ`wL zzl~0u85+68@Azi(%tGSc6OJFrVYe5DTG73!b6adqFPU>IK9XLhOjH-^>Ou~j9!NFM zg}@nXWVc%>J`1`uQx@=sE9_2V{kOYWs~WwPxWgjs~q2nsA(0 zS=EXP?B4wXI5GpNc4{To9*iT1Ckak%Lo_(N_-fGPDlG-?84Xgia}PB8?_5oJaZz0( z`rcFVBf*On&b1mlhOA`Z$ z(VHH$#pXEwy%SSD>FBags#*E$*T@8?%V=i*E)BvSJHK9p$xoR4{S)~Q2e zvz`yjW*DJ0S6F0x^8;Ze+n+xY&UMswoCKI#R1|N}bfU{&OwGiM^{aQ?_p zA=-_q>i2zedbPH`?ryfIw$^X_{9Lj+fL5~pwcyGrCdRQ_WFuQc`xC%?I$5t;Swq_S zu2yZT#@_#1Y_W^*L8H`QlaFPId+hEGy-P6Y63DZcQVP;xu3r59$CKz}L1OWShA%Aw z(bVyOo@Xo3(}R23RyNQm7nh1?^_@BCs(b-znTCn96^Vk4LGDLMDMvll6U<-B_v1=G@9*f=wEk;%Zrc@f|ispNNm|Jj9_ z#)W~#avu(E@wu^x++1v!N`L9-p6T)n$EK(jB8K;1C%(GQd^b|~?dl%l)6&<^&cv+W zGW>dd&vSk3;@9iEy<=vEoF>hunpX7A<-j}IyO&mw@=!@)=$!=buVt*5t?kS?GOaKT z42j`k;HraPMLROXii;z8_VLKoMe*p2oiv4`XDzI(Ty#FM_HBbEzjp6)9hcYntf;mm z`0ZO6hfqly5iTwceyOWhXD4!L&+{WW_wrv63J3t4YHemVi=>o8dfq8933IJ8kFVbl zOW9O*jdgH_yOL?eSyeMPA1dzCjMf2hYL~F5Cyf$ulv$;wse3Mm#?Et6J5176yuW|; zEsu=L9fU+|%A(v+v}YF{=*6S+G>PQ2ZX<%5s`=PcRPmXaB9~}HR7Z}`N^{lJwA8L& zzZH>_LlfhvEB7h#Ov$qn{MOsZ<5l-0I((D;b+d>X1tN4S@Vwsi!dpOKtff9xsVJzd zidoIcRsZr|fNZYcY_Po)cH;)^nAgDjT)?3)Y6uS^xdb1`>t23AOS?=Obw15P79trrMh-O`o0R|H1$AwY@5-hZNL_#<;)c(y&9Y1Z7Ur+M8cvHJUsmEz4CO0oYFjs8*Vn3P>&0Woxumk^n+ML{#l1Gggpy_! z_VDoB_{F|j5~+vkE0~^#Dp<=zLTPvh4F^%E7PeBI^~y*i;9Ki z;RMDuCW1RECK>@zk4kn-b*|}mxIeAYlk8dG%=gC;z1_sQf_l>#&0Ef3Lb;z5R$6-h{_Par$+FLh ztSL8w^0uYrZ7P*^B>lLC&PHM}hW4rwV?jz6A+US&u|EF4m;W1$|1F09Uwa|j1mFO; zCGiGpNJbhVsWuUq_(xL$*rw35YX^P26u`M#y3j53*Grhs>P+?kV#k+ ztc*wZ_Vf@wAsAi=6Sv*pOll)1pMxDre+He6@03J46NUAl6w5wwsb66*AO&O>!tkS9 zgLixblphJ+HKgw@_eFU>*FXRk0X4O%3b^zm-`)5Sq_fL`52!G_!4*OoObGC=ZJe7( zZH~$9#f~IW-(w6CKN73{DoCIbX>Vw%M{&zGbag$K{x^n8^-u*6JJqx46(rRyL{wg) zhld2g1%F1+$g>ZS+G2?ib$>YM-*@+(K+82Uf<)g78rK@qHbXSrif_Rw?Eyi#H1xm(4G(~Xy3o<1E2tbr3KL5hy*WT~y^MrA zv>0j9cw4kfJ&81d_huTVp;B0)Z+W9DblHn<0oyUV~v)r32DJNk8(k4AgRqr>rNneI>Bc^gyKfcf^4G zM6AW+wZz5>1U7{uG}QbW@gPt4{Px>j2%<+ z(EGfQMln(#sSXh+VkcUC=%px_skNZ&9IF(S@~GtnZzQ}9CIW=+@2IAeNMR?nS#n}& z8>L|@BS5##eGes!s+qSu+)>WhB@n?ygH+ev=ikm3QZwJXCA9oXa+UnAm++d{JNT&U cgmZ& + + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + +0.6 0 0 0.6 0 0 e + + + + + +0.5 0 0 0.5 0 0 e + + +0.6 0 0 0.6 0 0 e +0.4 0 0 0.4 0 0 e + + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h + + + + + +-0.5 -0.5 m +0.5 -0.5 l +0.5 0.5 l +-0.5 0.5 l +h + + +-0.6 -0.6 m +0.6 -0.6 l +0.6 0.6 l +-0.6 0.6 l +h +-0.4 -0.4 m +0.4 -0.4 l +0.4 0.4 l +-0.4 0.4 l +h + + + + + + +-0.43 -0.57 m +0.57 0.43 l +0.43 0.57 l +-0.57 -0.43 l +h + + +-0.43 0.57 m +0.57 -0.43 l +0.43 -0.57 l +-0.57 0.43 l +h + + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-0.8 0 l +-1 -0.333 l +h + + + + +-1 0.333 m +0 0 l +-1 -0.333 l + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + +0 0 m +-1 0.333 l +-1 -0.333 l +h +-1 0 m +-2 0.333 l +-2 -0.333 l +h + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +160 496 m +160 592 l +352 592 l +352 496 l +h + + +160 320 m +176 336 l +168 344 l +152 328 l +h + + +192 288 m +208 304 l +216 296 l +200 280 l +h + + + +256.212 460.259 m +256.006 524.517 l + + +172 544 m +172 584 l + +BL + +260 572 m +260 612 l +252 612 l +252 572 l +h + + +256 592 m +256 624 l + + +256 592 m +232 592 l + + +236 608 m +236 636 l + + +236 608 m +256 608 l +256 528 l + + +236 608 m +256 608 l +256 528 l + +SL + + +236.319 608.651 m +246.496 612.3 +251.437 612.459 +257.493 617.24 +261.955 608.953 c + +(x_w,y_w) + +226.257 618.674 m +13.8649 0 0 -13.8649 235.819 608.634 235.978 622.498 a + + +231.994 623.136 m +233.11 626.164 +239.484 627.28 +254.943 629.511 c + +\delta_w + +256 460 m +236 608 l + + +246.736 527.461 m +67.8648 0 0 -67.8648 256.13 460.249 256.056 528.114 a + +\theta_w + +251.63 529.708 m +252.319 533.706 +258.523 534.533 +264.037 535.498 +266.656 543.769 +273.136 545.561 c + + + diff --git a/doc/figures/tricycle_model.png b/doc/figures/tricycle_model.png new file mode 100644 index 0000000000000000000000000000000000000000..6aa055f7021e71b7af633d6addb207af9fb03eac GIT binary patch literal 25078 zcma&O2{@JE_CCBRlFXq@naeDq5@{zQQ^GcrkdP^nF+-8iK!!9Lip-TEW2r=js0;~- zM21E~Lec+zx6bc(uJ8X{*Y`Qsbt1WUF-C^k%#6H@Boc{vpN^(6 zi9~aOM4}C2pv5PH>lSm>7k@jisHVwG`rT++dt>v%HKdwct zydCpaLMrv`FeE4FVv@Yr*c_5K_}ihIVB|}DM@1T>7`4T+S`q- z-d{Dfv$Hcbz1_?nsd4Dgp$sW)wir!1I=Vdm-7?&xBO|#!D`PY-UAvaw%+97!Sy?Hy zG(R#j5>xL$xe%Y0cJRP~jAnMmHEW_Y@J$IRDJgk*^AU#n`uf9%591P^-LE=2_>^2; zRURMJ*Vi`<6dl>8q$9z3L2%d2R_Bi6c_dQT%9Sf!6diOrI)($^R~-BJuJZWR5N#4E zs=cFQJm~28joYtWxpFl`l|(8N;}w(_aaGE9xNk9B`}5~dW@hG?FuwKcJ9AC6lSQWk z)I>!^DHk4(_?$m~o}zN<=FOY2mnYq~R(tkW!0v z+}xT!c$=!6x_@8catmdIVej6(Y8&<^i>~M8o%sAY-)Ci4^%-AnleYH{gV)Jeq&Krm zWV|*K_h>lB6FmDR<8wa^iSg3X`%H!W_Axr4jiXGYS3A77u#kk-Y~-PLqL4_Io5vrp z;X#Y-+dYI$X-K7{NGVddOscQ-db3re#>}`3#>C_FBng&8&ovp0#P|BSHMmIRHIFUu z-`x6&OpSSC=4NKjo}L@n*?0TzA?46h|71~&qv@jHF4F?B)!|qF=i=umG$f}dVs+sK z|8*^H{VLi2+?4@qk|otm5G!liER;`6YCN^CJj#fU6i-K8g3DDSiJ9r?qy1H7j~+cb zf4=o`#EP9ecbXU*k9n?p%!?ZsSl5Q0+`aor;->vWA3s+6j{E$c{aR>U{_Xw4Yu`>p zOdPs&=+%QNMJI~bhKuXi*=P8Dk{n@ufo5banUg|d((1RzDe-LjT_}2 zeQ!!#O?a=ZbP6}hdZDsOT-@M_V%DL$Gn)*r=j7z17rBy;&xI^K@aP-!q%-1-lseqj z)7Dnx(f2@0ORM9~ey{E#+kk-WeuXP|6@ABEeeS*+9ToMupDo>>)XU=J^9uqD!ZDgt zzXrls1=VZxF3Tw>DEMw;?0ovAr$mLn?ZuTXCB?;Ko@?S+i>1V02L2d)^0lx0$bAcr z7)_Ccnt;j7u)j+yRXDgr(nuW@>ki@zV8&H8qWU*hS5L zee3_+ncI=7;AkJjG7|dxYdJkL?{*cHz@Q*{disrGVo8@T=LhI-+qNz8)kw?byEc^? zT6___)N4GyDCp(kA$w9O!$=}QM`O>PzEanZBO}RhSK~bqoZN9g7k~f4ZiI)2=bGF( zG11xd>eW5J&u`XlGMxGKtJZh?(opv?k%Y0i$&b4hXF8e^H~pR&Kh0)U;eF`XvHLr& zv8HNd?(V*OaMM6{YpX%>p0B-SadjU|gZJ9Vbw7LNgtHSD7dI`lxja8NH}?6n`C{0{ zS}C|ycH=Si&_8d>j+(2gR^$76VwbA@zjMeBlzXLJeM!n0o*HQxj#fW5q*-+}BiWKm z!v)^d(9qDeq$|P6&B<9{S<>+4&Frg;J)7_BUagU_SwUg4Iaylu#Fq;H?{;P8Au`-p z2NU1cFEisEKCG$n@vj~R|7vecZ{XVff)Yk1S-w5#y( z^2%8jN4AW-DLt(t){OH|_4mBC)#KFZ)09B@2AFUk4)NFbfm)}gOTv53l)Airiv2RU zwpDcRIR*xX#+NVe&K#NyP}{ll;p(*;L*{;bz@iGR$}XM!Q&UrOFmyia_H8j4nHQa% zN$=EshIJy192_!t7&02sB^qVbv%ZnBsqk(|kyU(iTV$$Xf13Jv0o4aEEj-p(U-{iw zue`6CBCqf4PXp5YTEfX;CBeU7c?s)#^AFaBVyRYTZe4c|)~5V5fiDT}3QfIsZLR#M zPo=~CfG=jsrP}0bw}dPpA%hMl$B*ABnG{=*Zy7lJ<*kaP5Eec03QH? zRba<6hx^ViQV)!qb8~Yu2p>IqG;2SbOSo@&R>IyrTq7y6)(^Y})U{^C_7*reI;#4A z3t9Rze=uy3(rS=Tm!MPX+9inDMAWy}-=$Gdh9 zgXX6WYiK;pHz)poa$4F;=Mp;F-M|g_Tgx*vb)Lp=uI85pq~>I8ylMt z>W{JUB~6iB+$@?!V2A%VHNT0@3j(UWJv}@;Jg@uP|9(#kTiVS);fr!~bY$1KrG4>@ z?=`XqIVbbeb;7*H#@4T0E6Ne$^rd0M$HU$I+q=qJexjm1Jw20?ldK!m&c?+BPYv$_ z0eBenGiYW^A#loR)vD;U(1m?5af$|RZwh*%Sp{WeWu-mV#noqIWb{}2Z~AnKBlFfR z|8MUs?d&|Aolgusi`n5f!PZw-S9j#)&Bd8<6NiA7zP{i1TW?46J1)O0xDIvYQC`1( zt$1S4rxH+nJ~p;J<*L!&Khyg)H8q`_hEK0zRS%ha>|E0Kz(X(c&dr^{4_rIw6B7)z z-uCr{!Qp_@$!@FO17q;O2^-u(X2xC@*;d1a6B85r%8qu!eWKQ^eR@_c+uU<>tM&Q-_~pWB}ZP?)Tl8IT(Hv2 zdi>+ZkKkY5fWZJwke%_*pRvf7>gwu1vzM){S=Hy8gXgEjFSp!4teR;6^x!zEDl74~ zK8`@2MgU)P85u<$#MKX-Vdi68N$c}6GIAv!AD=<#1|2cPAkpBCyR}QY z=37daXyflW)W^bD&HLLvc#ClmQ8Meu1_v#Xgm=~ctinn-^y}BJBc!oxPz&hDHh_U{ zQBt~H?Z-Jcekbk3m$x6jeS43yzp~|sNaWPiROsJDe?7exMCnUm92yy~>g%H_^a3V2 zB{y9B)goWZRvYx|n=OLsj}LWCjwi-mMWiOuDUsKeNKM_Bk9-BIl~gO%5i7obKiAjt z_MJQ1U>Jjg_CP;ozO9|JsqzOOeR+K=8Uz7rmUNdI2V$*a-5_>TaW{iO>gErv*S1Xr zi1Moi&$9SmBlBKvQ&ZCx=NE&M0Y5)?AFT2npB{b5t^x1K`XO7|jo>3qmkm4-5Y~M6P3-!v zu%)hibKg?Kb^R5`PJc0LKPb$b4T@)5lsIKXF*eqqU$^zz&akDB@ni9@Wk5+?G^?}^ z;%biW7BR2^7@Vn>lc8Z7$U&f*9UvaG%&;?5s3A|tl>Mx9H2^fx8aC_f;2^T-8WbGd z@mfj1Z~Bu#vFJH|l}km=E}uU(Q+U>|m$9u%O8q?c@gt0J!+5!Mg?C1I`LF9L$A=A{ zf@SmV@O1%4pJ@Oz?@Qdc@5obK1{N02?xKD?08n-YA+4d~G&t3VWA3yF<)3C6y`7$xV%f;1|rMIr$TAU-M;etRg z?in8+&n~*RbLzTpTc%bVzSVG^@ARotOiWA*GvmkqE}cAhZ~(-|%F60$YUS z=;&1|SMsZ#DA2yRLHX+*nUw>D*4xDQuO|6D&FB;PT1cD6D0i8UpWpq+k!>!o(m^F; zZL2z4TH^kA+}Po_)$^UiqCKyM%d@lW?IS73qXYy5Y5_>U&x>)yz@G59;9GmXN|MqQ zkcR9%PmwG#ys-o556b!l+0^dcG?o1zc)%D?zWjwtpgWvgTsL+Gr_4}2Hc~$k2SpL^`oxjTnBr5dskOUfjci+TZ#DKab4uabrqAUjRS}x^sF0X z9EIggOgc{fo=up3P4jARs?++a0GhK){_^r_YS-Vj=N!0YTYaLbaGko>d$p+^rN~!t z>*UX{2>qxwTa?-DV-AxBp0TQ{buXrN%%0DnQ!AsRpb5p ziSQh&FXMFI0_&ESliMiB5gQlBr|j05ENz}Amx9FV1W+(DwsK|`9@^X2XKiKm21bJo zIdkSrd?1bcp>_{HIn%?=&Q%o^p!TQUwHF_5LDF*9w)zdi+sj>llkVHEi)eMmy^yyI z!=|65oIQJX0A&7Y)SC8bt(|IW7vSf>HUvH04Bh?vFQ$$$ke0m19_Vz-k+vu*hAzz? z8Q-pzB;g8Y1|51|8}@y4RNIgydH4SPUj0>m?a~90k&!P_74|ILE+{NKdGaKpjILVP z8j?{I3v>RaO`Cpw?fV6|8%_by@Ooc!B}el;oC551XXu|)iN`{}Tox7<__lev_Y{j) zjtn;?Vox`Q*%Ui9AwHHBsVkC_U)?;!<}02b^n2DLP5mS!f<{^Jf!^>uu3@ZO29y{}%qdiFL zj`{k&XlvV8UQt;Y7#P?!(|8oide`3%c$KTR(RX+Mg4*_ z*xDXxyS|UfZ~pghdFuz~QjMe%bU-~1gT@a|D{;h#ZQIuU_N~RYR&A0J5iMZtwEq77JQttpTmzvRdwpx0=R0tpJq~)H zNSDO*+}+(TY}jd4W~_l&`R4Ay{my%#$sq8j6uo8&-wKMKaU5n}HuEC8v(prp8rS~o zlnO2t7Z-L~Jh-OJwd$h45{#3()~zHd>s&N)5)pn>9F~c zb;X;xNo18B3Uo>J4g%Y~?Lh*DQvihEYY%E_Y`z`sai7H{k)sd^@S~H#TJH^$jYCL- z^uQ?<*8+N)WxMDaN1e!^5^mKD`{zW8};0*V_8W? zYj3R2zvS%Z7WA=!dE7V+WSG%V?k>a+Sg)?09`bq^7+2tG@vJ#gI{Q~1`rYNlQX1es zNSnFC}$u4T7i8F*E6~#tu8^NCix5!CLTO;`eYb$I#$|X~;?e_NG2PAl{ zt*uS6Q2G#9%rbjOE+j%4>#BYC?i~uhXm`*!(YXirVxG;m_f zQ^20rRxtk(Qs2_>aMINCckgUqj&y=iJcud)r2Lej{MBhM2WkRKd?g=Anq*6?A0JD; ze7XGiXd7$oU^jB0P6(^Sb9!Ju9%AAMuwL#8q^21@bf3eJJc@&}RW5e^S6;3vX6L=`Z!bFr}yuOI;= zfozU^zOzGBRZ#VW`^Sd!oA%$lJ=;hwq$U!!*A9O!+N$z8QyzB)EWx zJUgy7AyI!hF%fa?3KG9W8ZdQ)XL9jiJ=R;10 z$m7*hJc7r7OriXSWMmEI7f+GYEXq%_Nz7WgV4nVWkkMYw(>?*v4RaJlEY}ath#J4fKvaetN7Ki=xsYZg1 z=1Nb5I(vaNI*zuvcZU!=`f;8z}$~RK|u>3fc@{@ zojZS?%k?(>m-i1`H}?SSWRjU;rI6K+j*+%xVaw%PG=hziE%R8+S+0(8e`3S>tDV4frCft zyy@xA=(>n&;cuVb?kiTBe-9N3f`Zsme}Ux|y0lOAJ;05u=qhmZQtT0|PHI4`<+^A9Wr32Qfby0jZ605Nk1Bx&6Xvm-6*AkhpBz+D4>eG?Fm zE|g`U7kX+KF%P@I00H$1@T3mub5; z=}ihp0yIt^2XN|$T~ayziGPQ0V`rz5y!`&zHps2Jckc$ib+y|VJAyvIa+L$8hC#6M zLQBEkrrsMu)*Yw~ySt!}oc`JGyXz+bZOt`U|5rkd`@~2e_txYr~>C6mbTYldNu#VPs)yIs@tqrt9!@26PD-wFkVy-v03i zZ#Qc)i!fmthn}t+zZNt*J4?jT@ej#RA({+tqi%T*z^luLQX>j0r`Z^pnfsx6RQ5MF zOKshn@^UoM8m`DW?-aH$zDcWyl6dJ7=ls2i?z=dU;L2*r3~1C0!U3lD9t6#PX&Uk2 zx}If|SX!zyRb+@*z$N3Qw=;%5;bNt5Br-Zl6=yrUn}vlQ;Pkq>x>Ac$3JUkJ-#`;Z|H`x*RRg3 zh*4DX=r#TMw;}KBraIW--3zC;@J;r^y!KI8E~TXG4SgnSRhpjXQI_?(Kx*5zawHAU zY6}YrZvS1Av=x?s1RYw0d9eH!gbz)+7PtK+*cwSmN##Ye)gUmCYg3juj-NO&J2jPe zHM{l77VX(`69fq$*FjfDXc1Vb!4d(B*=-8MT;ZuV+GOjCs4PJ;QSNEYZ#>JE2 z(pxGy;`oP)iz`I^paf^=(w{3+99HMd4;+9vvSui3|M-+Tl zvJNh}IMA-8Hy|DF-`hD#yKrR9(X;UDsCRO+vmbJCvH02Z5Se@v3k8X+kB`r##S>eO zw8_ZJqxi)!DS9CGJDv>o&`zgC>4toa+{8G5To3i(D~t?!FWa3YSPGIrkWVQG?rT1) zo;VfgR!4W0l$6BquqrmIEKWWb%&Scp;*_c1=GG~}qig}?2c(NLP_-;&sO|}(0acOD zt`_cfdmK)~kOA1~=x3w2`sauUKC2Fy_c8dbvrfC4No&sOsO*1w0dyy?T)Wn7`rvxK z1BBH5PclCt>_%VA%*@0pu@{g&Q@%ZLkzTcG70e%oE3rtI9cRfkf;#(3S{jYUzP{@0 zWOW#B2?W*F2YQB*kC4%$3Ub)V>BY5eciW8)!0#SDe2A1aYcBWzC6;y1v17-e6JsX} z4<}tY|NNuh*ItSk2at|jDLUqmWBgnN$&^rJ=%2aQwPAmudp6Y9Q>+U2L(oVW-Rve+ zZ*F8X_~Aps_3QdC;c5t#9Hx5Y^^)!-D)K5W4O2(uAzN0qO zi_8+{y{h~f$}~M4x|MC1k+PP;tr4NuL;~DEd1pC`$tfWLG}=Pe0_Xiy2x`{ z3Fy&JO;0d0|A=EjZi+j9LBP6|nJHzUw40&+`Sa(`p5<(($=mqm%^L>cEK%i1DYrJu z{rly`#GKl$t2k*T6c-nxxZnYJgE0~ct``losNzU z)Fvyl=1L8noi}PUzIdVEpbpO?N?nM>xP6Q)(g+zjIpE$Ah*n@pXV|VPIPTTeZAMC^ z^y*U{+5{kZmwWc--MOQ!re<2k?da>PlsCWkLF`$}N4yp$#)YN91+kAmPR@QGNJ~u} z8y$tJC^SBL04v;rD93a98!tI&Nc8)!ePWleUUR9JsOO;?#AOp}c?MgIq{g7Wu03kZ z79vpc{g())n+&dsom_jGVU>!Ciqq84#KZ)oAJq8#wm#U4ZT;F)vT4%qCy+VP2yi9+ z^*Cs1WPEJCMRz0vMf06GtrLYaZA;qAgYa8yH0qC&NUad`5M6~MfrHum({A3lVeqxD zOBYEZB;Ur4yAL?il9Q8TEx9t>B-LS*vJcG-X5@M@CO{$(`!B!ONO}Z#7MxdlickQ( z*Ld8L_WWvQ<{L_`9FyN08+9V%Z{4~z!-5E?$?$uAG_C73iK|44fikfJ>deQ%!NHM{ zpI>^m3|-tVFYg5q&*M+5c$stk?c29tq_g84qv(}T)Sa}nvNGmCRt*yX#cp?WLh85E z{~N#fs_xq;+X_WCwv4(V?p``>#_((;8CYV~mt6t4We?jmK0Xet)@Gmt6%Rc*y$Wd` zh~LIXHj>(Z#M%B{(WP2`i-d98$4Mf^mHAlIoLGd+P*?uSIst*5sA_3zKZlvVys9iM zBozLK;^|ogL3!Txh=+#(1yaJ~{2%OSqTdbVLqzVQ`I;e!{o!=&!Z7xR@)e-H~%ZG}0(E^wE$!5wP$O&?e3(*POi@`=Kt{JZNLmoKfex zyt7xMns*9c6w1&1asBYFS$$K3gbl7mg{aF)vFFjQSb^}c)3MZ?2}$w{Wa4SVd|VHCCWOHk+8NpX~2zg&;0ZM({5PN96Sl#r8< z&}+jpq`vY92z-SX4Gk%JzS};&{%6X|%TK*@Y&Ns8(GR)vz^zL+ zC>SMUSJ&La!onE?eYd{7HYdntlr?TUM&YMT73_Q)He6u4suo!6vkH=r6(~$$aUNI~ z(VhUViR8WL#47GMzrn{eZ>C;bRi46b9iVM@FGVBivLfM)^d6Y zaynvf{%rr2EnA>sAl1lftQ`LIi6d;for{fK0}3eluR*Sa>oujJYx~Bnq50+1koXlG zG|O(NZQVMH6cDuF?t`mbmalUste7I92O;irEqXn=Yr_%qtgg-x9SZo+?t%N2b30MA zg9wu4m9{H*wg6hBK?kSyx%I*~o@d>;qq<|q(80c&5=)Y|&b&k#%dvKCQ}9OAaX_54 z{t9Rq>guAcBzNW*l4%tDo1$PE1 z9si`CNl&lR5qs$Y#cdQRNu^CuyU7QIro7TerFJ-nZ63W&hYl&AQ3H_~(z@8jjn0P; z^YHSbRzHr;tYlpQaWOGp+`4`Du$f}~HOlI!rzn?#%0{0#14YH+NrSXq>_rX$b?(Qn zrbpb})lfwPt$G-`ScTP5{43BJRbBlXJqARZ0)&Dyk&%#T-d7$!GSZy9f}RQe3nA*e zP_fq6XRwj;M_U6!26E8n;48e!v3~@GZaz)CnR(^%I%n`m6O|`;&c1UxT_+ym7^(w z>G<(!hyzWJzGyJ`eR-QO_4gat1k@4jy?Z&1-7&i>t~>b?QM{&AkeAoZ!NJ&-ek%px z*c^={iwk~3+L@e$n3c4oYE}+9L-1T280@ETp${trs8M>~qRQIiZ>95WbNtMd3Gh>c zA>ra$kx0+?HI^n%RzT~7o`fhSvE45*M{_GmbUrQVyHB%efF5Pmc!*`zR7ufoR9H>X zU!R|84pw*XP#r)CFL>$2~Lzwk;lhA02bDOesW*a#gT3PHld zm#BY(BODx;8-@%6o--qAlvMr|*)dTvsI9G)DqF44wAp111$~*2inrQ74#!c|H#F#8 zE;BJS0El}!E0^YT4j_X?`m@-5Fzi6+aDu3I=3j$#WCjnEtAL;=WQlAfk9_(>9PmWd z?soKR$l29Y04#dEN`1%Mq1A#)q84Duf)3p-yTt?TTO(hAZZ4prj1bUM-E?MOPWzYz zm&V!p1%DNJwcw~B^{}Nc!^6);e7^Tr-+AY_je=qTN{geYD9zLwhuFxO3zT0$Hcpgm z)~H=q3v7qP@Z_51RXiuiA0ZCI;qu>pma;5$VZYXjhFnJ@qxgu3-B|Cpf!gn%KO=&f z>gnmB>FVIYgFW+&s4=1a(@6B8STMu$uwsd^L| z52&!6k&%&=6*zD=mQ3g?KQ+-Lhx%g;XlW+*1F7bgmZn}V1TS547o&viJ;^t3Zcv!K z^!(!?)gqVjEx|2CTVNq&xCGvSx&s6kxc_K`sU5vVB-@3^69pkBJ7qvEy%je zqU0oQkE@0cv)y>ZoNq3xp5=)cmUtlcqSXtD`c<7YiHH0cD2%zeb4gWA67||Dt(S*Z zkdKu{Ny&;-&vA~REPJ-PH}Wch$c4%_v8*n10pa=|O}59*Yal;+41S5DZ=;H^RI4QSDCE3maux-L1j5m zG%plDhTZAi5Vh6QJ*(E|x%h+L!g20f$Z*7<^$S6s%wmpEj5Y}z!$u{gq)S!3Wg=`0 z_+_5q$X%=0?>KAtg;6Z)810TdT<6)V?%jL!{CTwgKD6DVxv!+;INC2PEqmVHcSK*o ziIHZm`ncf(XJ3w@b1W$-DX&&UcXHod5p+{$Ji0UM+;&~kRz3zB+08^Ii}K5z@hxR| z0VzYe)oCqb6biAtY5_muh4+|Bs%Py`PkZTb-;I|~QCS%gE(pb^CRxB8Y|6yMk;v5^ z>QBOK_nupyk%^;p^-$vtU3Cm z#W?1F|L!jR7Vd1SiYp;NgRUSk7*0V5&duc%jIMKiQ*dR5d;P&~@E3wl*S#b`4(_^YC};pNMh7klYR z1!G2LTGfcV&5N;UVc70F7BAv=wo@|!wB0=RqKv;DGTb-H8Icw;?@>Y1(P5~}0{PkI z^&WL=$1*T2jjEf6>D=hngkMCa3Z5e(+eYcNM!) z@8phK{PSnSuF!%YJ~SDDJT;biuxWt1A#6ODiIpU4lXKcqoTCW^7-?Dm zJf0Bs+S1Y_8+MZiSb^=MkSj8?Le%2vVVRpnX92CK73u8CcXxDL%g1MJqE45}+>ocl zn@2FeTerCO!UkEY5>__cov&fu_(VSyZ@S zS)V?A>fFZZ!nHoX-6&@VTCx!1k6!J{y>sV?z5Ql%DWOLIO)2ObZY|X{NRqDqH9tLy zw1nEig3b_lrPH-|&8aYxjGBZDa7Fx+f)jJ&9ejP$_q20qBva$vzrDa^~ zJoF|ZyB|Ww2lR^J6o4wodk?PvkeQBr=tUZB6?moYG)Is7*0_4N_S;SHT~zW>2j@9) z-UQ`wl+10As?3DF1~Bixu_LRAV~TVY=cYDLB-rfvr))1Al6aBp= z7v33%P({@D!%y!cstydK6|G(%9oZf!CH7io&6+h+h0{}0oeOrr7Llbm)Q?e6ou?DXKEQ_nhxrAbJ>2K1`9bT^M^Us(Y3Bo`aj4?ouv8l!jIB z<6)yRlLL1pw~5Y`7}jYCBOfvxqx)?$5$cX^OU}?j(C+BMN5dtKQ?dI*{Z> zmp%U>?&YXxB*Oe{^AMv0Km~Nge+O=$r1H2+!HA887!R7uJ6QV&vWQ4>+|hc_o7b<| zhd7P+S%?v!H^XwaE5hjqUtw(Mkbw{zIfs@s{`6zB)%lmBCZRM8WRDN=hsb78%9!X` zwY;LW3@gzn1?y9t}Sht_#190VjUyuSg^nG`SJuPgW#isEGw@V#;4Z8h%x86ji6P z0y{-ri%|o_x1fDikJsaCskDp|CKeVt{%c8&PlzEX7Jfyh3erkaW5gb!o-2SJcuo!u z4S(tI3s{pzPCE-E#fMPz`wL0oTQ{3!3D_sgA}kW4Q>80pY*)BME`M<@BowqJG*rEx znWlm%-7e<7c)jQl*Q&Yppk^M#h4KHnkkhr*(7zZ&}blkfU(^l=gsBnmJ zWMyZ!Q$Hg%r*tnO2*}=xkCS7b46jtM1rmjKlwZ7fK}$=EYrWFKCymrSJkv!(@);zK z9phRns9Q%8T17RN|4fV#n??Mw57lsp6Upd%%4tNt0u~2p<=QthlJhAeIth55Sk*OB zCoQf*OoOTTDwuxoZnKizv}qK+4c(D2A~x!y$tI?xY(MMGTdvV z)~&Ez;YhJ+c>A{Ez^%Qh=5q8*r}vzpu}q$2XIROW7ryZWVZN?{>S?U&!pVutJIv}t zA+LI^Vn&yttMW|B`bX_5CHiQl(`^Ne645V5FDE5AIXS(;_DK8gG~I2N&L2SzXFGKi zy%AKK>V5w7DF$=c>Jt1=c1=$gsH1I0C$OBJCo5gYbDs^>sxSKdG zamb7=l`)!WMeJjjEYPtdM~-y#Gp0)~FNx^s9P^}oC5&6F$4s0H4e{SFI6TWgyCnwb z{%@=Pe>oAOY``)CUv@QDco43aTF4@SRqnA$8_o3dIkoI;Oem27sUG4 zkYyjB8iXb-$flU{t0VRDxpP@dfbyig8>R~AksUmZ1dVksjm&A z(+Gsn#^74|XYe$K{D{v0%B<3|E6Sbcbs*}YERQP^_FvLPb9ivYVJTdY7`7w^8<`H6nQi2b8z-EQ`e3O-l9HHw`++

MX@5!NMqIaY)MNks_9&g@GG^i-71cr33B6cQL z7!#8drwMDTXZtVreoUrrH`9~RX)TchZf;W82C}TZ#SLMi7{^dV6S0Hpw)JcvBEX={ z#VnmP)C;P(2wE;DcBB9i5QTs^y2O zYiW^26&mhLPk^B7&QF`SY`ImvI{XSOIv!BGJZMT-EOpQ{1B0f~7&Pr<{0A2V785qj z{SU8+CtZdM97)m@CD%*nFM@4$)&W4={ip!a(ZBMs#pY%3VOk+ay5fu)A~L;OF!cje zPfE?s%p5`%PT0lrojTa)W(WUr8v$?iMlHB}H~9@t&!dKgA|{T5B=!0Vg~l&D>nLf& zS0VrDhwE2bm;GC6oyiCiY|#1q#6%NxgP{tGHaczpIG1qhCMn5q=c67CQe-H@Ee{6N znR5Vn%!PC3LXe=MdkSL*MW@5ci5?`g`b7VT=D~QGzbw|3}}|Co$&fNG(^od-uRY`ovCBnazo2L{3>jm zXiP-QhVM4b-^kf8ZV4F}*iBjd3h^1$&EAE9JNpHhw=$|?&V-I+hKXRkKOb@3 zOsP<`1V^%6Yu)G^Il;-_-~m5k@|_&2hm?qs3gU-{2mAbTE(|)~QR-B7-Xn-1+`^p@ z5TU!6El^xt$APPC8t8&6EiN|x%QLu4A2z>2J@4bkk3VN-aGIDds@ek`gh)qSKVsO6 zT?5%@QB|0)ukXHnt2M@mI@O-wV+7^~Qozf zHH0WOBwRknkKd}gZ$%FdlfR~b zmE5AE9%?$%1KqRn;0?7v#hp8EgqZ$VuJhdb#N!?;hhh54`o=mB~&-F-X0Wo zPz1-RL4DU|NH}JMVY!FF=0a@o$9Yj1;oa(9~`3P7qoOG1-UqdWdK5(6iRX>SAy2J@!ht{@126 zIiZU`&3%Zn9GeDO7{too)dVJ1ly(w~9$+a$B?)zd&GiQk96;@lP{j51nZ%Fjt_FwP z!{9gG9`$YtoCec3P>}SzTn8TQWIpf6Em8Q~DMURjmu^->L`;r{&{VGp}75w|-lIt>LWO>@W2 zl?dl`6Z&9DnBAljp^O(WYP91801SAa-yzA`GfE^*&WCWv)u(>Qd;FMK7~1R3S?pwT zVWK7%`p!;Hh+?A7?^f#T>R!dkV_fQpqNQb&g z_Y=Cg%>>O&)bK~Lu$GMq~co9GQ{iT!88X1i4VF*DD`pg}B zsrEuU2SE>&13?v^VW^H+lp#oih8f+DGFUdI;y>-=TbV?>bixGPW8j3pCMWNgm1R}S zR^+Pt~U|(3mEN3-#-lg z_U#*tAu`4^d?mP{Jlb?TsXzGKx18?J8eFwlDm(%5-OT6i{X%T~JA*hG8-|usrokap zBY}=yU@n+XJW#@KPh>$Ej7HAbRgAA!bI8j`OHcp!u^wICB_-2{OL(V8kXQV)~_`V)PWh<0e1NxpPRggXvHNoYJ&!Gsm+&**G( z%T^#0sU!ns$8I4uhN!2|uzz+Hff1p(DG-ziH4f}|%GIl}Z0+6M*q!q?ZuFy~`t{3~ z-f}Omqes`2u!VEnv)IR+!*~B#3p$n16M(RX7FOsR)fk%rA^|a>;H;CgnZp=7y5^c2 z8qUzHIYCQht-hzw_i^QlCFVphH>cmFunW_`XpX_~0;X0WGsA8X+_H9`rZ*K39HbtX zuDMaghG)QcXnvS`>P4m|fS%MViges;hqqEmtnKcuAU4bepwk1f5>Fw3Lh?1{Y1{CP zkKO%jckeUvL*15U1^xEz+cC!B3`v*mj_*pGY2XjQfrf{5pX8XKg*^6j6w|@{(^n|KHa6Jp5%ZkoNU%Vq) zr~$M*$MUe2zrVu%-f{X(YFIuyEf`&(+D)H23c?HQ6GDCZ7uAzkJs#2gK?JFh){#kkzaTTBtV-=O-5N3#)BM%KO>AfU)rs-2c)gjSq>Z$$V5xq;o=_O$?#j zKirAr#g~dIw8cz<>T0U0eT|K0kz($krjRr$gFL)c3Qk9QMZ39xg^(hcR%D0MYQ<<~ zLIy*b3qPoLnOPkWk(E7PbcV){z=IrGy!$6RD+?WnP6(M$Vt&sO^H1n<>+Q7)DhFbt zs&;xeJcO*2!gvjUKguqQ7n!JFe8|sM32_2%S72ma&(Htj zgEyN7-Y0{W0?rLQgJevAv5YcFKwMY3Z=wI^Hxwwmy(GJQXZW&$N(P~K-G5U}cn=}T z9wncl`VnnjRluakv7g!!^77F~$z&BMF?p!d@>_mSjfb9!`4sbD(#WzJAT$%gn=8R+fo)FtvqB6)SZpX*L zk=ghh11_PTUhvGNmkChM`kRPZkFjS(Y|eb{S)xDT`E%(>Zj#RF`%?Xf-QC}TV~A^@ zHP91+{k!~KwC`3qEvrDjSAF!lBcr15A|pEgGe^nvkPjzZ=N7%UA0eZTs_R67v4Uq( z%GYsmp@buM+1j-l4FY&!5UMPf3Rn74BY#%MT*xC1%jD|n;N5;QGBTKYv-A6nofkHA zRbIC7y*4D{pSv+UF{dAK_3^zbL%h6ZSr$m*XV%~{S&ok#gnn{1PwRuQsRES)B^#F9 zySEwdy$H!V41IP?qjV@KW) zC;nwa{Z_alTOI^KBJPun)FW zMWpI<5-Tzob6EdJ@-ARB!q_SykrUzx^}EE%1`jHx_?ojtN~Np&UZ6ZlQ&z`IsUT`l z)EFZ1*TQw_Tk>h=HgDUu-_Q_pA<3j_L*5|YdO?W9KWT%ZJT22cvQ7`>fz=#(UV(5)*GV?-n9@PmQ<(4e_ zZ`(-1k=yw%oIlSD<;)tQ&Qks8H01@}S@G{Og9r0onuinHu>2^*zqHx^ZP_1K;4(SR z^I@CQ=UGkN$UiMjO_C}qSDt^AJh>8*{ltnjPfmyNP}6(7N{zaEgrb_xL_GfgW3$2) zy;rg2uwwH~Pu&&Vi`~IX%#7%vGA*By5z_B}PxIeL0(EO*McqWW?vI~!=olbWtmemb zLjTua{t!e271(Sa?sFIC*SV@8?{OHW;qYcEXdn{5tQaWgBB^rk94jF>8+#rIs94eX z@8NerW$AH$&^|)`u;s8A)fj#VE@N0S*DV(BmM3Kk16D_8zXlUbeg+NcPbc;O3UcaX0EEXU^&y8H|H5 zsa9Ffyt!;hipdw8DPnUa4T)4Hb%mpj0y8GQYeO2kNG(n1plHXNB7*_iT41@gg+=Eq z#oLv&GrG-)?p~JfF}k^hLt^~JQoz-uh&ANw_SZ(JYU`sypDSdFh20|Toq_V>NUH?A zV&O502Rw65SYL@0N63mTtRdOw?(E56d=z|zQI5oOZuS`8)jeE)4sTfdhD?_lJ1cLOuI^34aPa9lT0seu-8rXf-9s?0hIwnP$;A(}@lSBigIgaVT_kGP z1A-b{r(cKqv1TxK>IkuQ<~`mJDP^0!2XZxcneE{<`0W!qp+AQ|#^uoE%^lJFtxRhq z$bc>Q0pr!v)dfA^3|k$NuCMj`7u?QB)M1>rK01Gp)`+^EQgpaQ0jGwDU0csoEH&5C zfHxYsy{}P$6C9#?iltjwGxyhnHRO{n-%*_jmnpkpMJFg>w3dNFPU&@~{D@HG`r6NZ zrYk}Q0xk)f zZ1@}fHvNCsZp7?EP00Mz_3QkV!B1B;n^JF1R##8E$cc&%2qm$^_`A!M1kecLkU=4z zfAGeu2!_q%&KT7%FPJPG%V`9wGcT>c@P@eT(bJUL0|#{~n0*>FBhjOv;y|lat3X z`um3LOsq{@FCugA1%6j|cNxc36(pz4z10BkbP0?E^r*tPiGKsR0V3_}IN%efi4_lQ zi8$m(=IQ-=HDUoT!a#*BXC(~w^?L$62|zjh@Bg9>JAsF1-|)o`=EyWwnS+%Nv<~g442! zelsiVOoJ;&fQN1tgps&w`iYgayLEMAp>ZxqQ}=^_(;NZyGK;T`p(G5#ib8Yp1Jp|h_)UL4H~+Ivw?edNV*WqBN}!l(@<{0k zq z(+Jdh^@B;s{@l59OhoTO^(t&iu4tWvj0`7ZRPir`ty@3&R5ENZr`N3x`x^>%6P;Q} zT+Yhk_WD(Q=qKOf_`T)hq*;*--u;#T3uF>S6^D=6&N^=WaP+;)Qn$!UbPRy{@zU4E5tzQeSykGW8?oR>DuF=%G>xc zhRzgH$e5H-$f8U_E$tfvLaw$XEluKrd6>4eCxUEYlF{wnVH(AlD zD7q@FHc{gJ&b#xEV?HyV^Eq>V=Q+>wy`9(SgP~N=y(B$~CVtr4)mipHm;MPov4dpi zngh^@hMp~ivFN~K79OMK;HCa zB5ClAYUx_J(Kfw^y39~wbdn4>zgUl9qmnyK=-Ff_uUc2szF_$1HC>LilWA1OqA@!U zg|ajBhIRqfd)2z+M#@c>z1AGd>+y?KUJX`J1YFC8f^&mCb4VW#`V>>IU}D-+P+4=ULyr+9rdqbtD6KcdxkcEz7D76qL_&$ZZP z*{r_px^u_o`*=&JOSH7c$DcKgo~+cEK4der#j|1x_N3XZ`n;uj31!hoXI5Hmsy&+L zE;h@*>n_ecM3xxX_&u#qTBAWXEa0XohlPh&4B;Ok~gvZ^G3-dJt z@9i$EhsPbT8pal+5g^VaYSH<=(G^mOSXouot1V6geysQ}Q+GpZzb&{2Wo-Iqk%Yx^ zCX!9dp-w3(vKpyOXOijX5zY8xbadlxtkplm%x2VWUBPN&$jPqB=FUboY)m ze-%jv>pb6sf(Xr1>emWI(LXQ&r%m%&>Nm3+ANm;!lZVh|0f?#JY1F7fbGImnh>X0y z*L;{8v=ig#OC^}@f8LnG6kK)jo7*}Omq>%9W)KQqEK8sayFJWJOrn9w(?8IBa1czg z_K{}wE~$O~0K!z=rTfw6(eJ$jc>wlpS5x#0cuRB~K!Ma48Rm4(T{WlLIaIjA78ny7 z`!Tdpzr? zzwJYS5nScAq}sA&Z$Ey9kJIVbO-lm*W7Ku&(j{{zOAR}DB(A50rQ<3(~H!)CI;u}$0?!rL9OaYJ-1>hG@`LVN|nXRGM+uC42 zM%)}Q`kRh+0Tbiy>IdVxcNz&Hc|<~;sfC5bmMsYW`J!;0rLciru&{r>(5k&=FQ&=h z;1Wl@9pYY6`{=dIf`WbZo^b9Un{<6vPE=sSfGSraU+X zSYo;|a7|xNPX=GK3CJ%<9|oNc->Pa$-`|A*W_M0uZxO=ogeX(qsf9kWXPONU@2uD1f(?y^5s}SnnyYp^FhNIfj-e zqt#Fsmt#lPYP}qXJC1hMxD!CW4J{#-#jeK4YonV``ZjNVoOV39Thi=dz3B+kl%eR5 z&AqroFX!Mc5qZQOkd_04&GVw(u({Is@!#DH($A;Zxag~LO19fm)6csZ^q!ep^y!)` zr=;fc<-FiWSRx>rTio`TGExwssFIz#7-sD>%b`lQpWfqcM5EDKI`q%@6%z7aTMW)R zx_fzbu}*XbU51Dn1px*%?{!+E>!?SV56eAr^79Q@g)S=U>V4ha>%$+GCk|eHyg3}< zF`FhCl=;yc5e2%_L46G)0|9*rAH5L{5!EHop zH^tZ0(;teOYoUD^sZsxRTTcw&Jhr~2&Xe1fdjO^{D9S``29OuR5&G)R*&Jxb;YQ)} z!S>YQ*0;#}=5ob3i}6UW?etW1Z=e28rFmW2t14B;VfE+!_@%ur8HUXfR;Gc##hjYKx6zkrDTVFb4{QW0JuJr1QF|LMWlk2G01)8^`gGE;W7}o?z1)T7A zLN?gC%F4<5T48u&>mE!25pNz6PgTP+l1=pH1=njhe_8@{- zE!ixipiH$s_DG3Pj;VGgbZe>>5<~p{s#b^CS_&aat2PeXT1Fw_Qd`As=VS=xguidF z@UILrf=EmX-mlLis50+*jCVr!6(_G^h5wVZPngP3@yFj-m4%JC)I^UJ!xJP&jj*O9 z^5yu146#!1SPO^8mYK^ zG$*@Ik>Wp=j=?^9%0rlYWm(tMRBXNH`yBt&;VGv#V`CBf_I;dg1&aEd1k9ZFXX?6T}t%oNlD35jir^EPqsZPtlQ(mX$-bdUBDXb1|p7c)uZ zMnoN!JyhJIaUWzx)&g79nt;VLf(Ufw^A{9WC=q0+L2ek>vPmeg z;d&b8$`eB%Q2Z%|?uAGsUnqPzuq=&9X4q{Q*#bWw9#8;aQw7|Y z{xET$@U^-;w_T^b! +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Typesafe sign implementation with signum: +// https://stackoverflow.com/a/4609795 +template int sign(T val) { + return (T(0) < val) - (val < T(0)); +} + +namespace path_tracking_pid +{ + +#define RADIUS_EPS 0.001 // Smallest relevant radius [m] +#define VELOCITY_EPS 1e-3 // Neglegible velocity +#define LONG_DURATION 31556926 // A year (ros::Duration cannot be inf) + +enum ControllerMode +{ + frontAxleLateral = 0, + rearAxleLateral = 1, + rearAxleAngular = 2, + fixOrientation = 3, +}; + +struct TricycleSteeringCmdVel +{ + double steering_angle = 0.0; + double steering_angle_velocity = 0.0; + double speed = 0.0; + double acceleration = 0.0; +}; + +struct ControllerState +{ + size_t current_global_plan_index = 0; + size_t last_visited_pose_index = 0; + double current_x_vel = 0.0; + double current_yaw_vel = 0.0; + double previous_steering_angle = 0.0; + double previous_steering_x_vel = 0.0; + double previous_steering_yaw_vel = 0.0; + bool end_phase_enabled = false; + bool end_reached = false; + double error_integral_lat = 0.0; + double error_integral_ang = 0.0; + double tracking_error_lat = 0.0; + double tracking_error_ang = 0.0; + // Errors with little history + std::array error_lat = {0.0, 0.0, 0.0}; + std::array filtered_error_lat = {0.0, 0.0, 0.0}; + std::array error_deriv_lat = {0.0, 0.0, 0.0}; + std::array filtered_error_deriv_lat = {0.0, 0.0, 0.0}; + std::array error_ang = {0.0, 0.0, 0.0}; + std::array filtered_error_ang = {0.0, 0.0, 0.0}; + std::array error_deriv_ang = {0.0, 0.0, 0.0}; + std::array filtered_error_deriv_ang = {0.0, 0.0, 0.0}; +}; + +class Controller +{ +public: + Controller() = default; + + ~Controller() = default; + + /** + * Set holonomic configuration of the controller + * @param holonomic is holonomic robot? + */ + void setHolonomic(bool holonomic); + + /** + * Enable estimation of pose angles by looking at consecutive path points + * @param estimate_pose_angle + */ + void setEstimatePoseAngle(bool estimate_pose_angle); + + /** + * Set static configuration of the controller + * @param tricycle_model_enabled If tricycle model should be used + * @param estimate_pose_angle The transformation from base to steered wheel + */ + void setTricycleModel(bool tricycle_model_enabled, geometry_msgs::Transform tf_base_to_steered_wheel); + + /** + * Set plan + * @param current Where is the robot now? + * @param odom_twist Robot odometry + * @param global_plan Plan to follow + */ + void setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, + const std::vector& global_plan); + + /** + * Set plan + * @param current Where is the robot now? + * @param odom_twist Robot odometry + * @param tf_base_to_steered_wheel Where is the steered wheel now? + * @param steering_odom_twist Steered wheel odometry + * @param global_plan Plan to follow + */ + void setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, + geometry_msgs::Transform tf_base_to_steered_wheel, geometry_msgs::Twist steering_odom_twist, + const std::vector& global_plan); + /** + * Find position on plan by looking at the surroundings of last known pose. + * @param current Where is the robot now? + * @param controller_state_ptr The current state of the controller that gets updated by this function + * @return tf of found position on plan + * @return index of current path-pose if requested + */ + tf2::Transform findPositionOnPlan(const geometry_msgs::Transform current_tf, + ControllerState* controller_state_ptr, + size_t &path_pose_idx); + // Overloaded function definition for users that don't require the segment index + tf2::Transform findPositionOnPlan(const geometry_msgs::Transform current_tf, + ControllerState* controller_state_ptr) + { + size_t path_pose_idx; + return findPositionOnPlan(current_tf, controller_state_ptr, path_pose_idx); + } + + /** + * Run one iteration of a PID controller + * @param target_x_vel robot will try to reach target x velocity within (de)acceleration limits + * @param current Where is the robot now? + * @param odom_twist Robot odometry + * @param dt Duration since last update + * @return Velocity command + * @return eda Estimated Duration of Arrival + * @return progress Progress along the path [0,1] + * @return pid_debug Variable with information to debug the controller + */ + geometry_msgs::Twist update(const double target_x_vel, + const double target_end_x_vel, + const geometry_msgs::Transform current_tf, + const geometry_msgs::Twist odom_twist, + const ros::Duration dt, + double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug); + + /** + * Run one iteration of a PID controller with velocity limits applied + * @param current Where is the robot now? + * @param odom_twist Robot odometry + * @param dt Duration since last update + * @return Velocity command + * @return eda Estimated Duration of Arrival + * @return progress Progress along the path [0,1] + * @return pid_debug Variable with information to debug the controller + */ + geometry_msgs::Twist update_with_limits(const geometry_msgs::Transform current_tf, + const geometry_msgs::Twist odom_twist, + const ros::Duration dt, + double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug); + + /** + * Perform prediction steps on the lateral error and return a reduced velocity that stays within bounds + * @param current_tf Where is the robot now? + * @param odom_twist Robot odometry + * @return Velocity command + */ + double mpc_based_max_vel(const double target_x_vel, geometry_msgs::Transform current_tf, + geometry_msgs::Twist odom_twist); + + /** + * Select mode for the controller + * @param mode + */ + void selectMode(ControllerMode mode); + + /** + * Set dynamic parameters for the PID controller + * @param config + */ + void configure(path_tracking_pid::PidConfig& config); + + /** + * Set whether the controller is enabled + * @param value enable controller? + */ + void setEnabled(bool value); + + /** + * Reset controller state + */ + void reset(); + + /** + * Gets current dynamic configuration of the controller + * @return current controller configuration + */ + path_tracking_pid::PidConfig getConfig(); + + // Inline get-functions for transforms + tf2::Transform getCurrentGoal() const + { + return current_goal_; + } + tf2::Transform getCurrentWithCarrot() const + { + return current_with_carrot_; + } + tf2::Transform getCurrentPosOnPlan() const + { + return current_pos_on_plan_; + } + + // Inline get-function for controller-state + ControllerState getControllerState() const + { + return controller_state_; + } + + // Set new vel_max_external value + void setVelMaxExternal(double value); + + // Set new vel_max_obstacle value + void setVelMaxObstacle(double value); + + // Get vel_max_obstacle value + double getVelMaxObstacle(); + +private: + double distSquared(const tf2::Transform& pose_v, const tf2::Transform& pose_w) const; + double distSquared(const geometry_msgs::Pose& pose_v, const geometry_msgs::Pose& pose_w) const; + void distToSegmentSquared(const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w, + tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w); + + // Overloaded function for callers that don't need the additional results + double distToSegmentSquared(const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w) + { + tf2::Transform dummy_tf; + double dummy_double; + double result; + distToSegmentSquared(pose_p,pose_v, pose_w, dummy_tf, result, dummy_double); + return result; + } + + geometry_msgs::Twist computeTricycleModelForwardKinematics(double x_vel, double steering_angle); + TricycleSteeringCmdVel computeTricycleModelInverseKinematics(geometry_msgs::Twist cmd_vel); + /** + * Output some debug information about the current parameters + */ + void printParameters(); + + path_tracking_pid::PidConfig local_config_; + ControllerState controller_state_ = ControllerState(); + + // Global Plan variables + std::vector global_plan_tf_; // Global plan vector + std::vector distance_to_goal_vector_; // Vector with distances to goal + std::vector turning_radius_inv_vector_; // Vector with computed turning radius inverse + double distance_to_goal_; + tf2::Transform current_goal_; + tf2::Transform current_pos_on_plan_; + tf2::Transform current_with_carrot_; + + // Auxiliary variables + double current_target_x_vel_ = 0.0; + double control_effort_long_ = 0.0; // output of pid controller + double control_effort_lat_ = 0.0; // output of pid controller + double control_effort_ang_ = 0.0; // output of pid controller + + bool enabled_ = true; + bool feedback_lat_enabled_ = false; + bool feedback_ang_enabled_ = false; + bool feedforward_lat_enabled_ = false; + bool feedforward_ang_enabled_ = false; + bool holonomic_robot_enable_ = false; + bool track_base_link_enabled_ = false; + bool estimate_pose_angle_enabled_ = false; + + // feedforward controller + double feedforward_lat_ = 0.0; + double feedforward_ang_ = 0.0; + double xvel_ = 0.0; + double yvel_ = 0.0; + double thvel_ = 0.0; + + // tricycle model + bool use_tricycle_model_ = false; + geometry_msgs::Transform tf_base_to_steered_wheel_; + double max_steering_angle_; + double max_steering_x_vel_; + double max_steering_x_acc_; + double max_steering_yaw_vel_; + double max_steering_yaw_acc_; + double inverse_kinematics_matrix_[2][2]; + double forward_kinematics_matrix_[2][2]; + + bool debug_enabled_ = false; + + // Primary feedback controller parameters + double Kp_lat_ = 0.0; + double Ki_lat_ = 0.0; + double Kd_lat_ = 0.0; + double Kp_ang_ = 0.0; + double Ki_ang_ = 0.0; + double Kd_ang_ = 0.0; + double l_ = 0.0; + double target_x_vel_ = 0.0; + double target_end_x_vel_ = 0.0; + double target_x_acc_ = 0.0; + double target_x_decc_ = 0.0; + double max_error_x_vel_ = 0.0; + double abs_minimum_x_vel_ = 0.0; + double max_yaw_vel_ = 0.0; + double max_yaw_acc_ = 0.0; + double minimum_turning_radius_ = FLT_EPSILON; + + // Velocity limits that can be active external to the pid controller: + double vel_max_external_ = INFINITY; // Dynamic external max velocity requirement (e.g. no more power available) + double vel_max_obstacle_ = INFINITY; // Can be zero if lethal obstacles are detected + + // Cutoff frequency for the derivative calculation in Hz. + // Negative -> Has not been set by the user yet, so use a default. + double cutoff_frequency_long_ = -1.0; + double cutoff_frequency_lat_ = -1.0; + double cutoff_frequency_ang_ = -1.0; + + // Upper and lower saturation limits + double upper_limit_ = 100.0; + double lower_limit_ = -100.0; + + double ang_upper_limit_ = 100.0; + double ang_lower_limit_ = -100.0; + + // Anti-windup term. Limits the absolute value of the integral term. + double windup_limit_ = 1000.0; + + // Temporary variables + + double proportional_lat_ = 0; // proportional term of output + double integral_lat_ = 0; // integral term of output + double derivative_lat_ = 0; // derivative term of output + double proportional_ang_ = 0; // proportional term of output + double integral_ang_ = 0; // integral term of output + double derivative_ang_ = 0; // derivative term of output + + // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at + // 1/4 of the sample rate. + double c_lat_ = 1.; + double c_ang_ = 1.; + + // Used to check for tan(0)==>NaN in the filter calculation + double tan_filt_ = 1.; + + // MPC settings + int mpc_max_fwd_iter_; // Define # of steps that you look into the future with MPC [-] + int mpc_max_vel_optimization_iter_; // Set maximum # of velocity bisection iterations + // (maximum total iterations = max_opt_iter*max_iter) [-] + double mpc_simulation_sample_time_; // Define timestep [s] + double mpc_max_error_lat_; // Maximum allowed lateral error [m] + double mpc_min_x_vel_; // Minimum forward x velocity [m/s] +}; +} // namespace path_tracking_pid diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp new file mode 100644 index 00000000..084bbf1e --- /dev/null +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -0,0 +1,181 @@ +#pragma once + +#include "./controller.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define MAP_PARALLEL_THRESH 0.2 + + +BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x, y) + +namespace path_tracking_pid +{ +class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController +{ +private: + typedef boost::geometry::model::ring polygon_t; + + inline polygon_t union_(polygon_t polygon1, polygon_t polygon2) + { + std::vector output_vec; + boost::geometry::union_(polygon1, polygon2, output_vec); + return output_vec.at(0); // Only first vector element is filled + } + +public: + TrackingPidLocalPlanner(); + ~TrackingPidLocalPlanner(); + + /** + * @brief Initialize local planner + * @param name The name of the planner + * @param tf a pointer to TransformListener in TF Buffer + * @param costmap Costmap indicating free/occupied space + */ + void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap); + + /** + * @brief Set the plan we should be following + * @param global_plan Plan to follow as closely as we can + * @return whether the plan was successfully updated or not + */ + bool setPlan(const std::vector& global_plan); + + /** + * @brief Calculates the velocity command based on the current robot pose given by pose. See the interface in move + * base. + * @param cmd_vel Output the velocity command + * @return true if succeded. + */ + bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel); // NOLINT + + /** + * @brief Calculates the velocity command based on the current robot pose given by pose. The velocity + * and message are not set. See the interface in move base flex. + * @param pose Current robot pose + * @param velocity + * @param cmd_vel Output the velocity command + * @param message + * @return a status code defined in the move base flex ExePath action. + */ + uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, + geometry_msgs::TwistStamped& cmd_vel, std::string& message); // NOLINT + + /** + * @brief Returns true, if the goal is reached. Currently does not respect the parameters give + * @return true, if the goal is reached + */ + bool isGoalReached(); + + /** + * @brief Returns true, if the goal is reached. Currently does not respect the parameters given. + * @param dist_tolerance Tolerance in distance to the goal + * @param angle_tolerance Tolerance in the orientation to the goals orientation + * @return true, if the goal is reached + */ + bool isGoalReached(double dist_tolerance, double angle_tolerance); + + /** + * @brief Canceles the planner. + * @return True on cancel success. + */ + bool cancel(); + + /** Enumeration for custom SUCCESS feedback codes. See default ones: + * https://github.com/magazino/move_base_flex/blob/master/mbf_msgs/action/ExePath.action + */ + enum + { + SUCCESS = 0, + GRACEFULLY_CANCELLING = 1 + }; + +private: + /** + * Accept a new configuration for the PID controller + * @param config the new configuration + * @param level + */ + void reconfigure_pid(path_tracking_pid::PidConfig& config, uint32_t level); + + void pauseCallback(std_msgs::Bool pause); + + void curOdomCallback(const nav_msgs::Odometry& odom_msg); + + void velMaxExternalCallback(const std_msgs::Float64& msg); + + uint8_t projectedCollisionCost(); + + nav_msgs::Odometry latest_odom_; + ros::Time prev_time_; + ros::Duration prev_dt_; + path_tracking_pid::Controller pid_controller_; + + std::vector global_plan_; + nav_msgs::Path received_path_; + + // Obstacle collision detection + costmap_2d::Costmap2DROS* costmap_; + + // Cancel flags (multi threaded, so atomic bools) + std::atomic active_goal_{false}; + std::atomic cancel_requested_{false}; + std::atomic cancel_in_progress_{false}; + + // dynamic reconfiguration + boost::recursive_mutex config_mutex_; + std::unique_ptr> pid_server_; + + tf2_ros::Buffer* tf_; + geometry_msgs::TransformStamped tfCurPoseStamped_; + + ros::Publisher debug_pub_; // Debugging of controller internal parameters + + // Rviz visualization + ros::Publisher marker_pub_; + ros::Publisher path_pub_; + ros::Publisher collision_marker_pub_; + + ros::Subscriber sub_odom_; + ros::Publisher feedback_pub_; + + ros::Subscriber sub_vel_max_external_; + + std::string map_frame_; + std::string base_link_frame_; + bool initialized_ = false; + + // Used for tricycle model + bool use_tricycle_model_; + std::string steered_wheel_frame_; + geometry_msgs::TransformStamped tf_base_to_steered_wheel_stamped_; + + // Controller logic + bool controller_debug_enabled_ = false; +}; +}; // end namespace path_tracking_pid diff --git a/launch/path_tracking_pid_mbf.launch b/launch/path_tracking_pid_mbf.launch new file mode 100644 index 00000000..a3f7546b --- /dev/null +++ b/launch/path_tracking_pid_mbf.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/maps/grid.png b/maps/grid.png new file mode 100644 index 0000000000000000000000000000000000000000..100ca0c541e68d6cc5534aea646ea1d4db402158 GIT binary patch literal 1217 zcmeAS@N?(olHy`uVBq!ia0y~yVAKI&4mO}jWo=(6kYXuz@(kesf*OvL4j{L})5S5Q zV$R#Uj(&#?1RMg{zWxh0;}-K+vtY}P`<3rm7Hj?7I`zcNt=FoUIV$ekG92*d6q}$J z;n0@ADA~lC!ZElwcgpxbXc%vCeo2=cfr!Xg0ZyTXT5r z*0O#Eer2Mg>sao0hVw?oyWJnKTC9DfbDZsT>#x+bKXNh$2h0-$V`W#v)@$zsA1LAq zrKb94dw2ZCooaz;=D_E(XWj|+J!n1um*ut6ot%2*U+XFG@nOM};)S0C_xIKB<2$&E z!Q5o|22~cylNh=OExrYR1*LVq$Fpvo=K?3-$1`*vMrD8JI50UfaWLjjL~$X1ztJpV Ux{2Z%V41|=>FVdQ&MBb@0GaK6TL1t6 literal 0 HcmV?d00001 diff --git a/maps/grid.yaml b/maps/grid.yaml new file mode 100644 index 00000000..f97769b0 --- /dev/null +++ b/maps/grid.yaml @@ -0,0 +1,6 @@ +image: grid.png +resolution: 0.05 +origin: [-5, -5, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/msg/PidDebug.msg b/msg/PidDebug.msg new file mode 100644 index 00000000..182c4f81 --- /dev/null +++ b/msg/PidDebug.msg @@ -0,0 +1,12 @@ +# Error topic containing the 'control' error on which the PID acts +geometry_msgs/Twist control_error +# Error topic containing the 'tracking' error, i.e. the real error between path and tracked link +geometry_msgs/Twist tracking_error +# Control values +geometry_msgs/Twist proportional +geometry_msgs/Twist integral +geometry_msgs/Twist derivative +geometry_msgs/Twist feedforward +float32 steering_angle +float32 steering_yaw_vel +float32 steering_x_vel diff --git a/msg/PidFeedback.msg b/msg/PidFeedback.msg new file mode 100644 index 00000000..30a5010a --- /dev/null +++ b/msg/PidFeedback.msg @@ -0,0 +1,2 @@ +duration eda # Estimated (optimistic) duration remaining +float32 progress # Progress in distance of the path traveled diff --git a/package.xml b/package.xml new file mode 100644 index 00000000..c1f33750 --- /dev/null +++ b/package.xml @@ -0,0 +1,44 @@ + + + path_tracking_pid + 2.15.0 + Follows a trajectory with open-loop speed and closed loop (pid) lateral control + + Cesar Lopez + + Cesar Lopez + Michiel Francke + + TBD + + catkin + message_generation + roslint + rostest + message_runtime + actionlib + actionlib_msgs + dynamic_reconfigure + geometry_msgs + mbf_costmap_core + mbf_msgs + nav_core + nav_msgs + pluginlib + roscpp + rospy + std_msgs + std_srvs + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + mbf_costmap_nav + message_runtime + mobile_robot_simulator + + + + + + diff --git a/param/controllers.yaml b/param/controllers.yaml new file mode 100644 index 00000000..5d563b7c --- /dev/null +++ b/param/controllers.yaml @@ -0,0 +1,18 @@ +map_frame: 'map' +global_frame: 'map' +controller_frequency: 20.0 +controller_max_retries: 0 +controllers: + - name: 'PathTrackingPID' + type: 'path_tracking_pid/TrackingPidLocalPlanner' +PathTrackingPID: + holonomic_robot: false + track_base_link: true + estimate_pose_angle: false + base_link_frame: 'base_link' + target_x_acc: 1.0 + target_x_decc: 0.3 + abs_minimum_x_vel: 0.0 + anti_collision: true + use_mpc: false + controller_debug_enabled: true diff --git a/param/path_tracking_pid.yaml b/param/path_tracking_pid.yaml new file mode 100644 index 00000000..d6ee6669 --- /dev/null +++ b/param/path_tracking_pid.yaml @@ -0,0 +1,38 @@ +holonomic_robot: false +track_base_link: true +estimate_pose_angle: false +base_link_frame: 'base_link' +Kd_ang: 0.3 +Kd_lat: 0.3 +Ki_ang: 0.0 +Ki_lat: 0.0 +Kp_ang: 1.0 +Kp_lat: 1.0 +l: 2.0 +feedback_ang: false +feedback_lat: true +feedforward_ang: false +feedforward_lat: true +controller_debug_enabled: true +target_end_x_vel: 0.0 +target_x_acc: 2.0 +target_x_decc: 2.0 +target_x_vel: 2.0 +abs_minimum_x_vel: 0.025 +max_error_x_vel: 1.0 +max_yaw_vel: 2.0 +max_yaw_acc: 2.0 +min_turning_radius: 0.0 + +# MPC +use_mpc: false +mpc_max_error_lat: 0.5 +mpc_max_fwd_iter: 200 +mpc_max_vel_optimization_iterations: 5 +mpc_min_x_vel: 0.5 +mpc_simulation_sample_time: 0.05 + +# anti-collision: +anti_collision: true +obstacle_speed_reduction: true +collision_look_ahead_resolution: 1.0 # [m] diff --git a/path_tracking_pid_plugin.xml b/path_tracking_pid_plugin.xml new file mode 100644 index 00000000..ae82430c --- /dev/null +++ b/path_tracking_pid_plugin.xml @@ -0,0 +1,12 @@ + + + + Local planner that tries to stay on the global path as well as it can, using multiple SISO PID controllers for steering calculation and an open-loop strategy for forward velocity + + + + + Local planner that tries to stay on the global path as well as it can, using multiple SISO PID controllers for steering calculation and an open-loop strategy for forward velocity + + + diff --git a/src/controller.cpp b/src/controller.cpp new file mode 100644 index 00000000..21417e77 --- /dev/null +++ b/src/controller.cpp @@ -0,0 +1,1230 @@ +// +// Created by nobleo on 11-9-18. +// + +#include "path_tracking_pid/controller.hpp" +#include +#include +#include + +namespace path_tracking_pid +{ + +void Controller::setHolonomic(bool holonomic) +{ + // Set configuration parameters + ROS_WARN_COND(holonomic, "Holonomic mode is unmaintained. Expect bugs with y-direction"); + holonomic_robot_enable_ = holonomic; +} + +void Controller::setEstimatePoseAngle(bool estimate_pose_angle) +{ + // Set configuration parameters + estimate_pose_angle_enabled_ = estimate_pose_angle; +} + +void Controller::setTricycleModel(bool tricycle_model_enabled, geometry_msgs::Transform tf_base_to_steered_wheel) +{ + // Set tricycle model + use_tricycle_model_ = tricycle_model_enabled; + tf_base_to_steered_wheel_ = tf_base_to_steered_wheel; + double wheel_x = tf_base_to_steered_wheel_.translation.x; + double wheel_y = tf_base_to_steered_wheel_.translation.y; + + double distance_base_to_steered_wheel = hypot(wheel_x, wheel_y); + double wheel_theta = atan2(wheel_y, wheel_x); + inverse_kinematics_matrix_[0][0] = 1; + inverse_kinematics_matrix_[0][1] = - distance_base_to_steered_wheel * sin(wheel_theta); + inverse_kinematics_matrix_[1][0] = 0; + inverse_kinematics_matrix_[1][1] = - distance_base_to_steered_wheel * cos(wheel_theta); + + + double determinant = inverse_kinematics_matrix_[0][0] * inverse_kinematics_matrix_[1][1] + - inverse_kinematics_matrix_[0][1] * inverse_kinematics_matrix_[1][0]; + + if (determinant == 0) + { + ROS_ERROR("Steered wheel at base_link. Invalid for tricycle model, it will be disabled."); + use_tricycle_model_ = false; + return; + } + + forward_kinematics_matrix_[0][0] = inverse_kinematics_matrix_[1][1] / determinant; + forward_kinematics_matrix_[0][1] = -inverse_kinematics_matrix_[0][1] / determinant; + forward_kinematics_matrix_[1][0] = -inverse_kinematics_matrix_[1][0] / determinant; + forward_kinematics_matrix_[1][1] = inverse_kinematics_matrix_[0][0] / determinant; + + controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel_.rotation); +} + +geometry_msgs::Twist Controller::computeTricycleModelForwardKinematics(double x_vel, double steering_angle) +{ + geometry_msgs::Twist estimated_base_twist; + double x_alpha = x_vel*cos(steering_angle); + double y_alpha = x_vel*sin(steering_angle); + + estimated_base_twist.linear.x = forward_kinematics_matrix_[0][0]*x_alpha + forward_kinematics_matrix_[0][1]*y_alpha; + estimated_base_twist.angular.z = + forward_kinematics_matrix_[1][0]*x_alpha + forward_kinematics_matrix_[1][1]*y_alpha; + + return estimated_base_twist; +} + +TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics(geometry_msgs::Twist cmd_vel) +{ + TricycleSteeringCmdVel steering_cmd_vel; + double x_alpha = inverse_kinematics_matrix_[0][0]*cmd_vel.linear.x + + inverse_kinematics_matrix_[0][1]*cmd_vel.angular.z; + double y_alpha = inverse_kinematics_matrix_[1][0]*cmd_vel.linear.x + + inverse_kinematics_matrix_[1][1]*cmd_vel.angular.z; + + steering_cmd_vel.steering_angle = atan2(y_alpha, x_alpha); + steering_cmd_vel.speed = hypot(x_alpha, y_alpha); + + return steering_cmd_vel; +} + +void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, + const std::vector& global_plan) +{ + ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%d)", (int)global_plan.size()); + ROS_DEBUG("Plan is defined in frame '%s'", global_plan.at(0).header.frame_id.c_str()); + + tf2::Transform current_tf2; + tf2::convert(current_tf, current_tf2); + + /* Minimal sanity check */ + global_plan_tf_.clear(); + tf2::Transform transform; + tf2::fromMsg(global_plan[0].pose, transform); + global_plan_tf_.push_back(transform); + // For now do not allow repeated points or in-place rotation + // To allow that the way the progress is checked and the interpolation is done needs to be changed + // Also check if points suddenly go in the opposite direction, this could lead to deadlocks + for (int pose_idx = 1; pose_idx < global_plan.size() - 1; ++pose_idx) + { + auto prev_pose = global_plan[pose_idx - 1].pose; + auto pose = global_plan[pose_idx].pose; + auto next_pose = global_plan[pose_idx + 1].pose; + // Check if the angle of the pose is obtuse, otherwise warn and ignore this pose + // We check using Pythagorean theorem: if c*c > (a*a + b*b) it is an obtuse angle and thus we can follow it + double a_squared = distSquared(prev_pose, pose); + double b_squared = distSquared(pose, next_pose); + double c_squared = distSquared(prev_pose, next_pose); + if (c_squared > (a_squared + b_squared)) + { + tf2::fromMsg(pose, transform); + global_plan_tf_.push_back(transform); + } + else + { + ROS_WARN("Pose %d of path is not used since it is not in the expected direction of the path!", pose_idx); + } + } + // Add last pose as we didn't evaluate that one + tf2::Transform last_transform; + tf2::fromMsg(global_plan.back().pose, last_transform); + global_plan_tf_.push_back(last_transform); + + if (!track_base_link_enabled_) + { + // Add carrot length to plan using goal pose (we assume the last pose contains correct angle) + tf2::Transform carrotTF; + carrotTF.setOrigin(tf2::Vector3(l_, 0.0, 0.0)); + global_plan_tf_.push_back(last_transform * carrotTF); + } + + // Whenever a new path is recieved, computed the closest pose to + // the current carrot pose + controller_state_.current_global_plan_index = 0; + + // find closest current position to global plan + double minimum_distance_to_path = 1e3; + double dist_to_segment; + double iterative_dist_to_goal = 0.0; + distance_to_goal_vector_.clear(); + distance_to_goal_vector_.resize(global_plan_tf_.size()); + distance_to_goal_vector_[global_plan_tf_.size() - 1] = 0.0; + turning_radius_inv_vector_.clear(); + turning_radius_inv_vector_.resize(global_plan_tf_.size()); + turning_radius_inv_vector_[global_plan_tf_.size() - 1] = 0.0; + tf2::Transform deltaPlan; + // We define segment0 to be the segment connecting pose0 and pose1. + // Hence, when picking the starting path's pose, we mean to start at the segment connecting that and the next pose. + for (int idx_path = global_plan_tf_.size() - 2; idx_path >= 0; --idx_path) + { + /* Get distance to segment to determine if this is the segment to start at */ + dist_to_segment = distToSegmentSquared(current_tf2, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]); + // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose + // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! + if (dist_to_segment < minimum_distance_to_path) + { + minimum_distance_to_path = dist_to_segment; + controller_state_.current_global_plan_index = idx_path; + } + + /* Create distance and turning radius vectors once for usage later */ + deltaPlan = global_plan_tf_[idx_path].inverseTimes(global_plan_tf_[idx_path + 1]); + double dpX = deltaPlan.getOrigin().x(); + double dpY = deltaPlan.getOrigin().y(); + iterative_dist_to_goal += hypot(dpX, dpY); + distance_to_goal_vector_[idx_path] = iterative_dist_to_goal; + // compute turning radius based on trigonometric analysis + // radius such that next pose is connected from current pose with a semi-circle + double dpXY2 = dpY*dpY + dpX*dpX; + if (dpXY2 < FLT_EPSILON) + { + turning_radius_inv_vector_[idx_path] = std::numeric_limits::infinity(); + } + else + { + // 0.5*dpY*( 1 + dpX*dpX/(dpY*dPY) ); + // turning_radius_vector[idx_path] = 0.5*(1/dpY)*( dpY*dpY + dpX*dpX ); + turning_radius_inv_vector_[idx_path] = 2*dpY/dpXY2; + } + ROS_DEBUG("turning_radius_inv_vector[%d] = %f", idx_path, turning_radius_inv_vector_[idx_path]); + } + + // Set initial velocity + switch (local_config_.init_vel_method) + { + case Pid_Zero: + reset(); + break; + case Pid_Odom: + reset(); + controller_state_.current_x_vel = odom_twist.linear.x; + controller_state_.current_yaw_vel = odom_twist.angular.z; + ROS_INFO("Resuming on odom velocity x: %f, yaw: %f", odom_twist.linear.x, odom_twist.angular.z); + break; + default: + ROS_DEBUG("Internal controller_state stays valid"); + break; + } + + // When velocity error is too big reset current_x_vel + if (fabs(odom_twist.linear.x - controller_state_.current_x_vel) > max_error_x_vel_) + { + // TODO(clopez/mcfurry/nobleo): Give feedback to higher level software here + ROS_WARN("Large control error. Current_x_vel %f / odometry %f", + controller_state_.current_x_vel, odom_twist.linear.x); + } + controller_state_.end_phase_enabled = false; + controller_state_.end_reached = false; +} + +void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, + geometry_msgs::Transform tf_base_to_steered_wheel, geometry_msgs::Twist steering_odom_twist, + const std::vector& global_plan) +{ + setPlan(current_tf, odom_twist, global_plan); + controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel.rotation); + // TODO(clopez) use steering_odom_twist to check if setpoint is being followed +} + +// https://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment +// TODO(Cesar): expand to 3 dimensions +double Controller::distSquared(const tf2::Transform& pose_v, const tf2::Transform& pose_w) const +{ + // Returns square distance between 2 points + return (pose_v.getOrigin().x() - pose_w.getOrigin().x()) * (pose_v.getOrigin().x() - pose_w.getOrigin().x()) + + (pose_v.getOrigin().y() - pose_w.getOrigin().y()) * (pose_v.getOrigin().y() - pose_w.getOrigin().y()); +} + +double Controller::distSquared(const geometry_msgs::Pose& pose_v, const geometry_msgs::Pose& pose_w) const +{ + // Returns square distance between 2 points + return (pose_v.position.x - pose_w.position.x) * (pose_v.position.x - pose_w.position.x) + + (pose_v.position.y - pose_w.position.y) * (pose_v.position.y - pose_w.position.y); +} + +void Controller::distToSegmentSquared( + const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w, + tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w) +{ + double l2 = distSquared(pose_v, pose_w); + if (l2 == 0) + { + pose_projection = pose_w; + distance_to_w = 0.0; + distance_to_p = distSquared(pose_p, pose_w); + } + else + { + double t = ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) * (pose_w.getOrigin().x() - pose_v.getOrigin().x()) + + (pose_p.getOrigin().y() - pose_v.getOrigin().y()) * (pose_w.getOrigin().y() - pose_v.getOrigin().y())) / + l2; + t = fmax(0.0, fmin(1.0, t)); + pose_projection.setOrigin( + tf2::Vector3(pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()), + pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); + double yaw_projection = tf2::getYaw(pose_v.getRotation()); // get yaw of the first vector + t * + // (tf2::getYaw(pose_w.getRotation()) - + // tf2::getYaw(pose_v.getRotation())); + tf2::Quaternion pose_quaternion; + if (estimate_pose_angle_enabled_) + { + pose_quaternion.setRPY(0.0, 0.0, atan2(pose_w.getOrigin().y() - pose_v.getOrigin().y(), + pose_w.getOrigin().x() - pose_v.getOrigin().x())); + } + else + { + pose_quaternion.setRPY(0.0, 0.0, yaw_projection); + } + + pose_projection.setRotation(pose_quaternion); + distance_to_w = sqrt(distSquared(pose_projection, pose_w)); + distance_to_p = distSquared(pose_p, pose_projection); + } +} + +tf2::Transform Controller::findPositionOnPlan(const geometry_msgs::Transform current_tf, + ControllerState* controller_state_ptr, + size_t &path_pose_idx) +{ + tf2::Transform current_tf2; + tf2::convert(current_tf, current_tf2); + // 'Project' current_tf by removing z-component + tf2::Vector3 originProj = current_tf2.getOrigin(); + originProj.setZ(0.0); + current_tf2.setOrigin(originProj); + + // Computed the closest pose to the current provided pose + // by looking on the surroundings of the last known pose + + // find closest current position to global plan + double minimum_distance_to_path = FLT_MAX; + double distance_to_path; + tf2::Transform error; + + // We define segment0 to be the segment connecting pose0 and pose1. + // Hence, when idx_path==i we are currently tracking the section connection pose i and pose i+1 + + // First look in current position and in front + for (int idx_path = controller_state_ptr->current_global_plan_index; idx_path < global_plan_tf_.size(); idx_path++) + { + error = current_tf2.inverseTimes(global_plan_tf_[idx_path]); + // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose + // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! + distance_to_path = hypot(error.getOrigin().x(), error.getOrigin().y(), error.getOrigin().z()); + + if (distance_to_path <= minimum_distance_to_path) + { + minimum_distance_to_path = distance_to_path; + controller_state_ptr->current_global_plan_index = idx_path; + } + else + { + break; + } + } + + // Then look backwards + for (int idx_path = controller_state_ptr->current_global_plan_index - 1; idx_path >= 0; idx_path--) + { + error = current_tf2.inverseTimes(global_plan_tf_[idx_path]); + // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose + // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! + distance_to_path = hypot(error.getOrigin().x(), error.getOrigin().y(), error.getOrigin().z()); + + if (distance_to_path < minimum_distance_to_path) + { + minimum_distance_to_path = distance_to_path; + controller_state_ptr->current_global_plan_index = idx_path; + } + else + { + break; + } + } + ROS_DEBUG("progress: %lu of %lu", controller_state_ptr->current_global_plan_index, global_plan_tf_.size()-1); + // To finalize, compute the indexes of the start and end points of + // the closest line segment to the current carrot + tf2::Transform current_goal_local; + current_goal_local = global_plan_tf_[controller_state_ptr->current_global_plan_index]; + + tf2::Transform pose_projection_ahead; + tf2::Transform pose_projection_behind; + double distance2_to_line_ahead; + double distance2_to_line_behind; + double distance_to_end_line_ahead; + double distance_to_end_line_behind; + if (controller_state_ptr->current_global_plan_index == 0) + { + distToSegmentSquared(current_tf2, global_plan_tf_[0], global_plan_tf_[1], + pose_projection_ahead, distance2_to_line_ahead, distance_to_end_line_ahead); + current_goal_local = pose_projection_ahead; + distance_to_goal_ = distance_to_goal_vector_[1] + distance_to_end_line_ahead; + controller_state_ptr->last_visited_pose_index = 0; + path_pose_idx = controller_state_ptr->current_global_plan_index; + } + else if (controller_state_ptr->current_global_plan_index == global_plan_tf_.size() - 1) + { + distToSegmentSquared(current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], + global_plan_tf_[controller_state_ptr->current_global_plan_index], + pose_projection_behind, distance2_to_line_behind, distance_to_end_line_behind); + current_goal_local = pose_projection_behind; + distance_to_goal_ = distance_to_end_line_behind; + controller_state_ptr->last_visited_pose_index = global_plan_tf_.size() - 2; + path_pose_idx = controller_state_ptr->current_global_plan_index - 1; + } + else + { + distToSegmentSquared(current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index], + global_plan_tf_[controller_state_ptr->current_global_plan_index + 1], + pose_projection_ahead, distance2_to_line_ahead, distance_to_end_line_ahead); + distToSegmentSquared(current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], + global_plan_tf_[controller_state_ptr->current_global_plan_index], + pose_projection_behind, distance2_to_line_behind, distance_to_end_line_behind); + + if (distance2_to_line_ahead < distance2_to_line_behind) + { + current_goal_local = pose_projection_ahead; + distance_to_goal_ = + distance_to_goal_vector_[controller_state_ptr->current_global_plan_index + 1] + distance_to_end_line_ahead; + controller_state_ptr->last_visited_pose_index = controller_state_ptr->current_global_plan_index; + } + else + { + current_goal_local = pose_projection_behind; + distance_to_goal_ = + distance_to_goal_vector_[controller_state_ptr->current_global_plan_index] + distance_to_end_line_behind; + controller_state_ptr->last_visited_pose_index = controller_state_ptr->current_global_plan_index - 1; + } + path_pose_idx = controller_state_ptr->current_global_plan_index; + } + return current_goal_local; +} + +geometry_msgs::Twist Controller::update(const double target_x_vel, + const double target_end_x_vel, + const geometry_msgs::Transform current_tf, + const geometry_msgs::Twist odom_twist, + const ros::Duration dt, + double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug) +{ + double current_x_vel = controller_state_.current_x_vel; + const double current_yaw_vel = controller_state_.current_yaw_vel; + + // Compute location of the point to be controlled + double theda_rp = tf2::getYaw(current_tf.rotation); + tf2::Vector3 current_with_carrot_origin; + current_with_carrot_origin.setX(current_tf.translation.x + l_ * cos(theda_rp)); + current_with_carrot_origin.setY(current_tf.translation.y + l_ * sin(theda_rp)); + current_with_carrot_origin.setZ(0); + + current_with_carrot_.setOrigin(current_with_carrot_origin); + tf2::Quaternion cur_rot(current_tf.rotation.x, current_tf.rotation.y, current_tf.rotation.z, current_tf.rotation.w); + current_with_carrot_.setRotation(cur_rot); + + size_t path_pose_idx; + if (track_base_link_enabled_) + { + // Find closes robot position to path and then project carrot on goal + current_pos_on_plan_ = current_goal_ = findPositionOnPlan(current_tf, &controller_state_, path_pose_idx); + // To track the base link the goal is then transform to the control point goal + double theda_rp = tf2::getYaw(current_goal_.getRotation()); + tf2::Vector3 newControlOrigin; + newControlOrigin.setX(current_goal_.getOrigin().x() + l_ * cos(theda_rp)); + newControlOrigin.setY(current_goal_.getOrigin().y() + l_ * sin(theda_rp)); + newControlOrigin.setZ(0); + current_goal_.setOrigin(newControlOrigin); + } + else + { + // find position of current position with projected carrot + geometry_msgs::Transform current_with_carrot_g; + tf2::convert(current_with_carrot_, current_with_carrot_g); + current_pos_on_plan_ = current_goal_ = findPositionOnPlan(current_with_carrot_g, &controller_state_, path_pose_idx); + } + + *progress = 1.0 - distance_to_goal_ / distance_to_goal_vector_[0]; + + // Compute errorPose between controlPose and currentGoalPose + tf2::Transform error = current_with_carrot_.inverseTimes(current_goal_); + + //***** Feedback control *****// + if (!((Kp_lat_ <= 0. && Ki_lat_ <= 0. && Kd_lat_ <= 0.) || + (Kp_lat_ >= 0. && Ki_lat_ >= 0. && Kd_lat_ >= 0.))) // All 3 gains should have the same sign + ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); + if (!((Kp_ang_ <= 0. && Ki_ang_ <= 0. && Kd_ang_ <= 0.) || + (Kp_ang_ >= 0. && Ki_ang_ >= 0. && Kd_ang_ >= 0.))) // All 3 gains should have the same sign + ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); + + controller_state_.error_lat.at(2) = controller_state_.error_lat.at(1); + controller_state_.error_lat.at(1) = controller_state_.error_lat.at(0); + controller_state_.error_lat.at(0) = error.getOrigin().y(); // Current error goes to slot 0 + controller_state_.error_ang.at(2) = controller_state_.error_ang.at(1); + controller_state_.error_ang.at(1) = controller_state_.error_ang.at(0); + controller_state_.error_ang.at(0) = + angles::normalize_angle(tf2::getYaw(error.getRotation())); // Current error goes to slot 0 + + // tracking error for diagnostic purposes + // Transform current pose into local-path-frame to get tracked-frame-error + tf2::Quaternion path_quat; + path_quat.setEuler(0.0, 0.0, + atan2(global_plan_tf_[path_pose_idx+1].getOrigin().y() - global_plan_tf_[path_pose_idx].getOrigin().y(), + global_plan_tf_[path_pose_idx+1].getOrigin().x() - global_plan_tf_[path_pose_idx].getOrigin().x())); + tf2::Transform path_segmen_tf = tf2::Transform(path_quat, + tf2::Vector3(global_plan_tf_[path_pose_idx].getOrigin().x(), + global_plan_tf_[path_pose_idx].getOrigin().y(), + global_plan_tf_[path_pose_idx].getOrigin().z())); + + tf2::Vector3 current_tracking_err = - (path_segmen_tf.inverse() * tf2::Vector3(current_tf.translation.x, + current_tf.translation.y, + current_tf.translation.z)); + + // trackin_error here represents the error between tracked link and position on plan + controller_state_.tracking_error_lat = current_tracking_err.y(); + controller_state_.tracking_error_ang = controller_state_.error_ang.at(0); + + // integrate the error + controller_state_.error_integral_lat += controller_state_.error_lat.at(0) * dt.toSec(); + controller_state_.error_integral_ang += controller_state_.error_ang.at(0) * dt.toSec(); + + // Apply windup limit to limit the size of the integral term + if (controller_state_.error_integral_lat > fabsf(windup_limit_)) + controller_state_.error_integral_lat = fabsf(windup_limit_); + if (controller_state_.error_integral_lat < -fabsf(windup_limit_)) + controller_state_.error_integral_lat = -fabsf(windup_limit_); + if (controller_state_.error_integral_ang > fabsf(windup_limit_)) + controller_state_.error_integral_ang = fabsf(windup_limit_); + if (controller_state_.error_integral_ang < -fabsf(windup_limit_)) + controller_state_.error_integral_ang = -fabsf(windup_limit_); + + // My filter reference was Julius O. Smith III, Intro. to Digital Filters With Audio Applications. + if (cutoff_frequency_lat_ != -1) + { + // Check if tan(_) is really small, could cause c = NaN + tan_filt_ = tan((cutoff_frequency_lat_ * 6.2832) * dt.toSec() / 2); + + // Avoid tan(0) ==> NaN + if ((tan_filt_ <= 0.) && (tan_filt_ > -0.01)) + tan_filt_ = -0.01; + if ((tan_filt_ >= 0.) && (tan_filt_ < 0.01)) + tan_filt_ = 0.01; + + c_lat_ = 1 / tan_filt_; + } + if (cutoff_frequency_ang_ != -1) + { + // Check if tan(_) is really small, could cause c = NaN + tan_filt_ = tan((cutoff_frequency_ang_ * 6.2832) * dt.toSec() / 2); + + // Avoid tan(0) ==> NaN + if ((tan_filt_ <= 0.) && (tan_filt_ > -0.01)) + tan_filt_ = -0.01; + if ((tan_filt_ >= 0.) && (tan_filt_ < 0.01)) + tan_filt_ = 0.01; + + c_ang_ = 1 / tan_filt_; + } + + controller_state_.filtered_error_lat.at(2) = controller_state_.filtered_error_lat.at(1); + controller_state_.filtered_error_lat.at(1) = controller_state_.filtered_error_lat.at(0); + controller_state_.filtered_error_lat.at(0) = + (1 / (1 + c_lat_ * c_lat_ + M_SQRT2 * c_lat_)) * + (controller_state_.error_lat.at(2) + 2 * controller_state_.error_lat.at(1) + controller_state_.error_lat.at(0) - + (c_lat_ * c_lat_ - M_SQRT2 * c_lat_ + 1) * controller_state_.filtered_error_lat.at(2) - + (-2 * c_lat_ * c_lat_ + 2) * controller_state_.filtered_error_lat.at(1)); + + controller_state_.filtered_error_ang.at(2) = controller_state_.filtered_error_ang.at(1); + controller_state_.filtered_error_ang.at(1) = controller_state_.filtered_error_ang.at(0); + controller_state_.filtered_error_ang.at(0) = + (1 / (1 + c_ang_ * c_ang_ + M_SQRT2 * c_ang_)) * + (controller_state_.error_ang.at(2) + 2 * controller_state_.error_ang.at(1) + controller_state_.error_ang.at(0) - + (c_ang_ * c_ang_ - M_SQRT2 * c_ang_ + 1) * controller_state_.filtered_error_ang.at(2) - + (-2 * c_ang_ * c_ang_ + 2) * controller_state_.filtered_error_ang.at(1)); + + // Take derivative of error, first the raw unfiltered data: + controller_state_.error_deriv_lat.at(2) = controller_state_.error_deriv_lat.at(1); + controller_state_.error_deriv_lat.at(1) = controller_state_.error_deriv_lat.at(0); + controller_state_.error_deriv_lat.at(0) = + (controller_state_.error_lat.at(0) - controller_state_.error_lat.at(1)) / dt.toSec(); + controller_state_.filtered_error_deriv_lat.at(2) = controller_state_.filtered_error_deriv_lat.at(1); + controller_state_.filtered_error_deriv_lat.at(1) = controller_state_.filtered_error_deriv_lat.at(0); + controller_state_.filtered_error_deriv_lat.at(0) = + (1 / (1 + c_lat_ * c_lat_ + M_SQRT2 * c_lat_)) * + (controller_state_.error_deriv_lat.at(2) + 2 * controller_state_.error_deriv_lat.at(1) + + controller_state_.error_deriv_lat.at(0) - + (c_lat_ * c_lat_ - M_SQRT2 * c_lat_ + 1) * controller_state_.filtered_error_deriv_lat.at(2) - + (-2 * c_lat_ * c_lat_ + 2) * controller_state_.filtered_error_deriv_lat.at(1)); + + controller_state_.error_deriv_ang.at(2) = controller_state_.error_deriv_ang.at(1); + controller_state_.error_deriv_ang.at(1) = controller_state_.error_deriv_ang.at(0); + controller_state_.error_deriv_ang.at(0) = + (controller_state_.error_ang.at(0) - controller_state_.error_ang.at(1)) / dt.toSec(); + controller_state_.filtered_error_deriv_ang.at(2) = controller_state_.filtered_error_deriv_ang.at(1); + controller_state_.filtered_error_deriv_ang.at(1) = controller_state_.filtered_error_deriv_ang.at(0); + controller_state_.filtered_error_deriv_ang.at(0) = + (1 / (1 + c_ang_ * c_ang_ + M_SQRT2 * c_ang_)) * + (controller_state_.error_deriv_ang.at(2) + 2 * controller_state_.error_deriv_ang.at(1) + + controller_state_.error_deriv_ang.at(0) - + (c_ang_ * c_ang_ - M_SQRT2 * c_ang_ + 1) * controller_state_.filtered_error_deriv_ang.at(2) - + (-2 * c_ang_ * c_ang_ + 2) * controller_state_.filtered_error_deriv_ang.at(1)); + + // calculate the control effort + proportional_lat_ = Kp_lat_ * controller_state_.filtered_error_lat.at(0); + integral_lat_ = Ki_lat_ * controller_state_.error_integral_lat; + derivative_lat_ = Kd_lat_ * controller_state_.filtered_error_deriv_lat.at(0); + + proportional_ang_ = Kp_ang_ * controller_state_.filtered_error_ang.at(0); + integral_ang_ = Ki_ang_ * controller_state_.error_integral_ang; + derivative_ang_ = Kd_ang_ * controller_state_.filtered_error_deriv_ang.at(0); + + + /***** Compute forward velocity *****/ + // Apply acceleration limits and end velocity + double current_target_acc; + double acc; + double t_end_phase_current; + double d_end_phase; + + // Compute time to reach end velocity from current velocity + // Compute estimate overall distance during end_phase + // The estimates are done a bit conservative to account that robot will take longer + // to de-accelerate and thus avoid abrupt velocity changes at the end of the trajectory + // The sample time plays an important role on how good these estimates are. + // Thus We add a distance to the end phase distance estimation depending on the sample time + if (current_x_vel > target_end_x_vel) + { + t_end_phase_current = (target_end_x_vel - current_x_vel) / (-target_x_decc_); + d_end_phase = current_x_vel * t_end_phase_current + - 0.5 * (target_x_decc_) * t_end_phase_current * t_end_phase_current + + target_x_vel * 2.0 * dt.toSec(); + } + else + { + t_end_phase_current = (target_end_x_vel - current_x_vel) / (target_x_acc_); + d_end_phase = current_x_vel * t_end_phase_current + + 0.5 * (target_x_acc_) * t_end_phase_current * t_end_phase_current + + target_x_vel * 2.0 * dt.toSec(); + } + ROS_DEBUG("t_end_phase_current: %f", t_end_phase_current); + ROS_DEBUG("d_end_phase: %f", d_end_phase); + ROS_DEBUG("distance_to_goal: %f", distance_to_goal_); + + // Get 'angle' towards current_goal + tf2::Transform robot_pose; + tf2::convert(current_tf, robot_pose); + tf2::Transform base_to_goal = robot_pose.inverseTimes(current_goal_); + double angle_to_goal = atan2(base_to_goal.getOrigin().x(), -base_to_goal.getOrigin().y()); + + // If we are as close to our goal or closer then we need to reach end velocity, enable end_phase. + // However, if robot is not facing to the same direction as the local velocity target vector, don't enable end_phase. + // This is to avoid skipping paths that start with opposite velocity. + if (distance_to_goal_ <= fabs(d_end_phase) + && sign(target_x_vel) == sign(angle_to_goal)) + { + // This state will be remebered to avoid jittering on target_x_vel + controller_state_.end_phase_enabled = true; + } + + if (controller_state_.end_phase_enabled && fabs(target_x_vel) > VELOCITY_EPS) + { + current_target_x_vel_ = target_end_x_vel; + } + else + { + controller_state_.end_phase_enabled = false; + current_target_x_vel_ = target_x_vel; + } + + if (current_x_vel < current_target_x_vel_) + { + current_target_acc = target_x_acc_; + } + else if (current_x_vel > current_target_x_vel_) + { + current_target_acc = -target_x_decc_; + } + else + { + current_target_acc = 0; + } + double acc_desired = (current_target_x_vel_ - current_x_vel) / dt.toSec(); + double acc_abs = fmin(fabs(acc_desired), fabs(current_target_acc)); + acc = copysign(acc_abs, current_target_acc); + + double new_x_vel = current_x_vel + acc * dt.toSec(); + + // For low target_end_x_vel we have a minimum velocity to ensure the goal is reached + double min_vel = copysign(1.0, l_) * abs_minimum_x_vel_; + if (!controller_state_.end_reached && controller_state_.end_phase_enabled + && fabs(target_end_x_vel) <= fabs(min_vel) + VELOCITY_EPS + && fabs(new_x_vel) <= fabs(min_vel) + VELOCITY_EPS) + { + new_x_vel = min_vel; + } + + // When velocity error is too big reset current_x_vel + if (fabs(odom_twist.linear.x) < fabs(current_target_x_vel_) && + fabs(odom_twist.linear.x - new_x_vel) > max_error_x_vel_) + { + // TODO(clopez/mcfurry/nobleo): Give feedback to higher level software here + ROS_WARN_THROTTLE(1.0, "Large tracking error. Current_x_vel %f / odometry %f", + new_x_vel, odom_twist.linear.x); + } + + // Force target_end_x_vel at the very end of the path + // Or when the end velocity is reached. + // Warning! If target_end_x_vel == 0 and min_vel = 0 then the robot might not reach end pose + if ((distance_to_goal_ == 0.0 && target_end_x_vel >= VELOCITY_EPS) + || (controller_state_.end_phase_enabled && new_x_vel >= target_end_x_vel - VELOCITY_EPS + && new_x_vel <= target_end_x_vel + VELOCITY_EPS)) + { + controller_state_.end_reached = true; + controller_state_.end_phase_enabled = false; + *progress = 1.0; + *eda = 0.0; + enabled_ = false; + } + else + { + controller_state_.end_reached = false; + // eda (Estimated duration of arrival) estimation + if (fabs(target_x_vel) > VELOCITY_EPS) + { + double t_const = (copysign(distance_to_goal_, target_x_vel) - d_end_phase) / target_x_vel; + *eda = fmin(fmax(t_end_phase_current, 0.0) + fmax(t_const, 0.0), LONG_DURATION); + } + else + { + *eda = LONG_DURATION; + } + } + /******* end calculation of forward velocity ********/ + + + //***** Overall control *****// + // Controller logic && overall control effort + control_effort_long_ = new_x_vel; + control_effort_lat_ = 0.0; + control_effort_ang_ = 0.0; + + if (feedback_lat_enabled_) + control_effort_lat_ = proportional_lat_ + integral_lat_ + derivative_lat_; + if (feedback_ang_enabled_) + control_effort_ang_ = proportional_ang_ + integral_ang_ + derivative_ang_; + + + //***** Feedforward control *****// + if (feedforward_lat_enabled_) + { + feedforward_lat_ = 0.0; // Not implemented + control_effort_lat_ = control_effort_lat_ + feedforward_lat_; + } + else + feedforward_lat_ = 0.0; + + if (feedforward_ang_enabled_) + { + feedforward_ang_ = turning_radius_inv_vector_[controller_state_.last_visited_pose_index]*control_effort_long_; + ROS_DEBUG("turning_radius_inv_vector[%lu] = %f", + controller_state_.last_visited_pose_index, turning_radius_inv_vector_[controller_state_.last_visited_pose_index]); + + control_effort_ang_ = control_effort_ang_ + feedforward_ang_; + } + else + feedforward_ang_ = 0.0; + + + // Apply saturation limits + if (control_effort_lat_ > upper_limit_) + control_effort_lat_ = upper_limit_; + else if (control_effort_lat_ < lower_limit_) + control_effort_lat_ = lower_limit_; + + if (control_effort_ang_ > ang_upper_limit_) + control_effort_ang_ = ang_upper_limit_; + else if (control_effort_ang_ < ang_lower_limit_) + control_effort_ang_ = ang_lower_limit_; + + // Populate debug output + // Error topic containing the 'control' error on which the PID acts + pid_debug->control_error.linear.x = 0.0; + pid_debug->control_error.linear.y = controller_state_.error_lat.at(0); + pid_debug->control_error.angular.z = controller_state_.error_ang.at(0); + // Error topic containing the 'tracking' error, i.e. the real error between path and tracked link + pid_debug->tracking_error.linear.x = 0.0; + pid_debug->tracking_error.linear.y = controller_state_.tracking_error_lat; + pid_debug->tracking_error.angular.z = controller_state_.tracking_error_ang; + + pid_debug->proportional.linear.x = 0.0; + pid_debug->proportional.linear.y = proportional_lat_; + pid_debug->proportional.angular.z = proportional_ang_; + + pid_debug->integral.linear.x = 0.0; + pid_debug->integral.linear.y = integral_lat_; + pid_debug->integral.angular.z = integral_ang_; + + pid_debug->derivative.linear.x = 0.0; + pid_debug->derivative.linear.y = derivative_lat_; + pid_debug->derivative.angular.z = derivative_ang_; + + pid_debug->feedforward.linear.x = new_x_vel; + pid_debug->feedforward.linear.y = feedforward_lat_; + pid_debug->feedforward.angular.z = feedforward_ang_; + + geometry_msgs::Twist output_combined; + // Generate twist message + if (holonomic_robot_enable_) + { + output_combined.linear.x = control_effort_long_; + output_combined.linear.y = control_effort_lat_; + output_combined.linear.z = 0; + output_combined.angular.x = 0; + output_combined.angular.y = 0; + output_combined.angular.z = control_effort_ang_; + output_combined.angular.z = std::clamp(output_combined.angular.z, -max_yaw_vel_, max_yaw_vel_); + } + else + { + output_combined.linear.x = control_effort_long_; + output_combined.linear.y = 0; + output_combined.linear.z = 0; + output_combined.angular.x = 0; + output_combined.angular.y = 0; + output_combined.angular.z = copysign(1.0, l_) * control_effort_lat_ + + control_effort_ang_; // Take the sign of l for the lateral control effort + output_combined.angular.z = std::clamp(output_combined.angular.z, -max_yaw_vel_, max_yaw_vel_); + // For non-holonomic robots apply saturation based on minimum turning radius + double max_ang_twist_tr; + if (minimum_turning_radius_ < RADIUS_EPS) + { + // Rotation in place is allowed + // minimum_turning_radius = RADIUS_EPS; // This variable is not used anymore so it does not matter + // do not restrict angular velocity. Thus use the biggets number possible + max_ang_twist_tr = std::numeric_limits::infinity(); + } + else + { + max_ang_twist_tr = fabs(output_combined.linear.x / minimum_turning_radius_); + } + output_combined.angular.z = std::clamp(output_combined.angular.z, -max_ang_twist_tr, max_ang_twist_tr); + } + // Apply max acceleration limit to yaw + double yaw_acc = std::clamp((output_combined.angular.z - current_yaw_vel) / dt.toSec(), + -max_yaw_acc_, max_yaw_acc_); + double new_yaw_vel = current_yaw_vel + (yaw_acc * dt.toSec()); + output_combined.angular.z = new_yaw_vel; + + // Transform velocity commands at base_link to steer when using tricycle model + if (use_tricycle_model_) + { + geometry_msgs::Twist output_steering; + TricycleSteeringCmdVel steering_cmd = computeTricycleModelInverseKinematics(output_combined); + if (output_combined.linear.x < 0.0 && steering_cmd.speed > 0.0) + { + steering_cmd.speed = - steering_cmd.speed; + if (steering_cmd.steering_angle > 0) + steering_cmd.steering_angle = steering_cmd.steering_angle - M_PI; + else + steering_cmd.steering_angle = steering_cmd.steering_angle + M_PI; + } + // Apply limits to steering commands + steering_cmd.steering_angle = std::clamp(steering_cmd.steering_angle, + -max_steering_angle_, max_steering_angle_); + double steering_yaw_vel = std::clamp((steering_cmd.steering_angle - controller_state_.previous_steering_angle) + / dt.toSec(), -max_steering_yaw_vel_, max_steering_yaw_vel_); + double steering_angle_acc = std::clamp((steering_yaw_vel - controller_state_.previous_steering_yaw_vel)/ dt.toSec(), + -max_steering_yaw_acc_, max_steering_yaw_acc_); + steering_cmd.steering_angle_velocity = controller_state_.previous_steering_yaw_vel + + (steering_angle_acc * dt.toSec()); + steering_cmd.steering_angle = controller_state_.previous_steering_angle + + (steering_cmd.steering_angle_velocity * dt.toSec()); + + steering_cmd.speed = std::clamp(steering_cmd.speed, -max_steering_x_vel_, max_steering_x_vel_); + steering_cmd.acceleration = std::clamp((steering_cmd.speed - controller_state_.previous_steering_x_vel)/ dt.toSec(), + -max_steering_x_acc_, max_steering_x_acc_); + steering_cmd.speed = controller_state_.previous_steering_x_vel + (steering_cmd.acceleration * dt.toSec()); + + controller_state_.previous_steering_angle = steering_cmd.steering_angle; + controller_state_.previous_steering_yaw_vel = steering_cmd.steering_angle_velocity; + controller_state_.previous_steering_x_vel = steering_cmd.speed; + + // Compute velocities back to base_link and update controller state + output_steering = computeTricycleModelForwardKinematics(steering_cmd.speed, steering_cmd.steering_angle); + controller_state_.current_x_vel = output_steering.linear.x; + controller_state_.current_yaw_vel = output_steering.angular.z; + + pid_debug->steering_angle = steering_cmd.steering_angle; + pid_debug->steering_yaw_vel = steering_cmd.steering_angle_velocity; + pid_debug->steering_x_vel = steering_cmd.speed; + + output_combined = output_steering; + } + + // Publish control effort if controller enabled + if (!enabled_) // Do nothing reset integral action and all filters + { + controller_state_.error_integral_lat = 0.0; + controller_state_.error_integral_ang = 0.0; + } + + controller_state_.current_x_vel = new_x_vel; + controller_state_.current_yaw_vel = new_yaw_vel; + return output_combined; +} + +geometry_msgs::Twist Controller::update_with_limits(const geometry_msgs::Transform current_tf, + const geometry_msgs::Twist odom_twist, + const ros::Duration dt, + double* eda, double* progress, + path_tracking_pid::PidDebug* pid_debug) +{ + // All limits are absolute + double max_x_vel = std::abs(target_x_vel_); + + // Apply external limit + max_x_vel = std::min(max_x_vel, vel_max_external_); + + // Apply obstacle limit + max_x_vel = std::min(max_x_vel, vel_max_obstacle_); + + // Apply mpc limit (last because less iterations required if max vel is already limited) + double vel_max_mpc = std::numeric_limits::infinity(); + if (local_config_.use_mpc) + { + vel_max_mpc = std::abs(mpc_based_max_vel(std::copysign(max_x_vel, target_x_vel_), current_tf, odom_twist)); + max_x_vel = std::min(max_x_vel, vel_max_mpc); + } + + // Some logging: + ROS_DEBUG("max_x_vel=%.3f, target_x_vel=%.3f, vel_max_external=%.3f, vel_max_obstacle=%.3f, vel_max_mpc=%.3f", + max_x_vel, target_x_vel_, vel_max_external_, vel_max_obstacle_, vel_max_mpc); + if (max_x_vel != target_x_vel_) + { + if (max_x_vel == vel_max_external_) + { + ROS_WARN_THROTTLE(5.0, "External velocity limit active %.2fm/s", vel_max_external_); + } + else if (max_x_vel == vel_max_obstacle_) + { + ROS_WARN_THROTTLE(5.0, "Obstacle velocity limit active %.2fm/s", vel_max_obstacle_); + } + else if (max_x_vel == vel_max_mpc) + { + ROS_WARN_THROTTLE(5.0, "MPC velocity limit active %.2fm/s", vel_max_mpc); + } + } + + // The end velocity is bound by the same limits to avoid accelerating above the limit in the end phase + double max_end_x_vel = std::min({std::abs(target_end_x_vel_), vel_max_external_, vel_max_obstacle_, vel_max_mpc}); // NOLINT + max_end_x_vel = std::copysign(max_end_x_vel, target_end_x_vel_); + + // Update the controller with the new setting + max_x_vel = std::copysign(max_x_vel, target_x_vel_); + return update(max_x_vel, max_end_x_vel, current_tf, odom_twist, dt, eda, progress, pid_debug); +} + +// output updated velocity command: (Current position, current measured velocity, closest point index, estimated +// duration of arrival, debug info) +double Controller::mpc_based_max_vel(const double target_x_vel, geometry_msgs::Transform current_tf, + geometry_msgs::Twist odom_twist) +{ + // Temporary save global data + ControllerState controller_state_saved; + controller_state_saved = controller_state_; + + // Bisection optimisation parameters + double target_x_vel_prev = 0.0; // Previous iteration velocity command + int mpc_vel_optimization_iter = 0; + + // MPC parameters + int mpc_fwd_iter = 0; // Reset MPC iterations + + // Create predicted position vector + geometry_msgs::Transform predicted_tf = current_tf; + geometry_msgs::Twist pred_twist = odom_twist; + + double new_nominal_x_vel = target_x_vel; // Start off from the current velocity + double mpc_vel_limit = new_nominal_x_vel; + + // Loop MPC + while (mpc_fwd_iter < mpc_max_fwd_iter_ && mpc_vel_optimization_iter <= mpc_max_vel_optimization_iter_) + { + mpc_fwd_iter += 1; + + // Check if robot stays within bounds for all iterations, if the new_nominal_x_vel is smaller than + // max_target_x_vel we can increase it + if (mpc_fwd_iter == mpc_max_fwd_iter_ && fabs(controller_state_.error_lat.at(0)) <= mpc_max_error_lat_ && + fabs(new_nominal_x_vel) < abs(target_x_vel)) + { + mpc_vel_optimization_iter += 1; + + // When we reach the maximum allowed mpc optimization iterations, do not change velocity anymore + if (mpc_vel_optimization_iter > mpc_max_vel_optimization_iter_) + { + break; + } + + // Increase speed + double prev_save = new_nominal_x_vel; + new_nominal_x_vel = copysign(1.0, new_nominal_x_vel) + * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + new_nominal_x_vel; + target_x_vel_prev = prev_save; + + // Reset variables + controller_state_ = controller_state_saved; + + predicted_tf = current_tf; + pred_twist = odom_twist; + mpc_fwd_iter = 0; + } + // If the robot gets out of bounds earlier we decrease the velocity + else if (abs(controller_state_.error_lat.at(0)) >= mpc_max_error_lat_) + { + mpc_vel_optimization_iter += 1; + + // Lower speed + double prev_save = new_nominal_x_vel; + new_nominal_x_vel = -copysign(1.0, new_nominal_x_vel) + * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + new_nominal_x_vel; + target_x_vel_prev = prev_save; + + // Reset variables + controller_state_ = controller_state_saved; + + predicted_tf = current_tf; + pred_twist = odom_twist; + mpc_fwd_iter = 0; + + // Warning if new_nominal_x_vel becomes really low + if (abs(new_nominal_x_vel) < 0.01) + { + ROS_WARN_THROTTLE(5.0, "Lowering velocity did not decrease the lateral error enough."); + } + } + else if (mpc_fwd_iter != mpc_max_fwd_iter_) + { + // Run controller + // Output: pred_twist.[linear.x, linear.y, linear.z, angular.x, angular.y, angular.z] + path_tracking_pid::PidDebug pid_debug_unused; + double eda_unused, progress_unused; + pred_twist = Controller::update(new_nominal_x_vel, target_end_x_vel_, predicted_tf, pred_twist, + ros::Duration(mpc_simulation_sample_time_), + &eda_unused, &progress_unused, &pid_debug_unused); + + // Run plant model + double theta = tf2::getYaw(predicted_tf.rotation); + predicted_tf.translation.x += pred_twist.linear.x * cos(theta) * mpc_simulation_sample_time_; + predicted_tf.translation.y += pred_twist.linear.x * sin(theta) * mpc_simulation_sample_time_; + tf2::Quaternion q; + q.setRPY(0, 0, theta + pred_twist.angular.z * mpc_simulation_sample_time_); + predicted_tf.rotation = tf2::toMsg(q); + } + } + // Apply limits to the velocity + mpc_vel_limit = copysign(1.0, target_x_vel) * + fmin(fabs(target_x_vel), fabs(new_nominal_x_vel)); + mpc_vel_limit = copysign(1.0, new_nominal_x_vel) * + fmax(fabs(new_nominal_x_vel), mpc_min_x_vel_); + + // Revert global variables + controller_state_ = controller_state_saved; + + return std::abs(mpc_vel_limit); +} + +void Controller::selectMode(ControllerMode mode) +{ + switch (mode) + { + case ControllerMode::frontAxleLateral: + // Front axle lateral controller (default) + l_ = 0.5; + feedback_lat_enabled_ = true; + feedback_ang_enabled_ = false; + feedforward_lat_enabled_ = true; + feedforward_ang_enabled_ = false; + break; + case ControllerMode::rearAxleLateral: + // Rear axle lateral control + l_ = 0.0; // To prevent singular configuration + feedback_lat_enabled_ = true; + feedback_ang_enabled_ = false; + feedforward_lat_enabled_ = true; + feedforward_ang_enabled_ = false; + break; + case ControllerMode::rearAxleAngular: + // Rear axle angular controller + l_ = 0.0; + feedback_lat_enabled_ = false; + feedback_ang_enabled_ = true; + feedforward_lat_enabled_ = false; + feedforward_ang_enabled_ = false; + break; + case ControllerMode::fixOrientation: + // Fix orientation controller + l_ = 0.0; + feedback_lat_enabled_ = false; + feedback_ang_enabled_ = true; + feedforward_lat_enabled_ = false; + feedforward_ang_enabled_ = true; + break; + } + + printParameters(); +} + +void Controller::printParameters() +{ + ROS_INFO("CONTROLLER PARAMETERS"); + ROS_INFO("-----------------------------------------"); + ROS_INFO("Controller enabled: %i", enabled_); + ROS_INFO("Controller DEBUG enabled: %i", debug_enabled_); + ROS_INFO("Distance L: %f", l_); + ROS_INFO("Track base_link enabled?: %i", track_base_link_enabled_); + + ROS_INFO("Target forward velocities (xv: %f, xv_end,: %f)", target_x_vel_, target_end_x_vel_); + ROS_INFO("Target forward (de)accelerations (xacc: %f, xdecc,: %f)", target_x_acc_, target_x_decc_); + ROS_INFO("Maximum allowed forward velocity error: %f", max_error_x_vel_); + ROS_INFO("Feedback (lat, ang): ( %i, %i)", feedback_lat_enabled_, feedback_ang_enabled_); + ROS_INFO("Feedforward (lat, ang): (%i, %i)", feedforward_lat_enabled_, feedforward_ang_enabled_); + ROS_INFO("Lateral gains: (Kp: %f, Ki, %f, Kd, %f)", Kp_lat_, Ki_lat_, Kd_lat_); + ROS_INFO("Angular gains: (Kp: %f, Ki, %f, Kd, %f)", Kp_ang_, Ki_ang_, Kd_ang_); + + ROS_INFO("Robot type (holonomic): (%i)", holonomic_robot_enable_); + + ROS_INFO("Integral-windup limit: %f", windup_limit_); + ROS_INFO("Saturation limits xy: %f/%f", upper_limit_, lower_limit_); + ROS_INFO("Saturation limits ang: %f/%f", ang_upper_limit_, ang_lower_limit_); + ROS_INFO("-----------------------------------------"); +} + +void Controller::configure(path_tracking_pid::PidConfig& config) +{ + // Erase all queues when config changes + + std::fill(controller_state_.error_lat.begin(), controller_state_.error_lat.end(), 0); + std::fill(controller_state_.filtered_error_lat.begin(), controller_state_.filtered_error_lat.end(), 0); + std::fill(controller_state_.error_deriv_lat.begin(), controller_state_.error_deriv_lat.end(), 0); + std::fill(controller_state_.filtered_error_deriv_lat.begin(), controller_state_.filtered_error_deriv_lat.end(), 0); + + std::fill(controller_state_.error_ang.begin(), controller_state_.error_ang.end(), 0); + std::fill(controller_state_.filtered_error_ang.begin(), controller_state_.filtered_error_ang.end(), 0); + std::fill(controller_state_.error_deriv_ang.begin(), controller_state_.error_deriv_ang.end(), 0); + std::fill(controller_state_.filtered_error_deriv_ang.begin(), controller_state_.filtered_error_deriv_ang.end(), 0); + + Kp_lat_ = config.Kp_lat; + Ki_lat_ = config.Ki_lat; + Kd_lat_ = config.Kd_lat; + Kp_ang_ = config.Kp_ang; + Ki_ang_ = config.Ki_ang; + Kd_ang_ = config.Kd_ang; + + + track_base_link_enabled_ = config.track_base_link; + ROS_DEBUG("Track base_link? Then global path poses are needed! '%d'", (int)track_base_link_enabled_); + + l_ = config.l; + target_x_vel_ = config.target_x_vel; + l_ = copysign(1.0, target_x_vel_) * fabs(l_); + if (controller_state_.end_phase_enabled) + { + ROS_WARN_COND(abs(config.target_end_x_vel - target_end_x_vel_) > 1e-3, "Won't change end velocity in end phase"); + ROS_WARN_COND(abs(config.target_x_acc - target_x_acc_) > 1e-3, "Won't change accelerations in end phase"); + ROS_WARN_COND(abs(config.target_x_decc - target_x_decc_) > 1e-3, "Won't change accelerations in end phase"); + config.target_end_x_vel = target_end_x_vel_; + config.target_x_acc = target_x_acc_; + config.target_x_decc = target_x_decc_; + } + else + { + target_end_x_vel_ = config.target_end_x_vel; + target_x_acc_ = config.target_x_acc; + target_x_decc_ = config.target_x_decc; + } + abs_minimum_x_vel_ = config.abs_minimum_x_vel; + max_yaw_vel_ = config.max_yaw_vel; + max_yaw_acc_ = config.max_yaw_acc; + + max_error_x_vel_ = config.max_error_x_vel; + + minimum_turning_radius_ = config.min_turning_radius; + + debug_enabled_ = config.controller_debug_enabled; + feedforward_lat_enabled_ = config.feedforward_lat; + feedforward_ang_enabled_ = config.feedforward_ang; + feedback_lat_enabled_ = config.feedback_lat; + feedback_ang_enabled_ = config.feedback_ang; + + // MPC + config.groups.mpc_group.state = config.use_mpc; // Hide config options if disabled + mpc_max_fwd_iter_ = config.mpc_max_fwd_iterations; + mpc_max_vel_optimization_iter_ = config.mpc_max_vel_optimization_iterations; + mpc_simulation_sample_time_ = config.mpc_simulation_sample_time; + mpc_max_error_lat_ = config.mpc_max_error_lat; + mpc_min_x_vel_ = config.mpc_min_x_vel; + mpc_min_x_vel_ = fmin(mpc_min_x_vel_, fabs(target_x_vel_)); + + // Obstacle speed reduction + config.groups.collision_group.state = config.anti_collision; // Hide config options if disabled + + // Tricycle model + max_steering_angle_ = config.max_steering_angle; + max_steering_x_vel_ = config.max_steering_x_vel; + max_steering_x_acc_ = config.max_steering_x_acc; + max_steering_yaw_vel_ = config.max_steering_yaw_vel; + max_steering_yaw_acc_ = config.max_steering_yaw_acc; + + local_config_ = config; + + // printParameters(); +} + +path_tracking_pid::PidConfig Controller::getConfig() +{ + return local_config_; +} + +void Controller::setEnabled(bool value) +{ + ROS_DEBUG("Controller::setEnabled(%d)", value); + enabled_ = value; +} + +void Controller::reset() +{ + controller_state_.current_x_vel = 0.0; + controller_state_.current_yaw_vel = 0.0; + controller_state_.previous_steering_angle = 0.0; + controller_state_.previous_steering_yaw_vel = 0.0; + controller_state_.previous_steering_x_vel = 0.0; + controller_state_.error_integral_lat = 0.0; + controller_state_.error_integral_ang = 0.0; + controller_state_.error_lat = {0.0, 0.0, 0.0}; + controller_state_.filtered_error_lat = {0.0, 0.0, 0.0}; + controller_state_.error_deriv_lat = {0.0, 0.0, 0.0}; + controller_state_.filtered_error_deriv_lat = {0.0, 0.0, 0.0}; + controller_state_.error_ang = {0.0, 0.0, 0.0}; + controller_state_.filtered_error_ang = {0.0, 0.0, 0.0}; + controller_state_.error_deriv_ang = {0.0, 0.0, 0.0}; + controller_state_.filtered_error_deriv_ang = {0.0, 0.0, 0.0}; +} + +void Controller::setVelMaxExternal(double value) +{ + if (value < 0.0) + { + ROS_ERROR_THROTTLE(1.0, "External velocity limit (%f) has to be positive", value); + return; + } + if (value < 0.1) + ROS_WARN_THROTTLE(1.0, "External velocity limit is very small (%f), this could result in standstill", value); + vel_max_external_ = value; +} + +void Controller::setVelMaxObstacle(double value) +{ + ROS_WARN_COND(vel_max_obstacle_ != 0.0 && value == 0.0, "Collision imminent, slamming the brakes"); + vel_max_obstacle_ = value; +} + +double Controller::getVelMaxObstacle() +{ + return vel_max_obstacle_; +} + +} // namespace path_tracking_pid diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp new file mode 100644 index 00000000..25d347e2 --- /dev/null +++ b/src/path_tracking_pid_local_planner.cpp @@ -0,0 +1,635 @@ +// +// Created by nobleo on 12-9-18. +// + +#include "path_tracking_pid/path_tracking_pid_local_planner.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// register planner as move_base and move_base plugins +PLUGINLIB_EXPORT_CLASS(path_tracking_pid::TrackingPidLocalPlanner, nav_core::BaseLocalPlanner) +PLUGINLIB_EXPORT_CLASS(path_tracking_pid::TrackingPidLocalPlanner, mbf_costmap_core::CostmapController) + +namespace path_tracking_pid +{ +TrackingPidLocalPlanner::TrackingPidLocalPlanner() = default; + +TrackingPidLocalPlanner::~TrackingPidLocalPlanner() = default; + +void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig& config, uint32_t level) +{ + pid_controller_.configure(config); + controller_debug_enabled_ = config.controller_debug_enabled; + + if (controller_debug_enabled_ && !global_plan_.empty()) + { + received_path_.header = global_plan_.at(0).header; + received_path_.poses = global_plan_; + path_pub_.publish(received_path_); + } +} + +void TrackingPidLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap) +{ + ros::NodeHandle nh("~/" + name); + ros::NodeHandle gn; + ROS_DEBUG("TrackingPidLocalPlanner::initialize(%s, ..., ...)", name.c_str()); + // setup dynamic reconfigure + pid_server_ = std::make_unique>(config_mutex_, nh); + dynamic_reconfigure::Server::CallbackType cb1; + cb1 = boost::bind(&TrackingPidLocalPlanner::reconfigure_pid, this, _1, _2); + pid_server_->setCallback(cb1); + pid_controller_.setEnabled(false); + + bool holonomic_robot; + nh.param("holonomic_robot", holonomic_robot, false); + pid_controller_.setHolonomic(holonomic_robot); + + bool estimate_pose_angle; + nh.param("estimate_pose_angle", estimate_pose_angle, false); + pid_controller_.setEstimatePoseAngle(estimate_pose_angle); + + nh.param("base_link_frame", base_link_frame_, "base_link"); + + nh.param("use_tricycle_model", use_tricycle_model_, false); + nh.param("steered_wheel_frame", steered_wheel_frame_, "steer"); + + + collision_marker_pub_ = nh.advertise("collision_markers", 3); + marker_pub_ = nh.advertise("visualization_marker", 3); + debug_pub_ = nh.advertise("debug", 1); + path_pub_ = nh.advertise("visualization_path", 1, true); + + sub_odom_ = gn.subscribe("odom", 1, &TrackingPidLocalPlanner::curOdomCallback, this); + sub_vel_max_external_ = nh.subscribe("vel_max", 1, &TrackingPidLocalPlanner::velMaxExternalCallback, this); + feedback_pub_ = nh.advertise("feedback", 1); + + map_frame_ = costmap->getGlobalFrameID(); + costmap_ = costmap; + tf_ = tf; + + initialized_ = true; +} + +bool TrackingPidLocalPlanner::setPlan(const std::vector& global_plan) +{ + if (!initialized_) + { + ROS_ERROR("path_tracking_pid has not been initialized, please call initialize() before using this planner"); + return false; + } + + std::string path_frame = global_plan.at(0).header.frame_id; + ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%zu)", global_plan.size()); + ROS_DEBUG("Plan is defined in frame '%s'", path_frame.c_str()); + + global_plan_ = global_plan; + + /* If frame of received plan is not equal to mbf-map_frame, translate first */ + if (map_frame_.compare(path_frame)) + { + ROS_DEBUG("Transforming plan since my global_frame = '%s' and my plan is in frame: '%s'", map_frame_.c_str(), + path_frame.c_str()); + geometry_msgs::TransformStamped tf_transform; + tf_transform = tf_->lookupTransform(map_frame_, path_frame, ros::Time(0)); + // Check alignment, when path-frame is severly mis-aligned show error + double yaw, pitch, roll; + tf2::getEulerYPR(tf_transform.transform.rotation, yaw, pitch, roll); + if (std::fabs(pitch) > MAP_PARALLEL_THRESH || std::fabs(roll) > MAP_PARALLEL_THRESH) + { + ROS_ERROR("Path is given in %s frame which is severly mis-aligned with our map-frame: %s", path_frame.c_str(), + map_frame_.c_str()); + } + for (auto& pose_stamped : global_plan_) + { + tf2::doTransform(pose_stamped.pose, pose_stamped.pose, tf_transform); + pose_stamped.header.frame_id = map_frame_; + // 'Project' plan by removing z-component + pose_stamped.pose.position.z = 0.0; + } + } + + if (controller_debug_enabled_) + { + received_path_.header = global_plan_.at(0).header; + received_path_.poses = global_plan_; + path_pub_.publish(received_path_); + } + + try + { + ROS_DEBUG("map_frame: %s, plan_frame: %s, base_link_frame: %s", map_frame_.c_str(), path_frame.c_str(), + base_link_frame_.c_str()); + tfCurPoseStamped_ = tf_->lookupTransform(map_frame_, base_link_frame_, ros::Time(0)); + } + catch (tf2::TransformException ex) + { + ROS_ERROR("Received an exception trying to transform: %s", ex.what()); + return false; + } + + // Feasability check, but only when not resuming with odom-vel + if (pid_controller_.getConfig().init_vel_method != Pid_Odom && + std::abs(latest_odom_.twist.twist.linear.x - pid_controller_.getControllerState().current_x_vel) > 0.5) + { + ROS_ERROR("Significant diff between odom (%f) and controller_state (%f) detected. Aborting!", + latest_odom_.twist.twist.linear.x, pid_controller_.getControllerState().current_x_vel); + return false; + } + + if (use_tricycle_model_) + { + try + { + ROS_DEBUG("base_link_frame: %s, steered_wheel_frame: %s", base_link_frame_.c_str(), steered_wheel_frame_.c_str()); + tf_base_to_steered_wheel_stamped_ = tf_->lookupTransform(base_link_frame_, steered_wheel_frame_, ros::Time(0)); + } + catch (tf2::TransformException ex) + { + ROS_ERROR("Received an exception trying to transform: %s", ex.what()); + ROS_ERROR("Invalid transformation from base_link_frame to steered_wheel_frame. Tricycle model will be disabled"); + use_tricycle_model_ = false; + } + + pid_controller_.setTricycleModel(use_tricycle_model_, tf_base_to_steered_wheel_stamped_.transform); + + // TODO(clopez): subscribe to steered wheel odom + geometry_msgs::Twist steering_odom_twist; + pid_controller_.setPlan(tfCurPoseStamped_.transform, latest_odom_.twist.twist, + tf_base_to_steered_wheel_stamped_.transform, steering_odom_twist, global_plan_); + } + else + { + pid_controller_.setPlan(tfCurPoseStamped_.transform, latest_odom_.twist.twist, global_plan_); + } + + pid_controller_.setEnabled(true); + active_goal_ = true; + prev_time_ = ros::Time(0); + return true; +} + +bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) +{ + ros::Time now = ros::Time::now(); + if (prev_time_.isZero()) + { + prev_time_ = now - prev_dt_; // Initialisation round + } + ros::Duration dt = now - prev_time_; + if (dt.isZero()) + { + ROS_ERROR_THROTTLE(5, "dt=0 detected, skipping loop(s). Possible overloaded cpu or simulating too fast"); + cmd_vel = geometry_msgs::Twist(); + cmd_vel.linear.x = pid_controller_.getControllerState().current_x_vel; + cmd_vel.angular.z = pid_controller_.getControllerState().current_yaw_vel; + return true; // False is no use: https://github.com/magazino/move_base_flex/issues/195 + } + try + { + ROS_DEBUG("map_frame: %s, base_link_frame: %s", map_frame_.c_str(), base_link_frame_.c_str()); + tfCurPoseStamped_ = tf_->lookupTransform(map_frame_, base_link_frame_, ros::Time(0)); + } + catch (tf2::TransformException ex) + { + ROS_ERROR("Received an exception trying to transform: %s", ex.what()); + active_goal_ = false; + return false; + } + + // Handle obstacles + if (pid_controller_.getConfig().anti_collision) + { + auto cost = projectedCollisionCost(); + + if (cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + pid_controller_.setVelMaxObstacle(0.0); + } + else if (pid_controller_.getConfig().obstacle_speed_reduction) + { + double max_vel = pid_controller_.getConfig().max_x_vel; + double reduction_factor = static_cast(cost) / costmap_2d::INSCRIBED_INFLATED_OBSTACLE; + double limit = max_vel * (1 - reduction_factor); + ROS_DEBUG("Cost: %d, factor: %f, limit: %f", cost, reduction_factor, limit); + pid_controller_.setVelMaxObstacle(limit); + } + else + { + pid_controller_.setVelMaxObstacle(INFINITY); // set back to inf + } + } + else + { + pid_controller_.setVelMaxObstacle(INFINITY); // Can be disabled live, so set back to inf + } + + path_tracking_pid::PidDebug pid_debug; + double eda = 1 / FLT_EPSILON; // initial guess. Avoids errors in case function returns due to wrong delta_t; + double progress = 0.0; + cmd_vel = pid_controller_.update_with_limits(tfCurPoseStamped_.transform, latest_odom_.twist.twist, + dt, &eda, &progress, &pid_debug); + + path_tracking_pid::PidFeedback feedback_msg; + feedback_msg.eda = ros::Duration(eda); + feedback_msg.progress = progress; + feedback_pub_.publish(feedback_msg); + + if (cancel_requested_) + { + path_tracking_pid::PidConfig config = pid_controller_.getConfig(); + config.target_x_vel = 0.0; + config.target_end_x_vel = 0.0; + boost::recursive_mutex::scoped_lock lock(config_mutex_); + // When updating from own server no callback is called. Thus controller is updated first and then server is notified + pid_controller_.configure(config); + pid_server_->updateConfig(config); + lock.unlock(); + cancel_requested_ = false; + } + + + if (controller_debug_enabled_) + { + debug_pub_.publish(pid_debug); + + visualization_msgs::Marker mkCurPose, mkControlPose, mkGoalPose, mkPosOnPlan; + + // configure rviz visualization + mkCurPose.header.frame_id = mkControlPose.header.frame_id = map_frame_; + mkGoalPose.header.frame_id = mkPosOnPlan.header.frame_id = map_frame_; + mkCurPose.header.stamp = mkControlPose.header.stamp = ros::Time::now(); + mkGoalPose.header.stamp = mkPosOnPlan.header.stamp = ros::Time::now(); + mkCurPose.ns = "axle point"; + mkControlPose.ns = "control point"; + mkGoalPose.ns = "goal point"; + mkPosOnPlan.ns = "plan point"; + mkCurPose.action = mkControlPose.action = visualization_msgs::Marker::ADD; + mkGoalPose.action = mkPosOnPlan.action = visualization_msgs::Marker::ADD; + mkCurPose.pose.orientation.w = mkControlPose.pose.orientation.w = 1.0; + mkGoalPose.pose.orientation.w = mkPosOnPlan.pose.orientation.w = 1.0; + mkCurPose.id = __COUNTER__; // id has to be unique, so using a compile-time counter :) + mkControlPose.id = __COUNTER__; + mkGoalPose.id = __COUNTER__; + mkPosOnPlan.id = __COUNTER__; + mkCurPose.type = mkControlPose.type = visualization_msgs::Marker::POINTS; + mkGoalPose.type = mkPosOnPlan.type = visualization_msgs::Marker::POINTS; + mkCurPose.scale.x = 0.5; + mkCurPose.scale.y = 0.5; + mkControlPose.scale.x = 0.5; + mkControlPose.scale.y = 0.5; + mkGoalPose.scale.x = 0.5; + mkGoalPose.scale.y = 0.5; + mkCurPose.color.b = 1.0; + mkCurPose.color.a = 1.0; + mkControlPose.color.g = 1.0f; + mkControlPose.color.a = 1.0; + mkGoalPose.color.r = 1.0; + mkGoalPose.color.a = 1.0; + mkPosOnPlan.scale.x = 0.5; + mkPosOnPlan.scale.y = 0.5; + mkPosOnPlan.color.a = 1.0; + mkPosOnPlan.color.r = 1.0f; + mkPosOnPlan.color.g = 0.5f; + + geometry_msgs::Point p; + std_msgs::ColorRGBA color; + p.x = tfCurPoseStamped_.transform.translation.x; + p.y = tfCurPoseStamped_.transform.translation.y; + p.z = tfCurPoseStamped_.transform.translation.z; + mkCurPose.points.push_back(p); + + tf2::Transform tfControlPose = pid_controller_.getCurrentWithCarrot(); + p.x = tfControlPose.getOrigin().x(); + p.y = tfControlPose.getOrigin().y(); + p.z = tfControlPose.getOrigin().z(); + mkControlPose.points.push_back(p); + + tf2::Transform tfGoalPose = pid_controller_.getCurrentGoal(); + p.x = tfGoalPose.getOrigin().x(); + p.y = tfGoalPose.getOrigin().y(); + p.z = tfGoalPose.getOrigin().z(); + mkGoalPose.points.push_back(p); + + tf2::Transform tfCurPose = pid_controller_.getCurrentPosOnPlan(); + p.x = tfCurPose.getOrigin().x(); + p.y = tfCurPose.getOrigin().y(); + p.z = tfCurPose.getOrigin().z(); + mkPosOnPlan.points.push_back(p); + + marker_pub_.publish(mkCurPose); + marker_pub_.publish(mkControlPose); + marker_pub_.publish(mkGoalPose); + marker_pub_.publish(mkPosOnPlan); + } + + prev_time_ = now; + prev_dt_ = dt; // Store last known valid dt for next cycles (https://github.com/magazino/move_base_flex/issues/195) + return true; +} + +uint8_t TrackingPidLocalPlanner::projectedCollisionCost() +{ + // configure rviz visualization + visualization_msgs::Marker mkSteps; + mkSteps.header.frame_id = map_frame_; + mkSteps.header.stamp = ros::Time::now(); + mkSteps.ns = "extrapolated poses"; + mkSteps.action = visualization_msgs::Marker::ADD; + mkSteps.pose.orientation.w = 1.0; + mkSteps.id = __COUNTER__; + mkSteps.type = visualization_msgs::Marker::POINTS; + mkSteps.scale.x = 0.5; + mkSteps.scale.y = 0.5; + mkSteps.color.r = 1.0; + mkSteps.color.g = 0.5; + mkSteps.color.a = 1.0; + + visualization_msgs::Marker mkPosesOnPath; + mkPosesOnPath.header.frame_id = map_frame_; + mkPosesOnPath.header.stamp = ros::Time::now(); + mkPosesOnPath.ns = "goal poses on path"; + mkPosesOnPath.action = visualization_msgs::Marker::ADD; + mkPosesOnPath.pose.orientation.w = 1.0; + mkPosesOnPath.id = __COUNTER__; + mkPosesOnPath.type = visualization_msgs::Marker::POINTS; + mkPosesOnPath.scale.x = 0.5; + mkPosesOnPath.scale.y = 0.5; + mkPosesOnPath.color.r = 1.0; + mkPosesOnPath.color.g = 1.0; + mkPosesOnPath.color.a = 1.0; + + visualization_msgs::Marker mkCollisionFootprint; + mkCollisionFootprint.header.frame_id = map_frame_; + mkCollisionFootprint.header.stamp = ros::Time::now(); + mkCollisionFootprint.ns = "Collision footprint"; + mkCollisionFootprint.action = visualization_msgs::Marker::ADD; + mkCollisionFootprint.pose.orientation.w = 1.0; + mkCollisionFootprint.id = __COUNTER__; + mkCollisionFootprint.type = visualization_msgs::Marker::LINE_LIST; + mkCollisionFootprint.scale.x = 0.1; + mkCollisionFootprint.color.b = 1.0; + mkCollisionFootprint.color.a = 0.3; + + visualization_msgs::Marker mkCollisionHull; + mkCollisionHull.header.frame_id = map_frame_; + mkCollisionHull.header.stamp = ros::Time::now(); + mkCollisionHull.ns = "Collision polygon"; + mkCollisionHull.action = visualization_msgs::Marker::ADD; + mkCollisionHull.pose.orientation.w = 1.0; + mkCollisionHull.id = __COUNTER__; + mkCollisionHull.type = visualization_msgs::Marker::LINE_STRIP; + mkCollisionHull.scale.x = 0.2; + mkCollisionHull.color.r = 1.0; + mkCollisionHull.color.a = 0.3; + + visualization_msgs::Marker mkCollisionIndicator; + mkCollisionIndicator.header.frame_id = map_frame_; + mkCollisionIndicator.header.stamp = ros::Time::now(); + mkCollisionIndicator.ns = "Collision object"; + mkCollisionIndicator.pose.orientation.w = 1.0; + mkCollisionIndicator.id = __COUNTER__; + mkCollisionIndicator.type = visualization_msgs::Marker::CYLINDER; + mkCollisionIndicator.scale.x = 0.5; + mkCollisionIndicator.scale.y = 0.5; + mkCollisionIndicator.color.r = 1.0; + mkCollisionIndicator.color.a = 0.0; + visualization_msgs::MarkerArray mkCollision; + + // Check how far we should check forward + double x_vel = pid_controller_.getControllerState().current_x_vel; + double collision_look_ahead_distance = x_vel*x_vel / (2*pid_controller_.getConfig().target_x_decc) + + pid_controller_.getConfig().collision_look_ahead_length_offset; + uint n_steps = std::ceil(collision_look_ahead_distance / pid_controller_.getConfig().collision_look_ahead_resolution); + double x_resolution = collision_look_ahead_distance / n_steps; + + // Define a x_step transform which will be used to step forward the position. + tf2::Transform x_step_tf; + x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, x_vel), 0.0, 0.0)); + + // Use a controller state to forward project the position on the path + ControllerState projected_controller_state = pid_controller_.getControllerState(); + geometry_msgs::Transform current_tf = tfCurPoseStamped_.transform; + + // Step until lookahead is reached, for every step project the pose back to the path + std::vector projected_steps_tf; + tf2::Transform projected_step_tf; + tf2::fromMsg(current_tf, projected_step_tf); + projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link + projected_step_tf = pid_controller_.findPositionOnPlan(current_tf, &projected_controller_state); + projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose + for (uint step = 0; step < n_steps; step++) + { + tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf; + projected_step_tf = pid_controller_.findPositionOnPlan(tf2::toMsg(next_straight_step_tf), + &projected_controller_state); + projected_steps_tf.push_back(projected_step_tf); + + // Fill markers: + geometry_msgs::Point mkStep; + tf2::toMsg(next_straight_step_tf.getOrigin(), mkStep); + mkSteps.points.push_back(mkStep); + geometry_msgs::Point mkPointOnPath; + tf2::toMsg(projected_step_tf.getOrigin(), mkPointOnPath); + mkPosesOnPath.points.push_back(mkPointOnPath); + } + + polygon_t previous_footprint_xy; + polygon_t collision_polygon; + for (const auto& projection_tf : projected_steps_tf) + { + // Project footprint forward + double x = projection_tf.getOrigin().x(); + double y = projection_tf.getOrigin().y(); + double yaw = tf2::getYaw(projection_tf.getRotation()); + std::vector footprint; + costmap_2d::transformFootprint(x, y, yaw, costmap_->getRobotFootprint(), footprint); + + // Append footprint to polygon + polygon_t two_footprints = previous_footprint_xy; + previous_footprint_xy.clear(); + for (const auto& point : footprint) + { + boost::geometry::append(two_footprints, point); + boost::geometry::append(previous_footprint_xy, point); + } + + boost::geometry::correct(two_footprints); + polygon_t two_footprint_hull; + boost::geometry::convex_hull(two_footprints, two_footprint_hull); + collision_polygon = union_(collision_polygon, two_footprint_hull); + + // Add footprint to marker + geometry_msgs::Point previous_point = footprint.back(); + for (const auto& point : footprint) + { + mkCollisionFootprint.points.push_back(previous_point); + mkCollisionFootprint.points.push_back(point); + previous_point = point; + } + } + + // Create a convex hull so we can use costmap2d->convexFillCells + costmap_2d::Costmap2D* costmap2d = costmap_->getCostmap(); + polygon_t collision_polygon_hull; + boost::geometry::convex_hull(collision_polygon, collision_polygon_hull); + std::vector collision_polygon_hull_map; + + // Convert to map coordinates + for (const auto& point : collision_polygon_hull) + { + int map_x, map_y; + costmap2d->worldToMapEnforceBounds(point.x, point.y, map_x, map_y); + costmap_2d::MapLocation map_point{static_cast(map_x), static_cast(map_y)}; + collision_polygon_hull_map.push_back(map_point); + } + + // Get the relevant cells + std::vector cells_in_polygon; + costmap2d->convexFillCells(collision_polygon_hull_map, cells_in_polygon); + + // Get the max cost inside the concave polygon + uint8_t max_cost = 0.0; + for (const auto& cell_in_polygon : cells_in_polygon) + { + // Cost checker is cheaper than polygon checker, so lets do that first + uint8_t cell_cost = costmap2d->getCost(cell_in_polygon.x, cell_in_polygon.y); + if (cell_cost > max_cost && cell_cost != costmap_2d::NO_INFORMATION) + { + // Check if in concave polygon + geometry_msgs::Point point; + costmap2d->mapToWorld(cell_in_polygon.x, cell_in_polygon.y, point.x, point.y); + if (boost::geometry::within(point, collision_polygon)) + { + // Protip: uncomment below and 'if (cell_cost > max_cost)' to see evaluated cells + // boost::unique_lock lock_controller(*(costmap2d->getMutex())); + // costmap2d->setCost(cell_in_polygon.x, cell_in_polygon.y, 100); + + max_cost = cell_cost; + // Set collision indicator on suspected cell with current cost + mkCollisionIndicator.scale.z = cell_cost / 255.0; + mkCollisionIndicator.color.a = cell_cost / 255.0; + point.z = mkCollisionIndicator.scale.z * 0.5; + mkCollisionIndicator.pose.position = point; + if (max_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + break; // Collision detected, no need to evaluate further + } + } + } + } + if (mkCollisionIndicator.scale.z > std::numeric_limits::epsilon()) + { + mkCollisionIndicator.action = visualization_msgs::Marker::ADD; + } + else + { + mkCollisionIndicator.action = visualization_msgs::Marker::DELETE; + } + mkCollision.markers.push_back(mkCollisionIndicator); + + // Fiddle the polygon into a marker message + for (const geometry_msgs::Point point : collision_polygon) + { + mkCollisionHull.points.push_back(point); + } + + mkCollision.markers.push_back(mkCollisionFootprint); + mkCollision.markers.push_back(mkCollisionHull); + if (n_steps > 0) + { + mkCollision.markers.push_back(mkSteps); + mkCollision.markers.push_back(mkPosesOnPath); + } + collision_marker_pub_.publish(mkCollision); + + return max_cost; +} + +uint32_t TrackingPidLocalPlanner::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, + const geometry_msgs::TwistStamped& velocity, + geometry_msgs::TwistStamped& cmd_vel, std::string& message) +{ + if (!initialized_) + { + ROS_ERROR("path_tracking_pid has not been initialized, please call initialize() before using this planner"); + active_goal_ = false; + return mbf_msgs::ExePathResult::NOT_INITIALIZED; + } + // TODO(Cesar): Use provided pose and odom + if (!computeVelocityCommands(cmd_vel.twist)) + { + active_goal_ = false; + return mbf_msgs::ExePathResult::FAILURE; + } + cmd_vel.header.stamp = ros::Time::now(); + cmd_vel.header.frame_id = base_link_frame_; + + bool moving = std::abs(cmd_vel.twist.linear.x) > VELOCITY_EPS; + if (cancel_in_progress_) + { + if (!moving) + { + ROS_INFO("Cancel requested and we now (almost) reached velocity 0: %f", cmd_vel.twist.linear.x); + cancel_in_progress_ = false; + active_goal_ = false; + return mbf_msgs::ExePathResult::CANCELED; + } + ROS_INFO_THROTTLE(1.0, "Cancel in progress... remaining x_vel: %f", cmd_vel.twist.linear.x); + return TrackingPidLocalPlanner::GRACEFULLY_CANCELLING; + } + + if (!moving && pid_controller_.getVelMaxObstacle() < VELOCITY_EPS) + { + active_goal_ = false; + return mbf_msgs::ExePathResult::BLOCKED_PATH; + } + + if (isGoalReached()) + active_goal_ = false; + return mbf_msgs::ExePathResult::SUCCESS; +} + +bool TrackingPidLocalPlanner::isGoalReached() +{ + // Return reached boolean, but never succeed when we're preempting + return pid_controller_.getControllerState().end_reached && !cancel_in_progress_; +} + +bool TrackingPidLocalPlanner::isGoalReached(double dist_tolerance, double angle_tolerance) +{ + return isGoalReached(); +} + +bool TrackingPidLocalPlanner::cancel() +{ + // This function runs in a separate thread + cancel_requested_ = true; + cancel_in_progress_ = true; + ros::Rate r(10); + ROS_INFO("Cancel requested, waiting in loop for cancel to finish"); + while (active_goal_) + { + r.sleep(); + } + ROS_INFO("Finished waiting loop, done cancelling"); + return true; +} + +void TrackingPidLocalPlanner::curOdomCallback(const nav_msgs::Odometry& odom_msg) +{ + latest_odom_ = odom_msg; +} + +void TrackingPidLocalPlanner::velMaxExternalCallback(const std_msgs::Float64& msg) +{ + pid_controller_.setVelMaxExternal(msg.data); +} +} // namespace path_tracking_pid diff --git a/test/__init__.py b/test/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/test/local_costmap_params.yaml b/test/local_costmap_params.yaml new file mode 100644 index 00000000..fb179cf8 --- /dev/null +++ b/test/local_costmap_params.yaml @@ -0,0 +1,41 @@ +local_costmap: + # http://wiki.ros.org/costmap_2d/hydro/obstacles + footprint: [[-6, -2.5], [-6, 2.5], [3, 2], [4.5, 0], [3, -2]] + footprint_padding: 0.1 + transform_tolerance: 1.0 + global_frame: odom + robot_base_frame: base_link + update_frequency: 5.0 + publish_frequency: 5.0 + width: 40.0 + height: 40.0 + resolution: 0.5 # This resolution should be bigger than the spatial resolution of velodyne. Otherwise incorrect costmap clearing occurs + rolling_window: true + + plugins: + - name: obstacles_layer + type: "costmap_2d::ObstacleLayer" + - name: inflation_layer + type: "costmap_2d::InflationLayer" + + obstacles_layer: + enabled: true + footprint_clearing_enabled: false + obstacle_range: 50 + observation_sources: pointcloud + pointcloud: + data_type: PointCloud + topic: /pointcloud + marking: true + clearing: false + + inflation_layer: + enabled: true + inflation_radius: 20 + cost_scaling_factor: 0.1 + +# Disable global costmap: +global_costmap: + plugins: + - name: empty_layer + type: "costmap_2d::InflationLayer" diff --git a/test/paths.py b/test/paths.py new file mode 100644 index 00000000..541aedba --- /dev/null +++ b/test/paths.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python +from copy import deepcopy +from math import cos, pi, sin + +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Path +from numpy import linspace +from tf.transformations import quaternion_from_euler + + +def points_on_circle(angles, radius=1, center=(0, 0)): + return [ + ( + center[0] + (sin(angle) * radius), # x + center[1] - (cos(angle) * radius), # y + angle, # yaw + ) + for angle in angles + ] + + +def back_and_forth(radius): + # Construct path to go 10m forward, turn and go back + path = Path() + pose = PoseStamped() + pose.header.frame_id = "map" + pose.pose.orientation.w = 1.0 + pose.pose.position.x = 0.0 + path.poses.append(deepcopy(pose)) + + for (x, y, yaw) in points_on_circle(linspace(0, pi), radius=radius, center=(10, radius)): + pose.pose.position.x = x + pose.pose.position.y = y + quat = quaternion_from_euler(0, 0, yaw) + pose.pose.orientation.x = quat[0] + pose.pose.orientation.y = quat[1] + pose.pose.orientation.z = quat[2] + pose.pose.orientation.w = quat[3] + path.poses.append(deepcopy(pose)) + + pose.pose.position.x = 0 + path.poses.append(deepcopy(pose)) + return path + + +def reverse_path(path): + path.poses.reverse() + return path + + +def create_path(x, y, yaw): + # Construct a path based on three vectors + path = Path() + pose = PoseStamped() + pose.header.frame_id = "map" + + for (x, y, yaw) in zip(x, y, yaw): + pose.pose.position.x = x + pose.pose.position.y = y + quat = quaternion_from_euler(0, 0, yaw) + pose.pose.orientation.x = quat[0] + pose.pose.orientation.y = quat[1] + pose.pose.orientation.z = quat[2] + pose.pose.orientation.w = quat[3] + path.poses.append(deepcopy(pose)) + + return path diff --git a/test/test.rviz b/test/test.rviz new file mode 100644 index 00000000..562854eb --- /dev/null +++ b/test/test.rviz @@ -0,0 +1,238 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 316 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: /move_base_flex/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Map + Topic: /move_base_flex/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.05000000074505806 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.019999999552965164 + Shaft Length: 0.05000000074505806 + Topic: /move_base_flex/PathTrackingPID/visualization_path + Unreliable: false + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 10000 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.019999999552965164 + Head Radius: 0.05000000074505806 + Shaft Length: 0.05000000074505806 + Shaft Radius: 0.019999999552965164 + Value: Arrow + Topic: /odom + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Footprint + Topic: /move_base_flex/global_costmap/footprint + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /move_base_flex/PathTrackingPID/visualization_marker + Name: Marker + Namespaces: + axle point: true + control point: true + goal point: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /move_base_flex/PathTrackingPID/collision_markers + Name: MarkerArray + Namespaces: + Collision footprint: true + Collision polygon: true + extrapolated poses: true + goal poses on path: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /set_pose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: -1.5700007677078247 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 20.735715866088867 + Target Frame: + Value: TopDownOrtho (rviz) + X: 7.837769985198975 + Y: 3.2284839153289795 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 738 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000023ffc0200000008fb0000001200530065006c0065006300740069006f006e01000000410000006e0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ec000001c200000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000b5000001cb000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000022f0000023f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: true + Time: + collapsed: true + Tool Properties: + collapsed: true + Views: + collapsed: true + Width: 927 + X: 309 + Y: 143 diff --git a/test/test_path_tracking_pid.py b/test/test_path_tracking_pid.py new file mode 100755 index 00000000..7c5a407d --- /dev/null +++ b/test/test_path_tracking_pid.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python +import unittest + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from geometry_msgs.msg import Point32, PoseWithCovarianceStamped, Twist +from mbf_msgs.msg import ExePathAction, ExePathGoal +from path_tracking_pid.msg import PidDebug +from sensor_msgs.msg import PointCloud +from std_msgs.msg import Float64 +from tf.transformations import quaternion_from_euler + +from paths import back_and_forth, reverse_path + + +class SlowDownChecker(object): + def __init__(self, target_vel): + self.target_vel = target_vel + self.slowed_down = False + self.kept_slow = True # Flag to see if we did not accidentaly speed up again + self.vel = 0.0 + rospy.Subscriber("cmd_vel", Twist, self.cb) + + def cb(self, msg): + decelerating = msg.linear.x < self.vel + # if msg.linear.x < self.vel: + self.vel = msg.linear.x # deceleration phase + if not self.slowed_down and decelerating and self.vel < self.target_vel + 0.01: + rospy.loginfo("Slowed down to {}m/s".format(self.target_vel)) + self.slowed_down = True + if self.slowed_down and self.kept_slow and self.vel > self.target_vel + 0.1: + rospy.logwarn("Sped up again while limit is still active") + self.kept_slow = False + +class ErrorCatcher(object): + def __init__(self): + self.error = Twist() + rospy.Subscriber("move_base_flex/PathTrackingPID/debug", PidDebug, self.cb) + + def cb(self, msg): + self.error = msg.tracking_error + +class TestPathTrackingPID(unittest.TestCase): + def test_exepath_action(self): + # Set inital pose to parameter + if rospy.has_param("~initialpose"): + values = rospy.get_param("~initialpose") + rospy.loginfo("Initial pose to {}".format(values)) + pose = PoseWithCovarianceStamped() + pose.header.frame_id = "map" + pose.pose.pose.position.x = values[0] + pose.pose.pose.position.y = values[1] + quat = quaternion_from_euler(0, 0, values[2]) + pose.pose.pose.orientation.x = quat[0] + pose.pose.pose.orientation.y = quat[1] + pose.pose.pose.orientation.z = quat[2] + pose.pose.pose.orientation.w = quat[3] + initialpose_pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, latch=True, queue_size=1) + initialpose_pub.publish(pose) + rospy.sleep(0.1) # Fill tf buffers + + # Publisher for obstacles: + self.obstacle_pub = rospy.Publisher("pointcloud", PointCloud, latch=True, queue_size=1) + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + # Spawn desired obstacles: + pc = PointCloud() + pc.header.stamp = rospy.Time.now() + pc.header.frame_id = "map" + for point in rospy.get_param("~obstacles", []): + pc.points.append(Point32(x=point[0], y=point[1])) + self.obstacle_pub.publish(pc) + + path = back_and_forth(rospy.get_param("~radius", 5.0)) + + # Listen to errors: + error_catcher = ErrorCatcher() + + # Start goal and evaluate outcome + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + client.send_goal(ExePathGoal(path=path)) + + max_vel = rospy.get_param("~max_vel", 0.0) + if max_vel > 0.0: + reconfigure.update_configuration({"target_end_x_vel": 5.0}) + checker = SlowDownChecker(max_vel) + max_vel_pub = rospy.Publisher("move_base_flex/PathTrackingPID/vel_max", Float64, queue_size=1) + rospy.sleep(3.0) # Accelerate first + max_vel_pub.publish(max_vel) + rospy.sleep(10.0) + self.assertTrue(checker.slowed_down) + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + + if max_vel > 0.0: + self.assertTrue(checker.kept_slow) + return + + # Check the errors + self.assertLess(error_catcher.error.linear.x, 0.1) + self.assertLess(error_catcher.error.linear.y, 0.1) + self.assertLess(error_catcher.error.angular.z, 0.1) + + # Do the same for backward movements if last path was a success + if client.get_state() != GS.SUCCEEDED or rospy.get_param("backward", True): + return + + reconfigure.update_configuration({"target_x_vel": -5.0}) + client.send_goal(ExePathGoal(path=reverse_path(path))) + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid", anonymous=False) + rostest.rosrun("back_and_forth", "rostest_path_tracking_pid", TestPathTrackingPID) diff --git a/test/test_path_tracking_pid.test b/test/test_path_tracking_pid.test new file mode 100644 index 00000000..bfbc5410 --- /dev/null +++ b/test/test_path_tracking_pid.test @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + [0, 2, 0] + False + + + [0, 2, 3] + False + + + [0, 0, 1.57] + False + + + [20, 0, 0] + False + + + + [3, -25, 0] + False + + + + [[10.0, 10.0]] + 4 + + + [[15.0, -8.0]] + + + 1.0 + true + + + 1.0 + + + + + + diff --git a/test/test_path_tracking_pid_accel.py b/test/test_path_tracking_pid_accel.py new file mode 100755 index 00000000..8f3e4067 --- /dev/null +++ b/test/test_path_tracking_pid_accel.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python +import unittest +from math import hypot + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry + +from paths import create_path + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + + def reconfigure(self): + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": 2.0}) + reconfigure.update_configuration({"target_end_x_vel": 0}) + reconfigure.update_configuration({"target_x_acc": 1.0}) + reconfigure.update_configuration({"target_x_decc": 1.0}) + + def odom_cb(self, msg): + self.cur_odom = msg + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + # Subscribe to pose messages from + self.pose_sub = rospy.Subscriber("/odom", Odometry, self.odom_cb) + + path = create_path([0.0, 10.0], [0.0, 0.0], [0.0, 0.0]) + + # Start goal and evaluate outcome + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + self.reconfigure() + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + # Preempt action after 2s + rospy.sleep(2.0) + client.cancel_goal() + rospy.sleep(1.0) + self.assertTrue(1.0 < abs(self.cur_odom.twist.twist.linear.x) < 2.0, msg="Violated deceleration on preempt") + rospy.sleep(2.0) + self.assertTrue(abs(self.cur_odom.twist.twist.linear.x) < 0.1, msg="Violated deceleration on preempt") + preempt_in_time = client.wait_for_result(timeout=rospy.Duration(10)) + self.assertTrue(preempt_in_time, msg="Action call didn't preempt in time") + self.assertEqual(client.get_state(), GS.ABORTED, msg="Action didn't preempt on request") + + # Resume action + self.reconfigure() + client.send_goal(ExePathGoal(path=path)) + rospy.sleep(1.0) + self.assertEqual(client.get_state(), GS.ACTIVE, msg="Action didn't restart on request") + self.assertTrue(0.1 < abs(self.cur_odom.twist.twist.linear.x) < 2.0, msg="Violated acceleration on restart") + rospy.sleep(1.0) + self.assertTrue(abs(self.cur_odom.twist.twist.linear.x) > 1.9, msg="Violated acceleration on restart") + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + + # Get end-pose error + endpose_error = hypot(path.poses[-1].pose.position.x - self.cur_odom.pose.pose.position.x, + path.poses[-1].pose.position.y - self.cur_odom.pose.pose.position.y) + + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + self.assertTrue(endpose_error < 0.5, msg="Did not arrive on final path's pose! \ + pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_accel", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_accel", TestPathTrackingPID) diff --git a/test/test_path_tracking_pid_preempt.py b/test/test_path_tracking_pid_preempt.py new file mode 100755 index 00000000..dfa35294 --- /dev/null +++ b/test/test_path_tracking_pid_preempt.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python +import unittest +from math import hypot + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry + +from paths import create_path + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + + def reconfigure(self, end_vel=0.0): + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": 2.0}) + reconfigure.update_configuration({"target_end_x_vel": end_vel}) + reconfigure.update_configuration({"target_x_acc": 2.0}) + reconfigure.update_configuration({"target_x_decc": 1.0}) + + def odom_cb(self, msg): + self.cur_odom = msg + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + # Subscribe to pose messages from + self.pose_sub = rospy.Subscriber("/odom", Odometry, self.odom_cb) + + path = create_path([0.0, 2.0], [0.0, 0.0], [0.0, 0.0]) + + # Start goal and evaluate outcome + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + self.reconfigure(end_vel = 1.0) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + # Preempt action after 1.5s (in deceleration phase) + rospy.sleep(1.5) + rospy.logwarn("Preempting path!") + client.cancel_goal() + rospy.logwarn("Wait for result") + preempt_in_time = client.wait_for_result(timeout=rospy.Duration(10)) + rospy.logwarn("Got result") + self.assertTrue(preempt_in_time, msg="Action call didn't preempt in time") + # self.assertEqual(client.get_state(), GS.ABORTED, msg="Action didn't preempt on request") + + # # Resume action + rospy.logwarn("Reconfiguring for resume") + self.reconfigure(end_vel = 0.0) + rospy.logwarn("Creating resume path") + path = create_path([2.0, 4.0], [0.0, 0.0], [0.0, 0.0]) + rospy.logwarn("Sending new goal") + client.send_goal(ExePathGoal(path=path)) + rospy.logwarn("Goal sent!") + rospy.sleep(1.0) + rospy.logwarn("Waiting for new result") + self.assertEqual(client.get_state(), GS.ACTIVE, msg="Action didn't restart on request") + rospy.logwarn("FINISHED!") + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(5)) + + # Get end-pose error + endpose_error = hypot(path.poses[-1].pose.position.x - self.cur_odom.pose.pose.position.x, + path.poses[-1].pose.position.y - self.cur_odom.pose.pose.position.y) + + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + self.assertTrue(endpose_error < 0.5, msg="Did not arrive on final path's pose! \ + pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_preempt", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_preempt", TestPathTrackingPID) diff --git a/test/test_path_tracking_pid_turn_skip.py b/test/test_path_tracking_pid_turn_skip.py new file mode 100755 index 00000000..1fbfc925 --- /dev/null +++ b/test/test_path_tracking_pid_turn_skip.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python +import unittest +from math import cos, hypot, pi, sin + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry +from numpy import linspace + +from paths import create_path + + +def points_on_circle(angles, radius=1, center=(0, 0)): + x = [] + y = [] + yaw = [] + for angle in angles: + x.append(center[0] + (cos(angle) * radius)) + y.append(center[1] + (sin(angle) * radius)) + yaw.append(angle + pi*0.5) + return (x, y, yaw) + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + + def odom_cb(self, msg): + self.cur_odom = msg + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + # Subscribe to pose messages from + self.pose_sub = rospy.Subscriber("/odom", Odometry, self.odom_cb) + + path0a = create_path([0.0, 3.5], [0.0, 0.0], [0.0, 0.0]) + path0b = create_path([3.5, 4.5], [0.0, 0.0], [0.0, 0.0]) + path0c = create_path([4.5, 5.0], [0.0, 0.0], [0.0, 0.0]) + (x_circ, y_circ, yaw_circ) = points_on_circle(linspace(-0.5*pi, -0.75*pi), radius=2.0, center=(5.0, 2.0)) + path1 = create_path(x_circ, y_circ, yaw_circ) + path2 = create_path([cos(-0.75*pi)*2.0+5.0, cos(-0.75*pi)*2.0+10.0], + [sin(-0.75*pi)*2.0+2.0, sin(-0.75*pi)*2.0-3.0], + [-0.25*pi, -0.25*pi]) + + paths = [path0a, path0b, path0c, path1, path2] + speeds = [1.2, 1.7, 0.9, -0.6, 1.2] + endspeeds = [0.0, 0.833, 0.0, 0.0, 1.66] + + for (path, speed, endspeed) in zip(paths, speeds, endspeeds): + # Start goal and evaluate outcome + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": speed}) + reconfigure.update_configuration({"target_end_x_vel": endspeed}) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + + # Get end-pose error + endpose_error = hypot(path.poses[-1].pose.position.x - self.cur_odom.pose.pose.position.x, + path.poses[-1].pose.position.y - self.cur_odom.pose.pose.position.y) + + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + self.assertTrue(endpose_error < 0.5, msg="Did not arrive on final path's pose! \ + pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_turn_skip", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_turn_skip", TestPathTrackingPID) diff --git a/trajectories/back_and_forth.yaml b/trajectories/back_and_forth.yaml new file mode 100644 index 00000000..33e8b31f --- /dev/null +++ b/trajectories/back_and_forth.yaml @@ -0,0 +1,228 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 10 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/trajectories/coverage.yaml b/trajectories/coverage.yaml new file mode 100644 index 00000000..e2733dff --- /dev/null +++ b/trajectories/coverage.yaml @@ -0,0 +1,228 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 10 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 5.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/path_frame' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/trajectories/coverage_actiongoal.yaml b/trajectories/coverage_actiongoal.yaml new file mode 100644 index 00000000..dffb807a --- /dev/null +++ b/trajectories/coverage_actiongoal.yaml @@ -0,0 +1,241 @@ +header: + seq: 1 + stamp: + secs: 1545129449 + nsecs: 998500108 + frame_id: '' +goal_id: + stamp: + secs: 1545129449 + nsecs: 998473882 + id: "/axclient_15430_1545129402991-1-1545129449.998" +goal: + path: + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 10 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 5.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: "/map" + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/trajectories/coverage_map.yaml b/trajectories/coverage_map.yaml new file mode 100644 index 00000000..8a94c98b --- /dev/null +++ b/trajectories/coverage_map.yaml @@ -0,0 +1,228 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 10 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/trajectories/double_points.yaml b/trajectories/double_points.yaml new file mode 100644 index 00000000..706d0538 --- /dev/null +++ b/trajectories/double_points.yaml @@ -0,0 +1,245 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 6 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 9 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 10 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 13 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/trajectories/empty.yaml b/trajectories/empty.yaml new file mode 100644 index 00000000..d018af4a --- /dev/null +++ b/trajectories/empty.yaml @@ -0,0 +1,7 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: [] \ No newline at end of file diff --git a/trajectories/empty_frame_ids.yaml b/trajectories/empty_frame_ids.yaml new file mode 100644 index 00000000..89cecb8e --- /dev/null +++ b/trajectories/empty_frame_ids.yaml @@ -0,0 +1,92 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' + pose: + position: + x: 0.0 + y: 5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 diff --git a/trajectories/test_path.yaml b/trajectories/test_path.yaml new file mode 100644 index 00000000..5a1f7496 --- /dev/null +++ b/trajectories/test_path.yaml @@ -0,0 +1,92 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 2 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 4 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file diff --git a/trajectories/zigzag.yaml b/trajectories/zigzag.yaml new file mode 100644 index 00000000..c57533be --- /dev/null +++ b/trajectories/zigzag.yaml @@ -0,0 +1,143 @@ +header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' +poses: + - + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 1 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 3 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -1.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 5 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -2.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 7 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -3.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 8 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 5.0 + y: -4.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 11 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: -5.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - + header: + seq: 12 + stamp: + secs: 0 + nsecs: 0 + frame_id: '/map' + pose: + position: + x: 0.0 + y: 0.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file From f0ea20fe2787b88ff05d963d37e14ffbc652755a Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Tue, 3 Aug 2021 13:47:40 +0200 Subject: [PATCH 002/156] Robustify against negative dt values --- .../path_tracking_pid/path_tracking_pid_local_planner.hpp | 1 + src/path_tracking_pid_local_planner.cpp | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 084bbf1e..797f7b60 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -29,6 +29,7 @@ #include #define MAP_PARALLEL_THRESH 0.2 +constexpr double DT_MAX=1.5; BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x, y) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 25d347e2..f5e1af99 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -192,6 +192,11 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_ cmd_vel.angular.z = pid_controller_.getControllerState().current_yaw_vel; return true; // False is no use: https://github.com/magazino/move_base_flex/issues/195 } + else if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) + { + ROS_ERROR("Invalid time increment: %f. Aborting", dt.toSec()); + return false; + } try { ROS_DEBUG("map_frame: %s, base_link_frame: %s", map_frame_.c_str(), base_link_frame_.c_str()); From 727ad5fae4359199eed626e121dbe60076c15f70 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 16 Aug 2021 15:36:17 +0200 Subject: [PATCH 003/156] Make init-vel threshold tunable so users can pick a value for their application --- README.md | 2 ++ cfg/Pid.cfg | 1 + src/path_tracking_pid_local_planner.cpp | 4 +++- 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 787774ca..6bf07a7a 100644 --- a/README.md +++ b/README.md @@ -101,6 +101,8 @@ Tracking_pid parameters are all available through (rqt_)dynamic_reconfigure. The * **`l`** (double, default: `0.5`) Following distance from robot's rotational point to trajectory. * **`track_base_link`** (bool, default: `false`) Whether to track the path using the base_link instead of the control point ahead. A smooth path is needed. +* **`init_vel_method`** (enum, default: `1`) Method to choose initial velocity on new paths. +* **`init_vel_max_diff`** (double, default: `0.5`) How much velocity difference is acceptable upon starting a new path. If internal state and new path's velocity differ more than this, the path will be rejected. Set to -1 to ignore this check. Proportional, Integral and Derivative actions for the two closed loops: Lateral and angular loops. diff --git a/cfg/Pid.cfg b/cfg/Pid.cfg index 5b0a4e0a..67e188af 100755 --- a/cfg/Pid.cfg +++ b/cfg/Pid.cfg @@ -23,6 +23,7 @@ gen.add("init_vel_method", int_t, 0, "Initial velocity method", 1, 0, 3, edit_me gen.const("InternalState", int_t, 1, "Last internal state is new initial state"), gen.const("Odom", int_t, 2, "Start from current odometry value") ], "Initial velocity method enum")) +gen.add("init_vel_max_diff", double_t, 0, "Maximum vel-diff allowed when starting a path (-1 to ignore)", 0.5, -1, 10) gen.add("Kp_lat", double_t, 0, "Kp Lateral", 1, 0, 10) gen.add("Ki_lat", double_t, 0, "Ki Lateral", 0, 0, 2) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index f5e1af99..00ee308e 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -137,7 +137,9 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector 0.5) + pid_controller_.getConfig().init_vel_max_diff >= 0.0 && + std::abs(latest_odom_.twist.twist.linear.x - pid_controller_.getControllerState().current_x_vel) > + pid_controller_.getConfig().init_vel_max_diff) { ROS_ERROR("Significant diff between odom (%f) and controller_state (%f) detected. Aborting!", latest_odom_.twist.twist.linear.x, pid_controller_.getControllerState().current_x_vel); From 86e2dcfd396b6de0abfeaa0aa26eaed0af9e800c Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Fri, 20 Aug 2021 14:50:12 +0200 Subject: [PATCH 004/156] 2.16.0 ros-path_tracking_pid 2.16.0 81b78b9 ros-roslint 0.12.0 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index c1f33750..3a853727 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ path_tracking_pid - 2.15.0 + 2.16.0 Follows a trajectory with open-loop speed and closed loop (pid) lateral control Cesar Lopez From 503603d2560b56b4f82a6f3d4191e4454043ecef Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Mon, 23 Aug 2021 11:10:49 +0200 Subject: [PATCH 005/156] Enable industrial CI --- .github/workflows/industrial_ci_action.yml | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 .github/workflows/industrial_ci_action.yml diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml new file mode 100644 index 00000000..81d8b365 --- /dev/null +++ b/.github/workflows/industrial_ci_action.yml @@ -0,0 +1,16 @@ +name: CI + +on: [push, pull_request] + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: melodic, ROS_REPO: main} + - {ROS_DISTRO: noetic, ROS_REPO: main} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v1 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} From 5bb319c888efe01953d8529b5c1391c12fa984b5 Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Mon, 23 Aug 2021 11:26:05 +0200 Subject: [PATCH 006/156] See if noetic fails as well --- .github/workflows/industrial_ci_action.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 81d8b365..8bdb8931 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -7,8 +7,9 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: melodic, ROS_REPO: main} - - {ROS_DISTRO: noetic, ROS_REPO: main} + - {ROS_DISTRO: melodic} + - {ROS_DISTRO: noetic, CMAKE_ARGS: '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON'} + fail-fast: false runs-on: ubuntu-latest steps: - uses: actions/checkout@v1 From 388bc156ffaafe67a6a7252d975a5b60c6edb4cd Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Mon, 23 Aug 2021 13:01:37 +0200 Subject: [PATCH 007/156] Relax the criterium a bit --- test/test_path_tracking_pid_turn_skip.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/test/test_path_tracking_pid_turn_skip.py b/test/test_path_tracking_pid_turn_skip.py index 1fbfc925..b9e6560c 100755 --- a/test/test_path_tracking_pid_turn_skip.py +++ b/test/test_path_tracking_pid_turn_skip.py @@ -74,12 +74,12 @@ def test_exepath_action(self): self.assertTrue(finished_in_time, msg="Action call didn't return in time") self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") - self.assertTrue(endpose_error < 0.5, msg="Did not arrive on final path's pose! \ - pose: {}, {} endpoint: {}, {}".format( - self.cur_odom.pose.pose.position.x, - self.cur_odom.pose.pose.position.y, - path.poses[-1].pose.position.x, - path.poses[-1].pose.position.y)) + self.assertTrue(endpose_error < 1.0, msg= + "Did not arrive on final path's pose! pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) if __name__ == "__main__": From aed0080249b60ffbc6547cd0c46e4d9815cf7490 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Wed, 1 Dec 2021 18:19:43 +0100 Subject: [PATCH 008/156] Fix backwards cancel bug and add test for it --- src/path_tracking_pid_local_planner.cpp | 6 +- test/test_path_tracking_pid.test | 1 + test/test_path_tracking_pid_bw_turn_cancel.py | 105 ++++++++++++++++++ 3 files changed, 110 insertions(+), 2 deletions(-) create mode 100755 test/test_path_tracking_pid_bw_turn_cancel.py diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 00ee308e..f01d1806 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -252,8 +252,10 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_ if (cancel_requested_) { path_tracking_pid::PidConfig config = pid_controller_.getConfig(); - config.target_x_vel = 0.0; - config.target_end_x_vel = 0.0; + // Copysign here, such that when cancelling while driving backwards, we decelerate to -0.0 and hence + // the sign propagates correctly + config.target_x_vel = std::copysign(0.0, config.target_x_vel); + config.target_end_x_vel = std::copysign(0.0, config.target_x_vel); boost::recursive_mutex::scoped_lock lock(config_mutex_); // When updating from own server no callback is called. Thus controller is updated first and then server is notified pid_controller_.configure(config); diff --git a/test/test_path_tracking_pid.test b/test/test_path_tracking_pid.test index bfbc5410..b2a0e370 100644 --- a/test/test_path_tracking_pid.test +++ b/test/test_path_tracking_pid.test @@ -63,5 +63,6 @@ + diff --git a/test/test_path_tracking_pid_bw_turn_cancel.py b/test/test_path_tracking_pid_bw_turn_cancel.py new file mode 100755 index 00000000..1cdd5136 --- /dev/null +++ b/test/test_path_tracking_pid_bw_turn_cancel.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python +import unittest +from math import cos, hypot, pi, sin + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from geometry_msgs.msg import Point, Pose +from math import atan2 +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry +from numpy import linspace +from visualization_msgs.msg import Marker + +from paths import create_path + + +def points_on_circle(angles, radius=1, center=(0, 0)): + x = [] + y = [] + yaw = [] + for angle in angles: + x.append(center[0] + (cos(angle) * radius)) + y.append(center[1] + (sin(angle) * radius)) + yaw.append(angle + pi*0.5) + return (x, y, yaw) + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + + def vis_cb(self, msg): + if msg.ns == 'control point': + self.carrot = msg.points[0] + elif msg.ns == 'plan point': + self.pos_on_plan = msg.points[0] + + # Only start checking when both markers are received + if self.carrot is None or self.pos_on_plan is None: + return + + angle = atan2(self.carrot.y - self.pos_on_plan.y, + self.carrot.x - self.pos_on_plan.x) + # Check if marker direction doesn't flip, if it did, hold this boolean + if not self.carrot_dir_flipped and self.marker_angle is not None: + angle_diff = angle - self.marker_angle + # 'Wrap' + if angle_diff > pi: + angle_diff -= 2*pi + elif angle_diff < -pi: + angle_diff += 2*pi + # Check if angle flipped + self.carrot_dir_flipped = abs(angle_diff) > 0.8*pi + self.marker_angle = angle + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + self.carrot = None + self.pos_on_plan = None + self.marker_angle = None + self.carrot_dir_flipped = False + # Subscribe to visualization-markers to track control direction + self.vis_sub = rospy.Subscriber("move_base_flex/PathTrackingPID/visualization_marker", Marker, self.vis_cb) + + (x_circ, y_circ, yaw_circ) = points_on_circle(linspace(-0.5*pi, -0.75*pi), radius=3.0, center=(0.0, 3.0)) + path = create_path(x_circ, y_circ, yaw_circ) + + speed = -0.6 + endspeed = 0 + + # Start goal and cancel in between + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": speed}) + reconfigure.update_configuration({"target_end_x_vel": endspeed}) + reconfigure.update_configuration({"target_x_acc": 0.5}) + reconfigure.update_configuration({"target_x_decc": 0.5}) + # We require debug enabled here to retrieve the markers! + reconfigure.update_configuration({"controller_debug_enabled": True}) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + rospy.sleep(2.0) + rospy.logwarn("Preempting path!") + client.cancel_goal() + rospy.logwarn("Wait for result") + preempt_in_time = client.wait_for_result(timeout=rospy.Duration(10)) + rospy.logwarn("Got result") + self.assertTrue(preempt_in_time, msg="Action call didn't preempt in time") + + self.assertFalse(self.carrot_dir_flipped, msg="Guiding direction flipped while stopping!") + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_bw_turn_cancel", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_bw_turn_cancel", TestPathTrackingPID) From ada695d21efdc309c923929ad9d9ae4b0cff463d Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Wed, 1 Dec 2021 19:17:49 +0100 Subject: [PATCH 009/156] Fix test!? --- test/test_path_tracking_pid_accel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_path_tracking_pid_accel.py b/test/test_path_tracking_pid_accel.py index 8f3e4067..cb454f72 100755 --- a/test/test_path_tracking_pid_accel.py +++ b/test/test_path_tracking_pid_accel.py @@ -56,7 +56,7 @@ def test_exepath_action(self): self.assertTrue(abs(self.cur_odom.twist.twist.linear.x) < 0.1, msg="Violated deceleration on preempt") preempt_in_time = client.wait_for_result(timeout=rospy.Duration(10)) self.assertTrue(preempt_in_time, msg="Action call didn't preempt in time") - self.assertEqual(client.get_state(), GS.ABORTED, msg="Action didn't preempt on request") + self.assertEqual(client.get_state(), GS.PREEMPTED, msg="Action didn't preempt on request: {}".format(client.get_state())) # Resume action self.reconfigure() From 18146c1bc60ee235bd7ba703a394bf5af032bd41 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Thu, 20 Jan 2022 16:50:38 +0100 Subject: [PATCH 010/156] Fix acceleration/deceleration switching in reverse driving --- src/controller.cpp | 39 ++++++++++++++++++++++++++++++++------- 1 file changed, 32 insertions(+), 7 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 21417e77..e40c92d3 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -586,7 +586,8 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, // to de-accelerate and thus avoid abrupt velocity changes at the end of the trajectory // The sample time plays an important role on how good these estimates are. // Thus We add a distance to the end phase distance estimation depending on the sample time - if (current_x_vel > target_end_x_vel) + if ((sign(current_target_x_vel_) > 0 && current_x_vel > target_end_x_vel) || + (sign(current_target_x_vel_) < 0 && current_x_vel < target_end_x_vel)) { t_end_phase_current = (target_end_x_vel - current_x_vel) / (-target_x_decc_); d_end_phase = current_x_vel * t_end_phase_current @@ -630,18 +631,42 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, current_target_x_vel_ = target_x_vel; } - if (current_x_vel < current_target_x_vel_) + // Determine if we need to accelerate, decelerate or maintain speed + current_target_acc = 0; // Assume maintaining speed + if (fabs(current_target_x_vel_) <= VELOCITY_EPS) // Zero velocity requested { - current_target_acc = target_x_acc_; + if (current_x_vel > current_target_x_vel_) + { + current_target_acc = -target_x_decc_; + } + else + { + current_target_acc = target_x_decc_; + } } - else if (current_x_vel > current_target_x_vel_) + else if (sign(current_target_x_vel_) > 0) // Positive velocity requested { - current_target_acc = -target_x_decc_; + if (current_x_vel > current_target_x_vel_) + { + current_target_acc = -target_x_decc_; + } + else + { + current_target_acc = target_x_acc_; + } } - else + else // Negative velocity requested { - current_target_acc = 0; + if (current_x_vel > current_target_x_vel_) + { + current_target_acc = -target_x_acc_; + } + else + { + current_target_acc = target_x_decc_; + } } + double acc_desired = (current_target_x_vel_ - current_x_vel) / dt.toSec(); double acc_abs = fmin(fabs(acc_desired), fabs(current_target_acc)); acc = copysign(acc_abs, current_target_acc); From fa60109b0c0f466bc2629e0cef472fd4c869ecc0 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Thu, 20 Jan 2022 17:07:10 +0100 Subject: [PATCH 011/156] Add ugly test for checking forward and reverse acceleration/deceleration values --- test/test_path_tracking_pid.test | 1 + test/test_path_tracking_pid_fw_bw.py | 104 +++++++++++++++++++++++++++ 2 files changed, 105 insertions(+) create mode 100755 test/test_path_tracking_pid_fw_bw.py diff --git a/test/test_path_tracking_pid.test b/test/test_path_tracking_pid.test index b2a0e370..f9e9336b 100644 --- a/test/test_path_tracking_pid.test +++ b/test/test_path_tracking_pid.test @@ -64,5 +64,6 @@ + diff --git a/test/test_path_tracking_pid_fw_bw.py b/test/test_path_tracking_pid_fw_bw.py new file mode 100755 index 00000000..07dc8097 --- /dev/null +++ b/test/test_path_tracking_pid_fw_bw.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python +import unittest +from math import hypot + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry + +from paths import create_path + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + + def reconfigure(self, target_vel): + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": target_vel}) + reconfigure.update_configuration({"target_end_x_vel": 0}) + reconfigure.update_configuration({"target_x_acc": 3.0}) + reconfigure.update_configuration({"target_x_decc": 1.0}) + + def odom_cb(self, msg): + self.cur_odom = msg + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + # Subscribe to pose messages from + self.pose_sub = rospy.Subscriber("/odom", Odometry, self.odom_cb) + + path = create_path([0.0, 10.0], [0.0, 0.0], [0.0, 0.0]) + + # Start goal and evaluate outcome + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + self.reconfigure(2.0) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + rospy.sleep(0.666) + self.assertTrue(1.0 < self.cur_odom.twist.twist.linear.x < 2.1, msg="Violated acceleration") + rospy.sleep(4.0) + self.assertTrue(self.cur_odom.twist.twist.linear.x < 2.0, msg="Deceleration not started on time") + rospy.sleep(1.5) + self.assertTrue(0.0 < self.cur_odom.twist.twist.linear.x < 0.5, msg="Violated deceleration {}".format(self.cur_odom.twist.twist.linear.x)) + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + + # Get end-pose error + endpose_error = hypot(path.poses[-1].pose.position.x - self.cur_odom.pose.pose.position.x, + path.poses[-1].pose.position.y - self.cur_odom.pose.pose.position.y) + + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + self.assertTrue(endpose_error < 0.5, msg="Did not arrive on final path's pose! \ + pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) + + # Start bw-goal and evaluate outcome + path = create_path([10.0, 0.0], [0.0, 0.0], [0.0, 0.0]) + outcome_exp = rospy.get_param("~outcome", GS.SUCCEEDED) + self.reconfigure(-2.0) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + rospy.sleep(0.666) + self.assertTrue(-1.0 > self.cur_odom.twist.twist.linear.x > -2.1, msg="Violated acceleration") + rospy.sleep(4.0) + self.assertTrue(self.cur_odom.twist.twist.linear.x > -2.0, msg="Deceleration not started on time") + rospy.sleep(1.5) + self.assertTrue(0.0 > self.cur_odom.twist.twist.linear.x > -0.5, msg="Violated deceleration") + + finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + + # Get end-pose error + endpose_error = hypot(path.poses[-1].pose.position.x - self.cur_odom.pose.pose.position.x, + path.poses[-1].pose.position.y - self.cur_odom.pose.pose.position.y) + + self.assertTrue(finished_in_time, msg="Action call didn't return in time") + self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") + self.assertTrue(endpose_error < 0.5, msg="Did not arrive on final path's pose! \ + pose: {}, {} endpoint: {}, {}".format( + self.cur_odom.pose.pose.position.x, + self.cur_odom.pose.pose.position.y, + path.poses[-1].pose.position.x, + path.poses[-1].pose.position.y)) + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_fw_bw", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_fw_bw", TestPathTrackingPID) From 8876ba8b0132504612f05b84d6f66a8dd63d5165 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Thu, 20 Jan 2022 22:07:28 +0100 Subject: [PATCH 012/156] Slight test improvements --- test/test_path_tracking_pid_fw_bw.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/test/test_path_tracking_pid_fw_bw.py b/test/test_path_tracking_pid_fw_bw.py index 07dc8097..5381db1c 100755 --- a/test/test_path_tracking_pid_fw_bw.py +++ b/test/test_path_tracking_pid_fw_bw.py @@ -21,7 +21,7 @@ def reconfigure(self, target_vel): reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) reconfigure.update_configuration({"target_x_vel": target_vel}) reconfigure.update_configuration({"target_end_x_vel": 0}) - reconfigure.update_configuration({"target_x_acc": 3.0}) + reconfigure.update_configuration({"target_x_acc": 4.0}) reconfigure.update_configuration({"target_x_decc": 1.0}) def odom_cb(self, msg): @@ -47,12 +47,12 @@ def test_exepath_action(self): rospy.logwarn("Starting path!") client.send_goal(ExePathGoal(path=path)) - rospy.sleep(0.666) + rospy.sleep(0.6) self.assertTrue(1.0 < self.cur_odom.twist.twist.linear.x < 2.1, msg="Violated acceleration") rospy.sleep(4.0) self.assertTrue(self.cur_odom.twist.twist.linear.x < 2.0, msg="Deceleration not started on time") - rospy.sleep(1.5) - self.assertTrue(0.0 < self.cur_odom.twist.twist.linear.x < 0.5, msg="Violated deceleration {}".format(self.cur_odom.twist.twist.linear.x)) + rospy.sleep(1.4) + self.assertTrue(0.0 < self.cur_odom.twist.twist.linear.x < 0.5, msg="Violated deceleration") finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) @@ -76,11 +76,11 @@ def test_exepath_action(self): rospy.logwarn("Starting path!") client.send_goal(ExePathGoal(path=path)) - rospy.sleep(0.666) + rospy.sleep(0.6) self.assertTrue(-1.0 > self.cur_odom.twist.twist.linear.x > -2.1, msg="Violated acceleration") rospy.sleep(4.0) self.assertTrue(self.cur_odom.twist.twist.linear.x > -2.0, msg="Deceleration not started on time") - rospy.sleep(1.5) + rospy.sleep(1.4) self.assertTrue(0.0 > self.cur_odom.twist.twist.linear.x > -0.5, msg="Violated deceleration") finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) From c26e5cf948e80a639bb093cb92053500b536ca4a Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Tue, 25 Jan 2022 14:09:56 +0100 Subject: [PATCH 013/156] Change accel/decel check in test --- test/test_path_tracking_pid_fw_bw.py | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/test/test_path_tracking_pid_fw_bw.py b/test/test_path_tracking_pid_fw_bw.py index 5381db1c..c62935ac 100755 --- a/test/test_path_tracking_pid_fw_bw.py +++ b/test/test_path_tracking_pid_fw_bw.py @@ -16,6 +16,8 @@ class TestPathTrackingPID(unittest.TestCase): def setUp(self): self.cur_odom = Odometry() + self.prev_odom = Odometry() + self.cur_accel = 0.0 def reconfigure(self, target_vel): reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) @@ -25,7 +27,11 @@ def reconfigure(self, target_vel): reconfigure.update_configuration({"target_x_decc": 1.0}) def odom_cb(self, msg): + self.prev_odom = self.cur_odom self.cur_odom = msg + dv = self.cur_odom.twist.twist.linear.x - self.prev_odom.twist.twist.linear.x + dt = (self.cur_odom.header.stamp - self.prev_odom.header.stamp).to_sec() + self.cur_accel = dv / dt def test_exepath_action(self): @@ -47,12 +53,10 @@ def test_exepath_action(self): rospy.logwarn("Starting path!") client.send_goal(ExePathGoal(path=path)) - rospy.sleep(0.6) - self.assertTrue(1.0 < self.cur_odom.twist.twist.linear.x < 2.1, msg="Violated acceleration") - rospy.sleep(4.0) - self.assertTrue(self.cur_odom.twist.twist.linear.x < 2.0, msg="Deceleration not started on time") - rospy.sleep(1.4) - self.assertTrue(0.0 < self.cur_odom.twist.twist.linear.x < 0.5, msg="Violated deceleration") + rospy.sleep(0.5) + self.assertTrue(3.5 < self.cur_accel < 4.5, msg="Violated acceleration {}".format(self.cur_accel)) + rospy.sleep(4.5) + self.assertTrue(-1.5 < self.cur_accel < -0.5, msg="Violated deceleration {}".format(self.cur_accel)) finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) @@ -76,12 +80,10 @@ def test_exepath_action(self): rospy.logwarn("Starting path!") client.send_goal(ExePathGoal(path=path)) - rospy.sleep(0.6) - self.assertTrue(-1.0 > self.cur_odom.twist.twist.linear.x > -2.1, msg="Violated acceleration") - rospy.sleep(4.0) - self.assertTrue(self.cur_odom.twist.twist.linear.x > -2.0, msg="Deceleration not started on time") - rospy.sleep(1.4) - self.assertTrue(0.0 > self.cur_odom.twist.twist.linear.x > -0.5, msg="Violated deceleration") + rospy.sleep(0.5) + self.assertTrue(-4.5 < self.cur_accel < -3.5, msg="Violated acceleration {}".format(self.cur_accel)) + rospy.sleep(4.5) + self.assertTrue(0.5 < self.cur_accel < 1.5, msg="Violated deceleration {}".format(self.cur_accel)) finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) From 39c0a10e21058911f253583026d44c3bba2b5929 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Tue, 25 Jan 2022 20:54:28 +0100 Subject: [PATCH 014/156] Try fix test? --- test/test_path_tracking_pid.test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_path_tracking_pid.test b/test/test_path_tracking_pid.test index f9e9336b..84d67483 100644 --- a/test/test_path_tracking_pid.test +++ b/test/test_path_tracking_pid.test @@ -10,7 +10,7 @@ - + From bdf00570920f0e8260da22570c7cf7e6ed3fae55 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Wed, 26 Jan 2022 11:44:19 +0100 Subject: [PATCH 015/156] This seems to fix the tests? --- test/test_path_tracking_pid_accel.py | 1 + test/test_path_tracking_pid_bw_turn_cancel.py | 1 + test/test_path_tracking_pid_fw_bw.py | 1 + test/test_path_tracking_pid_preempt.py | 1 + test/test_path_tracking_pid_turn_skip.py | 1 + 5 files changed, 5 insertions(+) diff --git a/test/test_path_tracking_pid_accel.py b/test/test_path_tracking_pid_accel.py index cb454f72..e58ef1c6 100755 --- a/test/test_path_tracking_pid_accel.py +++ b/test/test_path_tracking_pid_accel.py @@ -23,6 +23,7 @@ def reconfigure(self): reconfigure.update_configuration({"target_end_x_vel": 0}) reconfigure.update_configuration({"target_x_acc": 1.0}) reconfigure.update_configuration({"target_x_decc": 1.0}) + reconfigure.update_configuration({"use_mpc": False}) def odom_cb(self, msg): self.cur_odom = msg diff --git a/test/test_path_tracking_pid_bw_turn_cancel.py b/test/test_path_tracking_pid_bw_turn_cancel.py index 1cdd5136..25dd8890 100755 --- a/test/test_path_tracking_pid_bw_turn_cancel.py +++ b/test/test_path_tracking_pid_bw_turn_cancel.py @@ -84,6 +84,7 @@ def test_exepath_action(self): reconfigure.update_configuration({"target_end_x_vel": endspeed}) reconfigure.update_configuration({"target_x_acc": 0.5}) reconfigure.update_configuration({"target_x_decc": 0.5}) + reconfigure.update_configuration({"use_mpc": False}) # We require debug enabled here to retrieve the markers! reconfigure.update_configuration({"controller_debug_enabled": True}) rospy.logwarn("Starting path!") diff --git a/test/test_path_tracking_pid_fw_bw.py b/test/test_path_tracking_pid_fw_bw.py index c62935ac..ada885b3 100755 --- a/test/test_path_tracking_pid_fw_bw.py +++ b/test/test_path_tracking_pid_fw_bw.py @@ -25,6 +25,7 @@ def reconfigure(self, target_vel): reconfigure.update_configuration({"target_end_x_vel": 0}) reconfigure.update_configuration({"target_x_acc": 4.0}) reconfigure.update_configuration({"target_x_decc": 1.0}) + reconfigure.update_configuration({"use_mpc": False}) def odom_cb(self, msg): self.prev_odom = self.cur_odom diff --git a/test/test_path_tracking_pid_preempt.py b/test/test_path_tracking_pid_preempt.py index dfa35294..7a46b7ce 100755 --- a/test/test_path_tracking_pid_preempt.py +++ b/test/test_path_tracking_pid_preempt.py @@ -23,6 +23,7 @@ def reconfigure(self, end_vel=0.0): reconfigure.update_configuration({"target_end_x_vel": end_vel}) reconfigure.update_configuration({"target_x_acc": 2.0}) reconfigure.update_configuration({"target_x_decc": 1.0}) + reconfigure.update_configuration({"use_mpc": False}) def odom_cb(self, msg): self.cur_odom = msg diff --git a/test/test_path_tracking_pid_turn_skip.py b/test/test_path_tracking_pid_turn_skip.py index b9e6560c..2355f5c9 100755 --- a/test/test_path_tracking_pid_turn_skip.py +++ b/test/test_path_tracking_pid_turn_skip.py @@ -63,6 +63,7 @@ def test_exepath_action(self): reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) reconfigure.update_configuration({"target_x_vel": speed}) reconfigure.update_configuration({"target_end_x_vel": endspeed}) + reconfigure.update_configuration({"use_mpc": False}) rospy.logwarn("Starting path!") client.send_goal(ExePathGoal(path=path)) From 6bb3f8b3cee7866cb3b861fdf4440df2d0d1f2b4 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Wed, 26 Jan 2022 11:54:59 +0100 Subject: [PATCH 016/156] using sign() function here is not necessary... --- src/controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index e40c92d3..e82c1f6a 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -586,8 +586,8 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, // to de-accelerate and thus avoid abrupt velocity changes at the end of the trajectory // The sample time plays an important role on how good these estimates are. // Thus We add a distance to the end phase distance estimation depending on the sample time - if ((sign(current_target_x_vel_) > 0 && current_x_vel > target_end_x_vel) || - (sign(current_target_x_vel_) < 0 && current_x_vel < target_end_x_vel)) + if ((current_target_x_vel_ > 0.0 && current_x_vel > target_end_x_vel) || + (current_target_x_vel_ < 0.0 && current_x_vel < target_end_x_vel)) { t_end_phase_current = (target_end_x_vel - current_x_vel) / (-target_x_decc_); d_end_phase = current_x_vel * t_end_phase_current From e6f4fcb3576dca247be44a43e6f9b4cb5aec1cb1 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Thu, 27 Jan 2022 13:03:41 +0100 Subject: [PATCH 017/156] sign() function not needed here --- src/controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller.cpp b/src/controller.cpp index e82c1f6a..58448028 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -644,7 +644,7 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, current_target_acc = target_x_decc_; } } - else if (sign(current_target_x_vel_) > 0) // Positive velocity requested + else if (current_target_x_vel_ > 0) // Positive velocity requested { if (current_x_vel > current_target_x_vel_) { From 1812b07defa80e76ebaf5eaab80dadefc9a2ea64 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Fri, 28 Jan 2022 12:11:56 +0100 Subject: [PATCH 018/156] Cleanup unused dependencies --- CMakeLists.txt | 3 --- package.xml | 3 +-- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d6258a64..d3eddf51 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,10 +17,8 @@ find_package(catkin REQUIRED pluginlib roscpp roslint - rospy rostest std_msgs - std_srvs tf2 tf2_geometry_msgs tf2_ros @@ -58,7 +56,6 @@ catkin_package( pluginlib roscpp std_msgs - std_srvs tf2_geometry_msgs tf2_ros visualization_msgs diff --git a/package.xml b/package.xml index 3a853727..583c5e18 100644 --- a/package.xml +++ b/package.xml @@ -26,9 +26,7 @@ nav_msgs pluginlib roscpp - rospy std_msgs - std_srvs tf2 tf2_geometry_msgs tf2_ros @@ -36,6 +34,7 @@ mbf_costmap_nav message_runtime mobile_robot_simulator + rospy From 12dc7f0f4783fc96905d010996f36cf6c9051430 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Fri, 28 Jan 2022 12:12:10 +0100 Subject: [PATCH 019/156] Fix compile warnings --- include/path_tracking_pid/controller.hpp | 2 +- src/controller.cpp | 18 +++++++++--------- src/path_tracking_pid_local_planner.cpp | 6 +++--- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 0d9ce4cb..90d26a0d 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -230,7 +230,7 @@ class Controller void setVelMaxObstacle(double value); // Get vel_max_obstacle value - double getVelMaxObstacle(); + double getVelMaxObstacle() const; private: double distSquared(const tf2::Transform& pose_v, const tf2::Transform& pose_w) const; diff --git a/src/controller.cpp b/src/controller.cpp index 21417e77..869cf877 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -483,14 +483,14 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, controller_state_.error_integral_ang += controller_state_.error_ang.at(0) * dt.toSec(); // Apply windup limit to limit the size of the integral term - if (controller_state_.error_integral_lat > fabsf(windup_limit_)) - controller_state_.error_integral_lat = fabsf(windup_limit_); - if (controller_state_.error_integral_lat < -fabsf(windup_limit_)) - controller_state_.error_integral_lat = -fabsf(windup_limit_); - if (controller_state_.error_integral_ang > fabsf(windup_limit_)) - controller_state_.error_integral_ang = fabsf(windup_limit_); - if (controller_state_.error_integral_ang < -fabsf(windup_limit_)) - controller_state_.error_integral_ang = -fabsf(windup_limit_); + if (controller_state_.error_integral_lat > std::abs(windup_limit_)) + controller_state_.error_integral_lat = std::abs(windup_limit_); + if (controller_state_.error_integral_lat < -std::abs(windup_limit_)) + controller_state_.error_integral_lat = -std::abs(windup_limit_); + if (controller_state_.error_integral_ang > std::abs(windup_limit_)) + controller_state_.error_integral_ang = std::abs(windup_limit_); + if (controller_state_.error_integral_ang < -std::abs(windup_limit_)) + controller_state_.error_integral_ang = -std::abs(windup_limit_); // My filter reference was Julius O. Smith III, Intro. to Digital Filters With Audio Applications. if (cutoff_frequency_lat_ != -1) @@ -1222,7 +1222,7 @@ void Controller::setVelMaxObstacle(double value) vel_max_obstacle_ = value; } -double Controller::getVelMaxObstacle() +double Controller::getVelMaxObstacle() const { return vel_max_obstacle_; } diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index f01d1806..9917f2f1 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -129,7 +129,7 @@ bool TrackingPidLocalPlanner::setPlan(const std::vectorlookupTransform(map_frame_, base_link_frame_, ros::Time(0)); } - catch (tf2::TransformException ex) + catch (const tf2::TransformException& ex) { ROS_ERROR("Received an exception trying to transform: %s", ex.what()); return false; @@ -153,7 +153,7 @@ bool TrackingPidLocalPlanner::setPlan(const std::vectorlookupTransform(base_link_frame_, steered_wheel_frame_, ros::Time(0)); } - catch (tf2::TransformException ex) + catch (const tf2::TransformException& ex) { ROS_ERROR("Received an exception trying to transform: %s", ex.what()); ROS_ERROR("Invalid transformation from base_link_frame to steered_wheel_frame. Tricycle model will be disabled"); @@ -204,7 +204,7 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_ ROS_DEBUG("map_frame: %s, base_link_frame: %s", map_frame_.c_str(), base_link_frame_.c_str()); tfCurPoseStamped_ = tf_->lookupTransform(map_frame_, base_link_frame_, ros::Time(0)); } - catch (tf2::TransformException ex) + catch (const tf2::TransformException& ex) { ROS_ERROR("Received an exception trying to transform: %s", ex.what()); active_goal_ = false; From 39b4b7065b9de6ffe05e38150b8ad7175b3a71e2 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Fri, 28 Jan 2022 16:31:18 +0100 Subject: [PATCH 020/156] Cleanup header files --- include/path_tracking_pid/controller.hpp | 19 +++++----- .../path_tracking_pid_local_planner.hpp | 36 ++++++++----------- src/controller.cpp | 5 ++- src/path_tracking_pid_local_planner.cpp | 16 +++++---- 4 files changed, 36 insertions(+), 40 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 90d26a0d..c495048f 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -1,17 +1,14 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include +#include #include -#include -#include -#include + +#include "geometry_msgs/PoseStamped.h" +#include "geometry_msgs/Transform.h" +#include "geometry_msgs/Twist.h" +#include "path_tracking_pid/PidConfig.h" +#include "path_tracking_pid/PidDebug.h" +#include "tf2/LinearMath/Transform.h" // Typesafe sign implementation with signum: // https://stackoverflow.com/a/4609795 diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 797f7b60..25002472 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -1,32 +1,24 @@ #pragma once -#include "./controller.hpp" #include #include #include #include #include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include + +#include "./controller.hpp" +#include "boost/thread/recursive_mutex.hpp" +#include "costmap_2d/costmap_2d_ros.h" +#include "dynamic_reconfigure/server.h" +#include "geometry_msgs/Point.h" +#include "mbf_costmap_core/costmap_controller.h" +#include "nav_core/base_local_planner.h" +#include "nav_msgs/Odometry.h" +#include "nav_msgs/Path.h" +#include "path_tracking_pid/PidConfig.h" +#include "std_msgs/Bool.h" +#include "std_msgs/Float64.h" +#include "tf2_ros/buffer.h" #define MAP_PARALLEL_THRESH 0.2 constexpr double DT_MAX=1.5; diff --git a/src/controller.cpp b/src/controller.cpp index 869cf877..dfd4ed73 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -3,10 +3,13 @@ // #include "path_tracking_pid/controller.hpp" -#include + #include #include +#include "angles/angles.h" +#include "tf2/utils.h" + namespace path_tracking_pid { diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 9917f2f1..f082b948 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -3,16 +3,20 @@ // #include "path_tracking_pid/path_tracking_pid_local_planner.hpp" -#include -#include + #include #include -#include #include -#include -#include #include +#include "mbf_msgs/ExePathResult.h" +#include "path_tracking_pid/PidDebug.h" +#include "path_tracking_pid/PidFeedback.h" +#include "pluginlib/class_list_macros.h" +#include "tf2/utils.h" +#include "visualization_msgs/Marker.h" +#include "visualization_msgs/MarkerArray.h" + // register planner as move_base and move_base plugins PLUGINLIB_EXPORT_CLASS(path_tracking_pid::TrackingPidLocalPlanner, nav_core::BaseLocalPlanner) PLUGINLIB_EXPORT_CLASS(path_tracking_pid::TrackingPidLocalPlanner, mbf_costmap_core::CostmapController) @@ -424,7 +428,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, x_vel), 0.0, 0.0)); // Use a controller state to forward project the position on the path - ControllerState projected_controller_state = pid_controller_.getControllerState(); + auto projected_controller_state = pid_controller_.getControllerState(); geometry_msgs::Transform current_tf = tfCurPoseStamped_.transform; // Step until lookahead is reached, for every step project the pose back to the path From 47806e253bcce7de04dee1f8bdb1404251b2298a Mon Sep 17 00:00:00 2001 From: Paul Verhoeckx Date: Tue, 1 Feb 2022 09:59:00 +0100 Subject: [PATCH 021/156] fix: Use target_x_vel to determine collision look ahead direction --- src/path_tracking_pid_local_planner.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 9917f2f1..2fcd43aa 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -421,7 +421,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Define a x_step transform which will be used to step forward the position. tf2::Transform x_step_tf; - x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, x_vel), 0.0, 0.0)); + double target_x_vel = pid_controller_.getConfig().target_x_vel; + x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, target_x_vel), 0.0, 0.0)); // Use a controller state to forward project the position on the path ControllerState projected_controller_state = pid_controller_.getControllerState(); From c088ff1ca986d9d7c9ad61b1c0f27f8858e683ea Mon Sep 17 00:00:00 2001 From: Paul Verhoeckx Date: Tue, 1 Feb 2022 11:08:42 +0100 Subject: [PATCH 022/156] fix: Prevent configuring a collision check resolution of zero --- cfg/Pid.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cfg/Pid.cfg b/cfg/Pid.cfg index 67e188af..cc0135e6 100755 --- a/cfg/Pid.cfg +++ b/cfg/Pid.cfg @@ -60,6 +60,6 @@ gen.add("anti_collision", bool_t, 0, "Stop on lethal obstacles", False) collision_group = gen.add_group("collision_group", type="hide") collision_group.add("obstacle_speed_reduction", bool_t, 0, "Slow down on near obstacles", True) collision_group.add("collision_look_ahead_length_offset", double_t, 0, "Offset in length to project rectangle collision along path", 1.0, 0, 5) -collision_group.add("collision_look_ahead_resolution", double_t, 0, "Spatial resolution to project rectangle collision along path", 1.0, 0, 10) +collision_group.add("collision_look_ahead_resolution", double_t, 0, "Spatial resolution to project rectangle collision along path", 1.0, 1e-9, 10) exit(gen.generate(PACKAGE, "path_tracking_pid", "Pid")) From 77eca58662131889f881b1ef9014a4b5fdbee63d Mon Sep 17 00:00:00 2001 From: Paul Verhoeckx Date: Tue, 1 Feb 2022 11:29:20 +0100 Subject: [PATCH 023/156] fix: Prevent dividing by zero when collision_look_ahead_distance is zero --- src/path_tracking_pid_local_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 9917f2f1..467bd958 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -417,7 +417,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() double collision_look_ahead_distance = x_vel*x_vel / (2*pid_controller_.getConfig().target_x_decc) + pid_controller_.getConfig().collision_look_ahead_length_offset; uint n_steps = std::ceil(collision_look_ahead_distance / pid_controller_.getConfig().collision_look_ahead_resolution); - double x_resolution = collision_look_ahead_distance / n_steps; + double x_resolution = collision_look_ahead_distance / std::max(static_cast(n_steps), 1); // Define a x_step transform which will be used to step forward the position. tf2::Transform x_step_tf; From 45d2bb33e60d7fcea8392de652602db8a0b21e1a Mon Sep 17 00:00:00 2001 From: Paul Verhoeckx Date: Thu, 3 Feb 2022 10:46:03 +0100 Subject: [PATCH 024/156] fix: Use absolute largest velocity to determine collision look ahead offset direction --- src/path_tracking_pid_local_planner.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 2fcd43aa..1bf5d19d 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -422,7 +422,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Define a x_step transform which will be used to step forward the position. tf2::Transform x_step_tf; double target_x_vel = pid_controller_.getConfig().target_x_vel; - x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, target_x_vel), 0.0, 0.0)); + double max_abs_x_vel = std::abs(x_vel) > std::abs(target_x_vel) ? x_vel : target_x_vel; + x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, max_abs_x_vel), 0.0, 0.0)); // Use a controller state to forward project the position on the path ControllerState projected_controller_state = pid_controller_.getControllerState(); From e82ce1aa69ee189c4637913702714407d11fd63b Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 2 Feb 2022 13:08:02 +0100 Subject: [PATCH 025/156] fix: check predicted footprints for LETHAL_OBSTACLE instead of INSCRIBED_INFLATED_OBSTACLE --- src/path_tracking_pid_local_planner.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index fd4f0177..52ed3b23 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -220,14 +220,14 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_ { auto cost = projectedCollisionCost(); - if (cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + if (cost >= costmap_2d::LETHAL_OBSTACLE) { pid_controller_.setVelMaxObstacle(0.0); } else if (pid_controller_.getConfig().obstacle_speed_reduction) { double max_vel = pid_controller_.getConfig().max_x_vel; - double reduction_factor = static_cast(cost) / costmap_2d::INSCRIBED_INFLATED_OBSTACLE; + double reduction_factor = static_cast(cost) / costmap_2d::LETHAL_OBSTACLE; double limit = max_vel * (1 - reduction_factor); ROS_DEBUG("Cost: %d, factor: %f, limit: %f", cost, reduction_factor, limit); pid_controller_.setVelMaxObstacle(limit); @@ -533,7 +533,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() mkCollisionIndicator.color.a = cell_cost / 255.0; point.z = mkCollisionIndicator.scale.z * 0.5; mkCollisionIndicator.pose.position = point; - if (max_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + if (max_cost >= costmap_2d::LETHAL_OBSTACLE) { break; // Collision detected, no need to evaluate further } From 05ee28a38603a442fdd83dcefb26c36bc3a29d29 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 2 Feb 2022 17:02:50 +0100 Subject: [PATCH 026/156] fix: make velocity scaledown consider costmap only at predicted base_link --- src/path_tracking_pid_local_planner.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 52ed3b23..a9f452be 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -456,14 +456,26 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() mkPosesOnPath.points.push_back(mkPointOnPath); } + costmap_2d::Costmap2D* costmap2d = costmap_->getCostmap(); polygon_t previous_footprint_xy; polygon_t collision_polygon; + uint8_t max_projected_step_cost = 0; for (const auto& projection_tf : projected_steps_tf) { - // Project footprint forward double x = projection_tf.getOrigin().x(); double y = projection_tf.getOrigin().y(); double yaw = tf2::getYaw(projection_tf.getRotation()); + + // Calculate cost by checking base link location in costmap + int map_x, map_y; + costmap2d->worldToMapEnforceBounds(x, y, map_x, map_y); + uint8_t projected_step_cost = costmap2d->getCost(map_x, map_y); + if (projected_step_cost > max_projected_step_cost) + { + max_projected_step_cost = projected_step_cost; + } + + // Project footprint forward std::vector footprint; costmap_2d::transformFootprint(x, y, yaw, costmap_->getRobotFootprint(), footprint); @@ -492,7 +504,6 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() } // Create a convex hull so we can use costmap2d->convexFillCells - costmap_2d::Costmap2D* costmap2d = costmap_->getCostmap(); polygon_t collision_polygon_hull; boost::geometry::convex_hull(collision_polygon, collision_polygon_hull); std::vector collision_polygon_hull_map; @@ -535,6 +546,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() mkCollisionIndicator.pose.position = point; if (max_cost >= costmap_2d::LETHAL_OBSTACLE) { + max_projected_step_cost = max_cost; break; // Collision detected, no need to evaluate further } } @@ -565,7 +577,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() } collision_marker_pub_.publish(mkCollision); - return max_cost; + return max_projected_step_cost; } uint32_t TrackingPidLocalPlanner::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, From 3cc2b532c32f7c8a8cacdb4f58606aff52e48f1e Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 3 Feb 2022 11:48:41 +0100 Subject: [PATCH 027/156] test: make tracking error assessment configurable --- test/test_path_tracking_pid.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/test/test_path_tracking_pid.py b/test/test_path_tracking_pid.py index 7c5a407d..aa4efb8e 100755 --- a/test/test_path_tracking_pid.py +++ b/test/test_path_tracking_pid.py @@ -62,6 +62,10 @@ def test_exepath_action(self): initialpose_pub.publish(pose) rospy.sleep(0.1) # Fill tf buffers + self.max_tracking_error_linear_x = rospy.get_param("~max_tracking_error_linear_x", 0.1) + self.max_tracking_error_linear_y = rospy.get_param("~max_tracking_error_linear_y", 0.1) + self.max_tracking_error_angular_z = rospy.get_param("~max_tracking_error_angular_z", 0.1) + # Publisher for obstacles: self.obstacle_pub = rospy.Publisher("pointcloud", PointCloud, latch=True, queue_size=1) reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) @@ -109,9 +113,9 @@ def test_exepath_action(self): return # Check the errors - self.assertLess(error_catcher.error.linear.x, 0.1) - self.assertLess(error_catcher.error.linear.y, 0.1) - self.assertLess(error_catcher.error.angular.z, 0.1) + self.assertLess(error_catcher.error.linear.x, self.max_tracking_error_linear_x) + self.assertLess(error_catcher.error.linear.y, self.max_tracking_error_linear_y) + self.assertLess(error_catcher.error.angular.z, self.max_tracking_error_angular_z) # Do the same for backward movements if last path was a success if client.get_state() != GS.SUCCEEDED or rospy.get_param("backward", True): From 12f8c95b990dd59eb1b049991e2a6c347fa30e2d Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 3 Feb 2022 11:49:49 +0100 Subject: [PATCH 028/156] test: disregard final tracking error in lethal obstacle test --- test/test_path_tracking_pid.test | 1 + 1 file changed, 1 insertion(+) diff --git a/test/test_path_tracking_pid.test b/test/test_path_tracking_pid.test index 84d67483..46a14c88 100644 --- a/test/test_path_tracking_pid.test +++ b/test/test_path_tracking_pid.test @@ -49,6 +49,7 @@ [[10.0, 10.0]] 4 + 1.0 [[15.0, -8.0]] From 45f30922c94179989bc381ccbf268493286e2040 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Fri, 28 Jan 2022 17:25:31 +0100 Subject: [PATCH 029/156] Refactor visualization to a separate class --- CMakeLists.txt | 6 +- .../path_tracking_pid_local_planner.hpp | 3 +- include/path_tracking_pid/visualization.hpp | 30 +++++++ src/path_tracking_pid_local_planner.cpp | 80 +++---------------- src/visualization.cpp | 78 ++++++++++++++++++ 5 files changed, 126 insertions(+), 71 deletions(-) create mode 100644 include/path_tracking_pid/visualization.hpp create mode 100644 src/visualization.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d3eddf51..e146a50e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,7 +61,11 @@ catkin_package( visualization_msgs ) -add_library(${PROJECT_NAME} src/controller.cpp src/${PROJECT_NAME}_local_planner.cpp) +add_library(${PROJECT_NAME} + src/${PROJECT_NAME}_local_planner.cpp + src/controller.cpp + src/visualization.cpp +) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 25002472..c3742a33 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -19,6 +19,7 @@ #include "std_msgs/Bool.h" #include "std_msgs/Float64.h" #include "tf2_ros/buffer.h" +#include "path_tracking_pid/visualization.hpp" #define MAP_PARALLEL_THRESH 0.2 constexpr double DT_MAX=1.5; @@ -150,7 +151,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co ros::Publisher debug_pub_; // Debugging of controller internal parameters // Rviz visualization - ros::Publisher marker_pub_; + std::unique_ptr visualization_; ros::Publisher path_pub_; ros::Publisher collision_marker_pub_; diff --git a/include/path_tracking_pid/visualization.hpp b/include/path_tracking_pid/visualization.hpp new file mode 100644 index 00000000..1f9233f3 --- /dev/null +++ b/include/path_tracking_pid/visualization.hpp @@ -0,0 +1,30 @@ +#include + +#include "geometry_msgs/Pose.h" +#include "ros/publisher.h" +#include "std_msgs/ColorRGBA.h" +#include "tf2/LinearMath/Transform.h" + +namespace path_tracking_pid +{ +class Visualization +{ +public: + Visualization(ros::NodeHandle nh); + + void publishControlPoint(const std_msgs::Header & header, const tf2::Transform & pose); + void publishAxlePoint(const std_msgs::Header & header, const tf2::Transform & pose); + void publishGoalPoint(const std_msgs::Header & header, const tf2::Transform & pose); + void publishPlanPoint(const std_msgs::Header & header, const tf2::Transform & pose); + +private: + void publishSphere( + const std_msgs::Header & header, std::string ns, int id, const tf2::Transform & pose, + const std_msgs::ColorRGBA & color); + void publishSphere( + const std_msgs::Header & header, std::string ns, int id, geometry_msgs::Pose pose, + const std_msgs::ColorRGBA & color); + + ros::Publisher marker_pub_; +}; +} // namespace path_tracking_pid diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index fd4f0177..66b87edf 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -67,7 +67,7 @@ void TrackingPidLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, collision_marker_pub_ = nh.advertise("collision_markers", 3); - marker_pub_ = nh.advertise("visualization_marker", 3); + visualization_ = std::make_unique(nh); debug_pub_ = nh.advertise("debug", 1); path_pub_ = nh.advertise("visualization_path", 1, true); @@ -273,74 +273,16 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_ { debug_pub_.publish(pid_debug); - visualization_msgs::Marker mkCurPose, mkControlPose, mkGoalPose, mkPosOnPlan; - - // configure rviz visualization - mkCurPose.header.frame_id = mkControlPose.header.frame_id = map_frame_; - mkGoalPose.header.frame_id = mkPosOnPlan.header.frame_id = map_frame_; - mkCurPose.header.stamp = mkControlPose.header.stamp = ros::Time::now(); - mkGoalPose.header.stamp = mkPosOnPlan.header.stamp = ros::Time::now(); - mkCurPose.ns = "axle point"; - mkControlPose.ns = "control point"; - mkGoalPose.ns = "goal point"; - mkPosOnPlan.ns = "plan point"; - mkCurPose.action = mkControlPose.action = visualization_msgs::Marker::ADD; - mkGoalPose.action = mkPosOnPlan.action = visualization_msgs::Marker::ADD; - mkCurPose.pose.orientation.w = mkControlPose.pose.orientation.w = 1.0; - mkGoalPose.pose.orientation.w = mkPosOnPlan.pose.orientation.w = 1.0; - mkCurPose.id = __COUNTER__; // id has to be unique, so using a compile-time counter :) - mkControlPose.id = __COUNTER__; - mkGoalPose.id = __COUNTER__; - mkPosOnPlan.id = __COUNTER__; - mkCurPose.type = mkControlPose.type = visualization_msgs::Marker::POINTS; - mkGoalPose.type = mkPosOnPlan.type = visualization_msgs::Marker::POINTS; - mkCurPose.scale.x = 0.5; - mkCurPose.scale.y = 0.5; - mkControlPose.scale.x = 0.5; - mkControlPose.scale.y = 0.5; - mkGoalPose.scale.x = 0.5; - mkGoalPose.scale.y = 0.5; - mkCurPose.color.b = 1.0; - mkCurPose.color.a = 1.0; - mkControlPose.color.g = 1.0f; - mkControlPose.color.a = 1.0; - mkGoalPose.color.r = 1.0; - mkGoalPose.color.a = 1.0; - mkPosOnPlan.scale.x = 0.5; - mkPosOnPlan.scale.y = 0.5; - mkPosOnPlan.color.a = 1.0; - mkPosOnPlan.color.r = 1.0f; - mkPosOnPlan.color.g = 0.5f; - - geometry_msgs::Point p; - std_msgs::ColorRGBA color; - p.x = tfCurPoseStamped_.transform.translation.x; - p.y = tfCurPoseStamped_.transform.translation.y; - p.z = tfCurPoseStamped_.transform.translation.z; - mkCurPose.points.push_back(p); - - tf2::Transform tfControlPose = pid_controller_.getCurrentWithCarrot(); - p.x = tfControlPose.getOrigin().x(); - p.y = tfControlPose.getOrigin().y(); - p.z = tfControlPose.getOrigin().z(); - mkControlPose.points.push_back(p); - - tf2::Transform tfGoalPose = pid_controller_.getCurrentGoal(); - p.x = tfGoalPose.getOrigin().x(); - p.y = tfGoalPose.getOrigin().y(); - p.z = tfGoalPose.getOrigin().z(); - mkGoalPose.points.push_back(p); - - tf2::Transform tfCurPose = pid_controller_.getCurrentPosOnPlan(); - p.x = tfCurPose.getOrigin().x(); - p.y = tfCurPose.getOrigin().y(); - p.z = tfCurPose.getOrigin().z(); - mkPosOnPlan.points.push_back(p); - - marker_pub_.publish(mkCurPose); - marker_pub_.publish(mkControlPose); - marker_pub_.publish(mkGoalPose); - marker_pub_.publish(mkPosOnPlan); + // publish rviz visualization + std_msgs::Header header; + header.stamp = now; + header.frame_id = map_frame_; + tf2::Transform tfCurPose; + tf2::fromMsg(tfCurPoseStamped_.transform, tfCurPose); + visualization_->publishAxlePoint(header, tfCurPose); + visualization_->publishControlPoint(header, pid_controller_.getCurrentWithCarrot()); + visualization_->publishGoalPoint(header, pid_controller_.getCurrentGoal()); + visualization_->publishPlanPoint(header, pid_controller_.getCurrentPosOnPlan()); } prev_time_ = now; diff --git a/src/visualization.cpp b/src/visualization.cpp new file mode 100644 index 00000000..a380725e --- /dev/null +++ b/src/visualization.cpp @@ -0,0 +1,78 @@ +#include "path_tracking_pid/visualization.hpp" + +#include + +#include "ros/node_handle.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "visualization_msgs/Marker.h" + +namespace path_tracking_pid +{ +Visualization::Visualization(ros::NodeHandle nh) +{ + marker_pub_ = nh.advertise("visualization_marker", 4); +} + +void Visualization::publishControlPoint( + const std_msgs::Header & header, const tf2::Transform & pose) +{ + std_msgs::ColorRGBA color; + color.a = 1.0; + color.g = 1.0; + // id has to be unique, so using a compile-time counter :) + publishSphere(header, "control point", __COUNTER__, pose, color); +} + +void Visualization::publishAxlePoint(const std_msgs::Header & header, const tf2::Transform & pose) +{ + std_msgs::ColorRGBA color; + color.a = 1.0; + color.b = 1.0; + publishSphere(header, "axle point", __COUNTER__, pose, color); +} + +void Visualization::publishGoalPoint(const std_msgs::Header & header, const tf2::Transform & pose) +{ + std_msgs::ColorRGBA color; + color.a = 1.0; + color.r = 1.0; + publishSphere(header, "goal point", __COUNTER__, pose, color); +} + +void Visualization::publishPlanPoint(const std_msgs::Header & header, const tf2::Transform & pose) +{ + std_msgs::ColorRGBA color; + color.a = 1.0; + color.g = 0.5; + color.r = 1.0; + publishSphere(header, "plan point", __COUNTER__, pose, color); +} + +void Visualization::publishSphere( + const std_msgs::Header & header, std::string ns, int id, const tf2::Transform & pose, + const std_msgs::ColorRGBA & color) +{ + geometry_msgs::Pose msg; + tf2::toMsg(pose, msg); + publishSphere(header, ns, id, msg, color); +} + +void Visualization::publishSphere( + const std_msgs::Header & header, std::string ns, int id, geometry_msgs::Pose pose, + const std_msgs::ColorRGBA & color) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = ns; + marker.id = id; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose = pose; + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + marker.color = color; + marker_pub_.publish(marker); +} + +} // namespace path_tracking_pid From 53c1f5e11030565fe18d6e5d171aaed8ca903181 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 13:59:27 +0000 Subject: [PATCH 030/156] Fixed signed/unsigned comparisons. --- src/controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index ebe79346..d9356a72 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -104,7 +104,7 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi // For now do not allow repeated points or in-place rotation // To allow that the way the progress is checked and the interpolation is done needs to be changed // Also check if points suddenly go in the opposite direction, this could lead to deadlocks - for (int pose_idx = 1; pose_idx < global_plan.size() - 1; ++pose_idx) + for (std::vector::size_type pose_idx = 1; pose_idx < global_plan.size() - 1; ++pose_idx) { auto prev_pose = global_plan[pose_idx - 1].pose; auto pose = global_plan[pose_idx].pose; @@ -121,7 +121,7 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi } else { - ROS_WARN("Pose %d of path is not used since it is not in the expected direction of the path!", pose_idx); + ROS_WARN("Pose %zu of path is not used since it is not in the expected direction of the path!", pose_idx); } } // Add last pose as we didn't evaluate that one @@ -304,7 +304,7 @@ tf2::Transform Controller::findPositionOnPlan(const geometry_msgs::Transform cur // Hence, when idx_path==i we are currently tracking the section connection pose i and pose i+1 // First look in current position and in front - for (int idx_path = controller_state_ptr->current_global_plan_index; idx_path < global_plan_tf_.size(); idx_path++) + for (auto idx_path = controller_state_ptr->current_global_plan_index; idx_path < global_plan_tf_.size(); idx_path++) { error = current_tf2.inverseTimes(global_plan_tf_[idx_path]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose From d1d5f47250dab68ce192496e190ba43d59b05ace Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 13:59:52 +0000 Subject: [PATCH 031/156] Stricter compiler settings. --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e146a50e..aa3a08e3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,4 @@ cmake_minimum_required(VERSION 3.0.2) -add_compile_options(-std=c++17) project(path_tracking_pid) ## Find catkin and any catkin packages @@ -67,6 +66,7 @@ add_library(${PROJECT_NAME} src/visualization.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) +target_compile_options(${PROJECT_NAME} PUBLIC -std=c++17 -Wall -Wextra -Wno-unused-parameter -Werror) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) include_directories( From fae62298dfa9f65abfc2d60fa58c88bc577617bb Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Mon, 14 Feb 2022 14:27:33 +0100 Subject: [PATCH 032/156] fixup! Refactor visualization to a separate class --- test/test_path_tracking_pid_bw_turn_cancel.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/test_path_tracking_pid_bw_turn_cancel.py b/test/test_path_tracking_pid_bw_turn_cancel.py index 25dd8890..47b67dfc 100755 --- a/test/test_path_tracking_pid_bw_turn_cancel.py +++ b/test/test_path_tracking_pid_bw_turn_cancel.py @@ -34,9 +34,9 @@ def setUp(self): def vis_cb(self, msg): if msg.ns == 'control point': - self.carrot = msg.points[0] + self.carrot = msg.pose.position elif msg.ns == 'plan point': - self.pos_on_plan = msg.points[0] + self.pos_on_plan = msg.pose.position # Only start checking when both markers are received if self.carrot is None or self.pos_on_plan is None: From f97694284778827aab985c27a44af4e0ffcbfa1e Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Mon, 14 Feb 2022 14:36:49 +0100 Subject: [PATCH 033/156] Let the test fail if exceptions occur in vis_cb --- test/test_path_tracking_pid_bw_turn_cancel.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/test_path_tracking_pid_bw_turn_cancel.py b/test/test_path_tracking_pid_bw_turn_cancel.py index 47b67dfc..5f799f72 100755 --- a/test/test_path_tracking_pid_bw_turn_cancel.py +++ b/test/test_path_tracking_pid_bw_turn_cancel.py @@ -68,7 +68,7 @@ def test_exepath_action(self): self.carrot = None self.pos_on_plan = None self.marker_angle = None - self.carrot_dir_flipped = False + self.carrot_dir_flipped = None # Subscribe to visualization-markers to track control direction self.vis_sub = rospy.Subscriber("move_base_flex/PathTrackingPID/visualization_marker", Marker, self.vis_cb) @@ -98,7 +98,7 @@ def test_exepath_action(self): rospy.logwarn("Got result") self.assertTrue(preempt_in_time, msg="Action call didn't preempt in time") - self.assertFalse(self.carrot_dir_flipped, msg="Guiding direction flipped while stopping!") + self.assertTrue(self.carrot_dir_flipped is False, msg="Guiding direction flipped while stopping!") if __name__ == "__main__": From f603fecff527321a9ee0165845981bd4fc578ad6 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 15 Feb 2022 13:07:02 +0000 Subject: [PATCH 034/156] Reworked to add_compile_options(). --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aa3a08e3..4fe52cd3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) +add_compile_options(-std=c++17 -Wall -Wextra -Wno-unused-parameter -Werror) project(path_tracking_pid) ## Find catkin and any catkin packages @@ -66,7 +67,6 @@ add_library(${PROJECT_NAME} src/visualization.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) -target_compile_options(${PROJECT_NAME} PUBLIC -std=c++17 -Wall -Wextra -Wno-unused-parameter -Werror) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) include_directories( From 36cacd924b954822c106c32281c4be56a437c395 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 14:01:00 +0000 Subject: [PATCH 035/156] Switched to enum classes. --- include/path_tracking_pid/controller.hpp | 2 +- .../path_tracking_pid_local_planner.hpp | 2 +- src/common.hpp | 12 ++++++++++++ src/path_tracking_pid_local_planner.cpp | 4 +++- 4 files changed, 17 insertions(+), 3 deletions(-) create mode 100644 src/common.hpp diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index c495048f..8b8b21d0 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -23,7 +23,7 @@ namespace path_tracking_pid #define VELOCITY_EPS 1e-3 // Neglegible velocity #define LONG_DURATION 31556926 // A year (ros::Duration cannot be inf) -enum ControllerMode +enum class ControllerMode { frontAxleLateral = 0, rearAxleLateral = 1, diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index c3742a33..7a9c37ff 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -103,7 +103,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co /** Enumeration for custom SUCCESS feedback codes. See default ones: * https://github.com/magazino/move_base_flex/blob/master/mbf_msgs/action/ExePath.action */ - enum + enum class ComputeVelocityCommandsResult { SUCCESS = 0, GRACEFULLY_CANCELLING = 1 diff --git a/src/common.hpp b/src/common.hpp new file mode 100644 index 00000000..eec35118 --- /dev/null +++ b/src/common.hpp @@ -0,0 +1,12 @@ +#include + +namespace path_tracking_pid { + +// Converts an enumeration to its underlying type. +template +constexpr std::underlying_type_t to_underlying(enum_type value) noexcept +{ + return static_cast>(value); +} + +} // namespace path_tracking_pid diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 66b87edf..50be4047 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -4,6 +4,8 @@ #include "path_tracking_pid/path_tracking_pid_local_planner.hpp" +#include "common.hpp" + #include #include #include @@ -540,7 +542,7 @@ uint32_t TrackingPidLocalPlanner::computeVelocityCommands(const geometry_msgs::P return mbf_msgs::ExePathResult::CANCELED; } ROS_INFO_THROTTLE(1.0, "Cancel in progress... remaining x_vel: %f", cmd_vel.twist.linear.x); - return TrackingPidLocalPlanner::GRACEFULLY_CANCELLING; + return to_underlying(ComputeVelocityCommandsResult::GRACEFULLY_CANCELLING); } if (!moving && pid_controller_.getVelMaxObstacle() < VELOCITY_EPS) From 1b54e83656ca59f87901b213fa01fc5a576bb909 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 15 Feb 2022 13:24:02 +0000 Subject: [PATCH 036/156] Forgot pragma once. And used ROS clang-format. --- src/common.hpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/common.hpp b/src/common.hpp index eec35118..79b4bd59 100644 --- a/src/common.hpp +++ b/src/common.hpp @@ -1,12 +1,15 @@ +#pragma once + #include -namespace path_tracking_pid { +namespace path_tracking_pid +{ // Converts an enumeration to its underlying type. -template +template constexpr std::underlying_type_t to_underlying(enum_type value) noexcept { - return static_cast>(value); + return static_cast>(value); } -} // namespace path_tracking_pid +} // namespace path_tracking_pid From a992823a307cf3dda731f02d37c26e03073c1112 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 14:07:04 +0000 Subject: [PATCH 037/156] Replaced macros with constants. --- include/path_tracking_pid/controller.hpp | 6 +++--- .../path_tracking_pid/path_tracking_pid_local_planner.hpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 8b8b21d0..bbe6b643 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -19,9 +19,9 @@ template int sign(T val) { namespace path_tracking_pid { -#define RADIUS_EPS 0.001 // Smallest relevant radius [m] -#define VELOCITY_EPS 1e-3 // Neglegible velocity -#define LONG_DURATION 31556926 // A year (ros::Duration cannot be inf) +inline constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] +inline constexpr double VELOCITY_EPS = 1e-3; // Neglegible velocity +inline constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) enum class ControllerMode { diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 7a9c37ff..7e56df49 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -21,8 +21,8 @@ #include "tf2_ros/buffer.h" #include "path_tracking_pid/visualization.hpp" -#define MAP_PARALLEL_THRESH 0.2 -constexpr double DT_MAX=1.5; +inline constexpr double MAP_PARALLEL_THRESH = 0.2; +inline constexpr double DT_MAX = 1.5; BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x, y) From fe6ba238948b4ca5d8994e9bb8d08f06dc14b483 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 15 Feb 2022 15:08:02 +0000 Subject: [PATCH 038/156] Reworked formatting. --- include/path_tracking_pid/controller.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index bbe6b643..4b01aaad 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -19,9 +19,9 @@ template int sign(T val) { namespace path_tracking_pid { -inline constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] -inline constexpr double VELOCITY_EPS = 1e-3; // Neglegible velocity -inline constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) +inline constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] +inline constexpr double VELOCITY_EPS = 1e-3; // Neglegible velocity +inline constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) enum class ControllerMode { From 20e290bf522069adb5a8df2db4559c6d6b960bf7 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Tue, 15 Feb 2022 23:26:21 +0100 Subject: [PATCH 039/156] Quaternion of carrotTF was not initialized, set to proper value --- src/controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller.cpp b/src/controller.cpp index d9356a72..2ce180cc 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -132,7 +132,7 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi if (!track_base_link_enabled_) { // Add carrot length to plan using goal pose (we assume the last pose contains correct angle) - tf2::Transform carrotTF; + tf2::Transform carrotTF(tf2::Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)); carrotTF.setOrigin(tf2::Vector3(l_, 0.0, 0.0)); global_plan_tf_.push_back(last_transform * carrotTF); } From 74f808079e86f11bd9b130914af225419edfc5da Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 14:15:09 +0000 Subject: [PATCH 040/156] Moved constants to reduce scope. --- include/path_tracking_pid/controller.hpp | 4 ---- .../path_tracking_pid_local_planner.hpp | 3 --- src/common.hpp | 2 ++ src/controller.cpp | 9 +++++++++ src/path_tracking_pid_local_planner.cpp | 8 ++++++++ 5 files changed, 19 insertions(+), 7 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 4b01aaad..78e0890c 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -19,10 +19,6 @@ template int sign(T val) { namespace path_tracking_pid { -inline constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] -inline constexpr double VELOCITY_EPS = 1e-3; // Neglegible velocity -inline constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) - enum class ControllerMode { frontAxleLateral = 0, diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 7e56df49..cd3c9e91 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -21,9 +21,6 @@ #include "tf2_ros/buffer.h" #include "path_tracking_pid/visualization.hpp" -inline constexpr double MAP_PARALLEL_THRESH = 0.2; -inline constexpr double DT_MAX = 1.5; - BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x, y) diff --git a/src/common.hpp b/src/common.hpp index 79b4bd59..cc63d240 100644 --- a/src/common.hpp +++ b/src/common.hpp @@ -5,6 +5,8 @@ namespace path_tracking_pid { +inline constexpr double VELOCITY_EPS = 1e-3; // Neglegible velocity + // Converts an enumeration to its underlying type. template constexpr std::underlying_type_t to_underlying(enum_type value) noexcept diff --git a/src/controller.cpp b/src/controller.cpp index d9356a72..cad874aa 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -4,6 +4,8 @@ #include "path_tracking_pid/controller.hpp" +#include "common.hpp" + #include #include @@ -13,6 +15,13 @@ namespace path_tracking_pid { +namespace { + +constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] +constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) + +} // namespace anonymous + void Controller::setHolonomic(bool holonomic) { // Set configuration parameters diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 50be4047..ccbdbdf2 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -25,6 +25,14 @@ PLUGINLIB_EXPORT_CLASS(path_tracking_pid::TrackingPidLocalPlanner, mbf_costmap_c namespace path_tracking_pid { + +namespace { + +constexpr double MAP_PARALLEL_THRESH = 0.2; +constexpr double DT_MAX = 1.5; + +} // namespace anonymous + TrackingPidLocalPlanner::TrackingPidLocalPlanner() = default; TrackingPidLocalPlanner::~TrackingPidLocalPlanner() = default; From 20397493983c4e14a7465443ddbb183b45d8963e Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 14:59:54 +0000 Subject: [PATCH 041/156] Moved helper function. --- include/path_tracking_pid/controller.hpp | 6 ------ src/controller.cpp | 6 ++++++ 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 78e0890c..a899cace 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -10,12 +10,6 @@ #include "path_tracking_pid/PidDebug.h" #include "tf2/LinearMath/Transform.h" -// Typesafe sign implementation with signum: -// https://stackoverflow.com/a/4609795 -template int sign(T val) { - return (T(0) < val) - (val < T(0)); -} - namespace path_tracking_pid { diff --git a/src/controller.cpp b/src/controller.cpp index cad874aa..424bb330 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -20,6 +20,12 @@ namespace { constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) +// Typesafe sign implementation with signum: +// https://stackoverflow.com/a/4609795 +template int sign(T val) { + return (T(0) < val) - (val < T(0)); +} + } // namespace anonymous void Controller::setHolonomic(bool holonomic) From e7fa49a65153fa777907ae7dc55c4d5df909c66c Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 15:04:51 +0000 Subject: [PATCH 042/156] Replaced C-style arrays. --- include/path_tracking_pid/controller.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index a899cace..97bd72f8 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -284,8 +284,8 @@ class Controller double max_steering_x_acc_; double max_steering_yaw_vel_; double max_steering_yaw_acc_; - double inverse_kinematics_matrix_[2][2]; - double forward_kinematics_matrix_[2][2]; + std::array, 2> inverse_kinematics_matrix_; + std::array, 2> forward_kinematics_matrix_; bool debug_enabled_ = false; From 5706086de17773fbdbd0ad1a0a9af9305534266a Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Wed, 16 Feb 2022 09:17:42 +0100 Subject: [PATCH 043/156] Fully initialize at once --- src/controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 2ce180cc..a5bb8c6a 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -132,8 +132,8 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi if (!track_base_link_enabled_) { // Add carrot length to plan using goal pose (we assume the last pose contains correct angle) - tf2::Transform carrotTF(tf2::Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)); - carrotTF.setOrigin(tf2::Vector3(l_, 0.0, 0.0)); + tf2::Transform carrotTF(tf2::Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), + tf2::Vector3(l_, 0.0, 0.0)); global_plan_tf_.push_back(last_transform * carrotTF); } From 2cd54007657876aad9ba2b6f1ff172ea730baf79 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 15:08:46 +0000 Subject: [PATCH 044/156] Remove unsused member variables. --- include/path_tracking_pid/controller.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 97bd72f8..0db03e4a 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -272,9 +272,6 @@ class Controller // feedforward controller double feedforward_lat_ = 0.0; double feedforward_ang_ = 0.0; - double xvel_ = 0.0; - double yvel_ = 0.0; - double thvel_ = 0.0; // tricycle model bool use_tricycle_model_ = false; @@ -313,7 +310,6 @@ class Controller // Cutoff frequency for the derivative calculation in Hz. // Negative -> Has not been set by the user yet, so use a default. - double cutoff_frequency_long_ = -1.0; double cutoff_frequency_lat_ = -1.0; double cutoff_frequency_ang_ = -1.0; From 733488f97640909a3bfa422865aed7aa3f51bded Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 15:26:34 +0000 Subject: [PATCH 045/156] Mark overriden member functions as such. --- .../path_tracking_pid_local_planner.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index cd3c9e91..d03da7f0 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -48,14 +48,14 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co * @param tf a pointer to TransformListener in TF Buffer * @param costmap Costmap indicating free/occupied space */ - void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap); + void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap) override; /** * @brief Set the plan we should be following * @param global_plan Plan to follow as closely as we can * @return whether the plan was successfully updated or not */ - bool setPlan(const std::vector& global_plan); + bool setPlan(const std::vector& global_plan) override; /** * @brief Calculates the velocity command based on the current robot pose given by pose. See the interface in move @@ -63,7 +63,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co * @param cmd_vel Output the velocity command * @return true if succeded. */ - bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel); // NOLINT + bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) override; // NOLINT /** * @brief Calculates the velocity command based on the current robot pose given by pose. The velocity @@ -75,13 +75,13 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co * @return a status code defined in the move base flex ExePath action. */ uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, - geometry_msgs::TwistStamped& cmd_vel, std::string& message); // NOLINT + geometry_msgs::TwistStamped& cmd_vel, std::string& message) override; // NOLINT /** * @brief Returns true, if the goal is reached. Currently does not respect the parameters give * @return true, if the goal is reached */ - bool isGoalReached(); + bool isGoalReached() override; /** * @brief Returns true, if the goal is reached. Currently does not respect the parameters given. @@ -89,13 +89,13 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co * @param angle_tolerance Tolerance in the orientation to the goals orientation * @return true, if the goal is reached */ - bool isGoalReached(double dist_tolerance, double angle_tolerance); + bool isGoalReached(double dist_tolerance, double angle_tolerance) override; /** * @brief Canceles the planner. * @return True on cancel success. */ - bool cancel(); + bool cancel() override; /** Enumeration for custom SUCCESS feedback codes. See default ones: * https://github.com/magazino/move_base_flex/blob/master/mbf_msgs/action/ExePath.action From 2008bd66cff307a8800df53b2c1a5060ea7c7848 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 15:27:00 +0000 Subject: [PATCH 046/156] Distinguish between system and normal headers. --- CMakeLists.txt | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4fe52cd3..a5abb930 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,11 +69,8 @@ add_library(${PROJECT_NAME} add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake -) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) +include_directories(include) # Configure roslint for nodes From 00b4b4303dddab4f81a73974484f4f20487ed9ea Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 15:39:07 +0000 Subject: [PATCH 047/156] Even stricter compiler settings. --- CMakeLists.txt | 2 +- .../path_tracking_pid_local_planner.hpp | 3 ++- src/controller.cpp | 2 +- src/path_tracking_pid_local_planner.cpp | 10 +++++----- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a5abb930..0b915db1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -add_compile_options(-std=c++17 -Wall -Wextra -Wno-unused-parameter -Werror) +add_compile_options(-std=c++17 -Wall -Wextra -Wpedantic -Werror) project(path_tracking_pid) ## Find catkin and any catkin packages diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index d03da7f0..515614dc 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -169,4 +169,5 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co // Controller logic bool controller_debug_enabled_ = false; }; -}; // end namespace path_tracking_pid + +} // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index 424bb330..8a5a5690 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -232,7 +232,7 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi } void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, - geometry_msgs::Transform tf_base_to_steered_wheel, geometry_msgs::Twist steering_odom_twist, + geometry_msgs::Transform tf_base_to_steered_wheel, geometry_msgs::Twist /* steering_odom_twist */, const std::vector& global_plan) { setPlan(current_tf, odom_twist, global_plan); diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index ccbdbdf2..b4c10dfd 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -37,7 +37,7 @@ TrackingPidLocalPlanner::TrackingPidLocalPlanner() = default; TrackingPidLocalPlanner::~TrackingPidLocalPlanner() = default; -void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig& config, uint32_t level) +void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig& config, uint32_t /* level */) { pid_controller_.configure(config); controller_debug_enabled_ = config.controller_debug_enabled; @@ -520,9 +520,9 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() return max_cost; } -uint32_t TrackingPidLocalPlanner::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, - const geometry_msgs::TwistStamped& velocity, - geometry_msgs::TwistStamped& cmd_vel, std::string& message) +uint32_t TrackingPidLocalPlanner::computeVelocityCommands(const geometry_msgs::PoseStamped& /* pose */, + const geometry_msgs::TwistStamped& /* velocity */, + geometry_msgs::TwistStamped& cmd_vel, std::string& /* message */) { if (!initialized_) { @@ -570,7 +570,7 @@ bool TrackingPidLocalPlanner::isGoalReached() return pid_controller_.getControllerState().end_reached && !cancel_in_progress_; } -bool TrackingPidLocalPlanner::isGoalReached(double dist_tolerance, double angle_tolerance) +bool TrackingPidLocalPlanner::isGoalReached(double /* dist_tolerance */, double /* angle_tolerance */) { return isGoalReached(); } From 2611f8b7901d9b86133e05dc2136158d2d0a2a48 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 15:39:51 +0000 Subject: [PATCH 048/156] Replaced bind with lambda. --- .../path_tracking_pid/path_tracking_pid_local_planner.hpp | 3 +-- src/path_tracking_pid_local_planner.cpp | 6 ++---- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 515614dc..af5cd057 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -110,9 +110,8 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co /** * Accept a new configuration for the PID controller * @param config the new configuration - * @param level */ - void reconfigure_pid(path_tracking_pid::PidConfig& config, uint32_t level); + void reconfigure_pid(path_tracking_pid::PidConfig& config); void pauseCallback(std_msgs::Bool pause); diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index b4c10dfd..e50da08e 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -37,7 +37,7 @@ TrackingPidLocalPlanner::TrackingPidLocalPlanner() = default; TrackingPidLocalPlanner::~TrackingPidLocalPlanner() = default; -void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig& config, uint32_t /* level */) +void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig& config) { pid_controller_.configure(config); controller_debug_enabled_ = config.controller_debug_enabled; @@ -57,9 +57,7 @@ void TrackingPidLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, ROS_DEBUG("TrackingPidLocalPlanner::initialize(%s, ..., ...)", name.c_str()); // setup dynamic reconfigure pid_server_ = std::make_unique>(config_mutex_, nh); - dynamic_reconfigure::Server::CallbackType cb1; - cb1 = boost::bind(&TrackingPidLocalPlanner::reconfigure_pid, this, _1, _2); - pid_server_->setCallback(cb1); + pid_server_->setCallback([this](auto& config, auto) { this->reconfigure_pid(config); }); pid_controller_.setEnabled(false); bool holonomic_robot; From 7f18225665d726dfdf9d2198256055c7de5f9734 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 16:24:48 +0000 Subject: [PATCH 049/156] Replaced member variables with locals. --- include/path_tracking_pid/controller.hpp | 9 -------- src/controller.cpp | 28 ++++++++++++------------ 2 files changed, 14 insertions(+), 23 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 0db03e4a..1a9c891b 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -323,15 +323,6 @@ class Controller // Anti-windup term. Limits the absolute value of the integral term. double windup_limit_ = 1000.0; - // Temporary variables - - double proportional_lat_ = 0; // proportional term of output - double integral_lat_ = 0; // integral term of output - double derivative_lat_ = 0; // derivative term of output - double proportional_ang_ = 0; // proportional term of output - double integral_ang_ = 0; // integral term of output - double derivative_ang_ = 0; // derivative term of output - // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at // 1/4 of the sample rate. double c_lat_ = 1.; diff --git a/src/controller.cpp b/src/controller.cpp index 20a8968f..aff0ce62 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -582,13 +582,13 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, (-2 * c_ang_ * c_ang_ + 2) * controller_state_.filtered_error_deriv_ang.at(1)); // calculate the control effort - proportional_lat_ = Kp_lat_ * controller_state_.filtered_error_lat.at(0); - integral_lat_ = Ki_lat_ * controller_state_.error_integral_lat; - derivative_lat_ = Kd_lat_ * controller_state_.filtered_error_deriv_lat.at(0); + const auto proportional_lat = Kp_lat_ * controller_state_.filtered_error_lat.at(0); + const auto integral_lat = Ki_lat_ * controller_state_.error_integral_lat; + const auto derivative_lat = Kd_lat_ * controller_state_.filtered_error_deriv_lat.at(0); - proportional_ang_ = Kp_ang_ * controller_state_.filtered_error_ang.at(0); - integral_ang_ = Ki_ang_ * controller_state_.error_integral_ang; - derivative_ang_ = Kd_ang_ * controller_state_.filtered_error_deriv_ang.at(0); + const auto proportional_ang = Kp_ang_ * controller_state_.filtered_error_ang.at(0); + const auto integral_ang = Ki_ang_ * controller_state_.error_integral_ang; + const auto derivative_ang = Kd_ang_ * controller_state_.filtered_error_deriv_ang.at(0); /***** Compute forward velocity *****/ @@ -746,9 +746,9 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, control_effort_ang_ = 0.0; if (feedback_lat_enabled_) - control_effort_lat_ = proportional_lat_ + integral_lat_ + derivative_lat_; + control_effort_lat_ = proportional_lat + integral_lat + derivative_lat; if (feedback_ang_enabled_) - control_effort_ang_ = proportional_ang_ + integral_ang_ + derivative_ang_; + control_effort_ang_ = proportional_ang + integral_ang + derivative_ang; //***** Feedforward control *****// @@ -794,16 +794,16 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, pid_debug->tracking_error.angular.z = controller_state_.tracking_error_ang; pid_debug->proportional.linear.x = 0.0; - pid_debug->proportional.linear.y = proportional_lat_; - pid_debug->proportional.angular.z = proportional_ang_; + pid_debug->proportional.linear.y = proportional_lat; + pid_debug->proportional.angular.z = proportional_ang; pid_debug->integral.linear.x = 0.0; - pid_debug->integral.linear.y = integral_lat_; - pid_debug->integral.angular.z = integral_ang_; + pid_debug->integral.linear.y = integral_lat; + pid_debug->integral.angular.z = integral_ang; pid_debug->derivative.linear.x = 0.0; - pid_debug->derivative.linear.y = derivative_lat_; - pid_debug->derivative.angular.z = derivative_ang_; + pid_debug->derivative.linear.y = derivative_lat; + pid_debug->derivative.angular.z = derivative_ang; pid_debug->feedforward.linear.x = new_x_vel; pid_debug->feedforward.linear.y = feedforward_lat_; From e107cb9c1852f9c1906cb47de0e98424a7069633 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 16:31:02 +0000 Subject: [PATCH 050/156] Replaced member variable with local. --- include/path_tracking_pid/controller.hpp | 3 --- src/controller.cpp | 24 ++++++++++++------------ 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 1a9c891b..fcd3488d 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -328,9 +328,6 @@ class Controller double c_lat_ = 1.; double c_ang_ = 1.; - // Used to check for tan(0)==>NaN in the filter calculation - double tan_filt_ = 1.; - // MPC settings int mpc_max_fwd_iter_; // Define # of steps that you look into the future with MPC [-] int mpc_max_vel_optimization_iter_; // Set maximum # of velocity bisection iterations diff --git a/src/controller.cpp b/src/controller.cpp index aff0ce62..2ea4d88f 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -514,28 +514,28 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, if (cutoff_frequency_lat_ != -1) { // Check if tan(_) is really small, could cause c = NaN - tan_filt_ = tan((cutoff_frequency_lat_ * 6.2832) * dt.toSec() / 2); + auto tan_filt = tan((cutoff_frequency_lat_ * 6.2832) * dt.toSec() / 2); // Avoid tan(0) ==> NaN - if ((tan_filt_ <= 0.) && (tan_filt_ > -0.01)) - tan_filt_ = -0.01; - if ((tan_filt_ >= 0.) && (tan_filt_ < 0.01)) - tan_filt_ = 0.01; + if ((tan_filt <= 0.) && (tan_filt > -0.01)) + tan_filt = -0.01; + if ((tan_filt >= 0.) && (tan_filt < 0.01)) + tan_filt = 0.01; - c_lat_ = 1 / tan_filt_; + c_lat_ = 1 / tan_filt; } if (cutoff_frequency_ang_ != -1) { // Check if tan(_) is really small, could cause c = NaN - tan_filt_ = tan((cutoff_frequency_ang_ * 6.2832) * dt.toSec() / 2); + auto tan_filt = tan((cutoff_frequency_ang_ * 6.2832) * dt.toSec() / 2); // Avoid tan(0) ==> NaN - if ((tan_filt_ <= 0.) && (tan_filt_ > -0.01)) - tan_filt_ = -0.01; - if ((tan_filt_ >= 0.) && (tan_filt_ < 0.01)) - tan_filt_ = 0.01; + if ((tan_filt <= 0.) && (tan_filt > -0.01)) + tan_filt = -0.01; + if ((tan_filt >= 0.) && (tan_filt < 0.01)) + tan_filt = 0.01; - c_ang_ = 1 / tan_filt_; + c_ang_ = 1 / tan_filt; } controller_state_.filtered_error_lat.at(2) = controller_state_.filtered_error_lat.at(1); From 4cadb68fd9012df702bdee8094a76822fd4bfe74 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 17:02:57 +0000 Subject: [PATCH 051/156] Replaced member variables with constants. --- include/path_tracking_pid/controller.hpp | 15 ------ src/controller.cpp | 61 +++++++++++++++--------- 2 files changed, 38 insertions(+), 38 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index fcd3488d..3c8db60c 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -308,21 +308,6 @@ class Controller double vel_max_external_ = INFINITY; // Dynamic external max velocity requirement (e.g. no more power available) double vel_max_obstacle_ = INFINITY; // Can be zero if lethal obstacles are detected - // Cutoff frequency for the derivative calculation in Hz. - // Negative -> Has not been set by the user yet, so use a default. - double cutoff_frequency_lat_ = -1.0; - double cutoff_frequency_ang_ = -1.0; - - // Upper and lower saturation limits - double upper_limit_ = 100.0; - double lower_limit_ = -100.0; - - double ang_upper_limit_ = 100.0; - double ang_lower_limit_ = -100.0; - - // Anti-windup term. Limits the absolute value of the integral term. - double windup_limit_ = 1000.0; - // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at // 1/4 of the sample rate. double c_lat_ = 1.; diff --git a/src/controller.cpp b/src/controller.cpp index 2ea4d88f..6e576934 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -20,6 +20,21 @@ namespace { constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) +// Cutoff frequency for the derivative calculation in Hz. +constexpr double cutoff_frequency_lat = -1.0; +constexpr double cutoff_frequency_ang = -1.0; + +// Upper and lower saturation limits +constexpr double lat_upper_limit = 100.0; +constexpr double lat_lower_limit = -100.0; + +constexpr double ang_upper_limit = 100.0; +constexpr double ang_lower_limit = -100.0; + +// Anti-windup term. Limits the absolute value of the integral term. +constexpr double windup_limit = 1000.0; + + // Typesafe sign implementation with signum: // https://stackoverflow.com/a/4609795 template int sign(T val) { @@ -501,20 +516,20 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, controller_state_.error_integral_ang += controller_state_.error_ang.at(0) * dt.toSec(); // Apply windup limit to limit the size of the integral term - if (controller_state_.error_integral_lat > std::abs(windup_limit_)) - controller_state_.error_integral_lat = std::abs(windup_limit_); - if (controller_state_.error_integral_lat < -std::abs(windup_limit_)) - controller_state_.error_integral_lat = -std::abs(windup_limit_); - if (controller_state_.error_integral_ang > std::abs(windup_limit_)) - controller_state_.error_integral_ang = std::abs(windup_limit_); - if (controller_state_.error_integral_ang < -std::abs(windup_limit_)) - controller_state_.error_integral_ang = -std::abs(windup_limit_); + if (controller_state_.error_integral_lat > std::abs(windup_limit)) + controller_state_.error_integral_lat = std::abs(windup_limit); + if (controller_state_.error_integral_lat < -std::abs(windup_limit)) + controller_state_.error_integral_lat = -std::abs(windup_limit); + if (controller_state_.error_integral_ang > std::abs(windup_limit)) + controller_state_.error_integral_ang = std::abs(windup_limit); + if (controller_state_.error_integral_ang < -std::abs(windup_limit)) + controller_state_.error_integral_ang = -std::abs(windup_limit); // My filter reference was Julius O. Smith III, Intro. to Digital Filters With Audio Applications. - if (cutoff_frequency_lat_ != -1) + if (cutoff_frequency_lat != -1) { // Check if tan(_) is really small, could cause c = NaN - auto tan_filt = tan((cutoff_frequency_lat_ * 6.2832) * dt.toSec() / 2); + auto tan_filt = tan((cutoff_frequency_lat * 6.2832) * dt.toSec() / 2); // Avoid tan(0) ==> NaN if ((tan_filt <= 0.) && (tan_filt > -0.01)) @@ -524,10 +539,10 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, c_lat_ = 1 / tan_filt; } - if (cutoff_frequency_ang_ != -1) + if (cutoff_frequency_ang != -1) { // Check if tan(_) is really small, could cause c = NaN - auto tan_filt = tan((cutoff_frequency_ang_ * 6.2832) * dt.toSec() / 2); + auto tan_filt = tan((cutoff_frequency_ang * 6.2832) * dt.toSec() / 2); // Avoid tan(0) ==> NaN if ((tan_filt <= 0.) && (tan_filt > -0.01)) @@ -773,15 +788,15 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, // Apply saturation limits - if (control_effort_lat_ > upper_limit_) - control_effort_lat_ = upper_limit_; - else if (control_effort_lat_ < lower_limit_) - control_effort_lat_ = lower_limit_; + if (control_effort_lat_ > lat_upper_limit) + control_effort_lat_ = lat_upper_limit; + else if (control_effort_lat_ < lat_lower_limit) + control_effort_lat_ = lat_lower_limit; - if (control_effort_ang_ > ang_upper_limit_) - control_effort_ang_ = ang_upper_limit_; - else if (control_effort_ang_ < ang_lower_limit_) - control_effort_ang_ = ang_lower_limit_; + if (control_effort_ang_ > ang_upper_limit) + control_effort_ang_ = ang_upper_limit; + else if (control_effort_ang_ < ang_lower_limit) + control_effort_ang_ = ang_lower_limit; // Populate debug output // Error topic containing the 'control' error on which the PID acts @@ -1130,9 +1145,9 @@ void Controller::printParameters() ROS_INFO("Robot type (holonomic): (%i)", holonomic_robot_enable_); - ROS_INFO("Integral-windup limit: %f", windup_limit_); - ROS_INFO("Saturation limits xy: %f/%f", upper_limit_, lower_limit_); - ROS_INFO("Saturation limits ang: %f/%f", ang_upper_limit_, ang_lower_limit_); + ROS_INFO("Integral-windup limit: %f", windup_limit); + ROS_INFO("Saturation limits xy: %f/%f", lat_upper_limit, lat_lower_limit); + ROS_INFO("Saturation limits ang: %f/%f", ang_upper_limit, ang_lower_limit); ROS_INFO("-----------------------------------------"); } From 7841bdc6bc83f769c5fb6cc3de7e012436c532c7 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 17:03:24 +0000 Subject: [PATCH 052/156] Ignore Python cache files. --- .gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..c18dd8d8 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ From 941217e810d2a3428e41750201a4f96b7912977e Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 16 Feb 2022 10:43:31 +0000 Subject: [PATCH 053/156] Reworked by moving gitignore. --- .gitignore => test/.gitignore | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .gitignore => test/.gitignore (100%) diff --git a/.gitignore b/test/.gitignore similarity index 100% rename from .gitignore rename to test/.gitignore From 982d02cb12065a6c3269a9c212f775223e78fa6d Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 17:46:08 +0000 Subject: [PATCH 054/156] Switched to std::clamp(). --- src/controller.cpp | 21 ++++----------------- 1 file changed, 4 insertions(+), 17 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 6e576934..2f4609a5 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -516,14 +516,8 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, controller_state_.error_integral_ang += controller_state_.error_ang.at(0) * dt.toSec(); // Apply windup limit to limit the size of the integral term - if (controller_state_.error_integral_lat > std::abs(windup_limit)) - controller_state_.error_integral_lat = std::abs(windup_limit); - if (controller_state_.error_integral_lat < -std::abs(windup_limit)) - controller_state_.error_integral_lat = -std::abs(windup_limit); - if (controller_state_.error_integral_ang > std::abs(windup_limit)) - controller_state_.error_integral_ang = std::abs(windup_limit); - if (controller_state_.error_integral_ang < -std::abs(windup_limit)) - controller_state_.error_integral_ang = -std::abs(windup_limit); + controller_state_.error_integral_lat = std::clamp(controller_state_.error_integral_lat, -windup_limit, windup_limit); + controller_state_.error_integral_ang = std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit); // My filter reference was Julius O. Smith III, Intro. to Digital Filters With Audio Applications. if (cutoff_frequency_lat != -1) @@ -788,15 +782,8 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, // Apply saturation limits - if (control_effort_lat_ > lat_upper_limit) - control_effort_lat_ = lat_upper_limit; - else if (control_effort_lat_ < lat_lower_limit) - control_effort_lat_ = lat_lower_limit; - - if (control_effort_ang_ > ang_upper_limit) - control_effort_ang_ = ang_upper_limit; - else if (control_effort_ang_ < ang_lower_limit) - control_effort_ang_ = ang_lower_limit; + control_effort_lat_ = std::clamp(control_effort_lat_, lat_lower_limit, lat_upper_limit); + control_effort_ang_ = std::clamp(control_effort_ang_, ang_lower_limit, ang_upper_limit); // Populate debug output // Error topic containing the 'control' error on which the PID acts From 6b2264fa9924578e1a319c7d47d664872f6ec01c Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 16:16:49 +0000 Subject: [PATCH 055/156] Explicitly disabled copy/move. --- include/path_tracking_pid/controller.hpp | 8 +++----- .../details/no_copy_no_move.hpp | 20 +++++++++++++++++++ .../path_tracking_pid_local_planner.hpp | 7 +++---- src/path_tracking_pid_local_planner.cpp | 4 ---- 4 files changed, 26 insertions(+), 13 deletions(-) create mode 100644 include/path_tracking_pid/details/no_copy_no_move.hpp diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 3c8db60c..a673e6cc 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include #include @@ -55,13 +57,9 @@ struct ControllerState std::array filtered_error_deriv_ang = {0.0, 0.0, 0.0}; }; -class Controller +class Controller : private details::NoCopyNoMove { public: - Controller() = default; - - ~Controller() = default; - /** * Set holonomic configuration of the controller * @param holonomic is holonomic robot? diff --git a/include/path_tracking_pid/details/no_copy_no_move.hpp b/include/path_tracking_pid/details/no_copy_no_move.hpp new file mode 100644 index 00000000..226f2a94 --- /dev/null +++ b/include/path_tracking_pid/details/no_copy_no_move.hpp @@ -0,0 +1,20 @@ +#pragma once + +namespace path_tracking_pid::details +{ + +// Helper class to mark a class as non-copyable and non-moveable. Use private inheritance. +class NoCopyNoMove +{ +public: + NoCopyNoMove(const NoCopyNoMove&) = delete; + NoCopyNoMove(NoCopyNoMove&&) = delete; + NoCopyNoMove& operator=(const NoCopyNoMove&) = delete; + NoCopyNoMove& operator=(NoCopyNoMove&&) = delete; + +protected: + NoCopyNoMove() = default; + ~NoCopyNoMove() = default; +}; + +} // namespace path_tracking_pid::details diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index af5cd057..804366d3 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include #include #include @@ -26,7 +28,7 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x, namespace path_tracking_pid { -class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController +class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController, private details::NoCopyNoMove { private: typedef boost::geometry::model::ring polygon_t; @@ -39,9 +41,6 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co } public: - TrackingPidLocalPlanner(); - ~TrackingPidLocalPlanner(); - /** * @brief Initialize local planner * @param name The name of the planner diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index e50da08e..51d7d437 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -33,10 +33,6 @@ constexpr double DT_MAX = 1.5; } // namespace anonymous -TrackingPidLocalPlanner::TrackingPidLocalPlanner() = default; - -TrackingPidLocalPlanner::~TrackingPidLocalPlanner() = default; - void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig& config) { pid_controller_.configure(config); From b0a26ef7ab72a7d98e1b4d953c6d220f2d2bd71c Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 16 Feb 2022 15:03:53 +0000 Subject: [PATCH 056/156] Reworked by switching to Boost. --- include/path_tracking_pid/controller.hpp | 4 ++-- .../details/no_copy_no_move.hpp | 20 ------------------- .../path_tracking_pid_local_planner.hpp | 4 ++-- 3 files changed, 4 insertions(+), 24 deletions(-) delete mode 100644 include/path_tracking_pid/details/no_copy_no_move.hpp diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index a673e6cc..f95d918c 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -1,10 +1,10 @@ #pragma once -#include #include #include +#include "boost/noncopyable.hpp" #include "geometry_msgs/PoseStamped.h" #include "geometry_msgs/Transform.h" #include "geometry_msgs/Twist.h" @@ -57,7 +57,7 @@ struct ControllerState std::array filtered_error_deriv_ang = {0.0, 0.0, 0.0}; }; -class Controller : private details::NoCopyNoMove +class Controller : private boost::noncopyable { public: /** diff --git a/include/path_tracking_pid/details/no_copy_no_move.hpp b/include/path_tracking_pid/details/no_copy_no_move.hpp deleted file mode 100644 index 226f2a94..00000000 --- a/include/path_tracking_pid/details/no_copy_no_move.hpp +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -namespace path_tracking_pid::details -{ - -// Helper class to mark a class as non-copyable and non-moveable. Use private inheritance. -class NoCopyNoMove -{ -public: - NoCopyNoMove(const NoCopyNoMove&) = delete; - NoCopyNoMove(NoCopyNoMove&&) = delete; - NoCopyNoMove& operator=(const NoCopyNoMove&) = delete; - NoCopyNoMove& operator=(NoCopyNoMove&&) = delete; - -protected: - NoCopyNoMove() = default; - ~NoCopyNoMove() = default; -}; - -} // namespace path_tracking_pid::details diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 804366d3..d3bf47fe 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -1,6 +1,5 @@ #pragma once -#include #include #include @@ -9,6 +8,7 @@ #include #include "./controller.hpp" +#include "boost/noncopyable.hpp" #include "boost/thread/recursive_mutex.hpp" #include "costmap_2d/costmap_2d_ros.h" #include "dynamic_reconfigure/server.h" @@ -28,7 +28,7 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x, namespace path_tracking_pid { -class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController, private details::NoCopyNoMove +class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController, private boost::noncopyable { private: typedef boost::geometry::model::ring polygon_t; From d1b30032fe3d91c571d2d8a35f16de596caa1e0e Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 17:49:41 +0000 Subject: [PATCH 057/156] Removed dead code. --- include/path_tracking_pid/controller.hpp | 5 -- src/controller.cpp | 61 +++++++----------------- 2 files changed, 17 insertions(+), 49 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index f95d918c..5db6fa29 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -306,11 +306,6 @@ class Controller : private boost::noncopyable double vel_max_external_ = INFINITY; // Dynamic external max velocity requirement (e.g. no more power available) double vel_max_obstacle_ = INFINITY; // Can be zero if lethal obstacles are detected - // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at - // 1/4 of the sample rate. - double c_lat_ = 1.; - double c_ang_ = 1.; - // MPC settings int mpc_max_fwd_iter_; // Define # of steps that you look into the future with MPC [-] int mpc_max_vel_optimization_iter_; // Set maximum # of velocity bisection iterations diff --git a/src/controller.cpp b/src/controller.cpp index 2f4609a5..1e156b89 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -20,10 +20,6 @@ namespace { constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) -// Cutoff frequency for the derivative calculation in Hz. -constexpr double cutoff_frequency_lat = -1.0; -constexpr double cutoff_frequency_ang = -1.0; - // Upper and lower saturation limits constexpr double lat_upper_limit = 100.0; constexpr double lat_lower_limit = -100.0; @@ -34,6 +30,11 @@ constexpr double ang_lower_limit = -100.0; // Anti-windup term. Limits the absolute value of the integral term. constexpr double windup_limit = 1000.0; +// Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at +// 1/4 of the sample rate. +constexpr double c_lat = 1.; +constexpr double c_ang = 1.; + // Typesafe sign implementation with signum: // https://stackoverflow.com/a/4609795 @@ -519,49 +520,21 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, controller_state_.error_integral_lat = std::clamp(controller_state_.error_integral_lat, -windup_limit, windup_limit); controller_state_.error_integral_ang = std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit); - // My filter reference was Julius O. Smith III, Intro. to Digital Filters With Audio Applications. - if (cutoff_frequency_lat != -1) - { - // Check if tan(_) is really small, could cause c = NaN - auto tan_filt = tan((cutoff_frequency_lat * 6.2832) * dt.toSec() / 2); - - // Avoid tan(0) ==> NaN - if ((tan_filt <= 0.) && (tan_filt > -0.01)) - tan_filt = -0.01; - if ((tan_filt >= 0.) && (tan_filt < 0.01)) - tan_filt = 0.01; - - c_lat_ = 1 / tan_filt; - } - if (cutoff_frequency_ang != -1) - { - // Check if tan(_) is really small, could cause c = NaN - auto tan_filt = tan((cutoff_frequency_ang * 6.2832) * dt.toSec() / 2); - - // Avoid tan(0) ==> NaN - if ((tan_filt <= 0.) && (tan_filt > -0.01)) - tan_filt = -0.01; - if ((tan_filt >= 0.) && (tan_filt < 0.01)) - tan_filt = 0.01; - - c_ang_ = 1 / tan_filt; - } - controller_state_.filtered_error_lat.at(2) = controller_state_.filtered_error_lat.at(1); controller_state_.filtered_error_lat.at(1) = controller_state_.filtered_error_lat.at(0); controller_state_.filtered_error_lat.at(0) = - (1 / (1 + c_lat_ * c_lat_ + M_SQRT2 * c_lat_)) * + (1 / (1 + c_lat * c_lat + M_SQRT2 * c_lat)) * (controller_state_.error_lat.at(2) + 2 * controller_state_.error_lat.at(1) + controller_state_.error_lat.at(0) - - (c_lat_ * c_lat_ - M_SQRT2 * c_lat_ + 1) * controller_state_.filtered_error_lat.at(2) - - (-2 * c_lat_ * c_lat_ + 2) * controller_state_.filtered_error_lat.at(1)); + (c_lat * c_lat - M_SQRT2 * c_lat + 1) * controller_state_.filtered_error_lat.at(2) - + (-2 * c_lat * c_lat + 2) * controller_state_.filtered_error_lat.at(1)); controller_state_.filtered_error_ang.at(2) = controller_state_.filtered_error_ang.at(1); controller_state_.filtered_error_ang.at(1) = controller_state_.filtered_error_ang.at(0); controller_state_.filtered_error_ang.at(0) = - (1 / (1 + c_ang_ * c_ang_ + M_SQRT2 * c_ang_)) * + (1 / (1 + c_ang * c_ang + M_SQRT2 * c_ang)) * (controller_state_.error_ang.at(2) + 2 * controller_state_.error_ang.at(1) + controller_state_.error_ang.at(0) - - (c_ang_ * c_ang_ - M_SQRT2 * c_ang_ + 1) * controller_state_.filtered_error_ang.at(2) - - (-2 * c_ang_ * c_ang_ + 2) * controller_state_.filtered_error_ang.at(1)); + (c_ang * c_ang - M_SQRT2 * c_ang + 1) * controller_state_.filtered_error_ang.at(2) - + (-2 * c_ang * c_ang + 2) * controller_state_.filtered_error_ang.at(1)); // Take derivative of error, first the raw unfiltered data: controller_state_.error_deriv_lat.at(2) = controller_state_.error_deriv_lat.at(1); @@ -571,11 +544,11 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, controller_state_.filtered_error_deriv_lat.at(2) = controller_state_.filtered_error_deriv_lat.at(1); controller_state_.filtered_error_deriv_lat.at(1) = controller_state_.filtered_error_deriv_lat.at(0); controller_state_.filtered_error_deriv_lat.at(0) = - (1 / (1 + c_lat_ * c_lat_ + M_SQRT2 * c_lat_)) * + (1 / (1 + c_lat * c_lat + M_SQRT2 * c_lat)) * (controller_state_.error_deriv_lat.at(2) + 2 * controller_state_.error_deriv_lat.at(1) + controller_state_.error_deriv_lat.at(0) - - (c_lat_ * c_lat_ - M_SQRT2 * c_lat_ + 1) * controller_state_.filtered_error_deriv_lat.at(2) - - (-2 * c_lat_ * c_lat_ + 2) * controller_state_.filtered_error_deriv_lat.at(1)); + (c_lat * c_lat - M_SQRT2 * c_lat + 1) * controller_state_.filtered_error_deriv_lat.at(2) - + (-2 * c_lat * c_lat + 2) * controller_state_.filtered_error_deriv_lat.at(1)); controller_state_.error_deriv_ang.at(2) = controller_state_.error_deriv_ang.at(1); controller_state_.error_deriv_ang.at(1) = controller_state_.error_deriv_ang.at(0); @@ -584,11 +557,11 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, controller_state_.filtered_error_deriv_ang.at(2) = controller_state_.filtered_error_deriv_ang.at(1); controller_state_.filtered_error_deriv_ang.at(1) = controller_state_.filtered_error_deriv_ang.at(0); controller_state_.filtered_error_deriv_ang.at(0) = - (1 / (1 + c_ang_ * c_ang_ + M_SQRT2 * c_ang_)) * + (1 / (1 + c_ang * c_ang + M_SQRT2 * c_ang)) * (controller_state_.error_deriv_ang.at(2) + 2 * controller_state_.error_deriv_ang.at(1) + controller_state_.error_deriv_ang.at(0) - - (c_ang_ * c_ang_ - M_SQRT2 * c_ang_ + 1) * controller_state_.filtered_error_deriv_ang.at(2) - - (-2 * c_ang_ * c_ang_ + 2) * controller_state_.filtered_error_deriv_ang.at(1)); + (c_ang * c_ang - M_SQRT2 * c_ang + 1) * controller_state_.filtered_error_deriv_ang.at(2) - + (-2 * c_ang * c_ang + 2) * controller_state_.filtered_error_deriv_ang.at(1)); // calculate the control effort const auto proportional_lat = Kp_lat_ * controller_state_.filtered_error_lat.at(0); From 120ffb6cc24a5b465f8d38ee322f30829fc14e5a Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 17:56:09 +0000 Subject: [PATCH 058/156] Remove dead code. --- include/path_tracking_pid/controller.hpp | 14 -------- src/controller.cpp | 41 ------------------------ 2 files changed, 55 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 5db6fa29..d8e708e0 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -15,14 +15,6 @@ namespace path_tracking_pid { -enum class ControllerMode -{ - frontAxleLateral = 0, - rearAxleLateral = 1, - rearAxleAngular = 2, - fixOrientation = 3, -}; - struct TricycleSteeringCmdVel { double steering_angle = 0.0; @@ -159,12 +151,6 @@ class Controller : private boost::noncopyable double mpc_based_max_vel(const double target_x_vel, geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist); - /** - * Select mode for the controller - * @param mode - */ - void selectMode(ControllerMode mode); - /** * Set dynamic parameters for the PID controller * @param config diff --git a/src/controller.cpp b/src/controller.cpp index 1e156b89..ac42e9f9 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -1045,47 +1045,6 @@ double Controller::mpc_based_max_vel(const double target_x_vel, geometry_msgs::T return std::abs(mpc_vel_limit); } -void Controller::selectMode(ControllerMode mode) -{ - switch (mode) - { - case ControllerMode::frontAxleLateral: - // Front axle lateral controller (default) - l_ = 0.5; - feedback_lat_enabled_ = true; - feedback_ang_enabled_ = false; - feedforward_lat_enabled_ = true; - feedforward_ang_enabled_ = false; - break; - case ControllerMode::rearAxleLateral: - // Rear axle lateral control - l_ = 0.0; // To prevent singular configuration - feedback_lat_enabled_ = true; - feedback_ang_enabled_ = false; - feedforward_lat_enabled_ = true; - feedforward_ang_enabled_ = false; - break; - case ControllerMode::rearAxleAngular: - // Rear axle angular controller - l_ = 0.0; - feedback_lat_enabled_ = false; - feedback_ang_enabled_ = true; - feedforward_lat_enabled_ = false; - feedforward_ang_enabled_ = false; - break; - case ControllerMode::fixOrientation: - // Fix orientation controller - l_ = 0.0; - feedback_lat_enabled_ = false; - feedback_ang_enabled_ = true; - feedforward_lat_enabled_ = false; - feedforward_ang_enabled_ = true; - break; - } - - printParameters(); -} - void Controller::printParameters() { ROS_INFO("CONTROLLER PARAMETERS"); From 1cf7b91e9cac90f9681891af680c0f932cb46220 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 10 Feb 2022 08:11:07 +0000 Subject: [PATCH 059/156] Fixed signed/unsigned conversion warnings. --- src/controller.cpp | 24 +++++++++++------------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index ac42e9f9..f81f4092 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -185,38 +185,36 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi tf2::Transform deltaPlan; // We define segment0 to be the segment connecting pose0 and pose1. // Hence, when picking the starting path's pose, we mean to start at the segment connecting that and the next pose. - for (int idx_path = global_plan_tf_.size() - 2; idx_path >= 0; --idx_path) + for (auto idx_path = global_plan_tf_.size() - 1; idx_path > 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ - dist_to_segment = distToSegmentSquared(current_tf2, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]); + dist_to_segment = distToSegmentSquared(current_tf2, global_plan_tf_[idx_path - 1], global_plan_tf_[idx_path]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! if (dist_to_segment < minimum_distance_to_path) { minimum_distance_to_path = dist_to_segment; - controller_state_.current_global_plan_index = idx_path; + controller_state_.current_global_plan_index = idx_path - 1; } /* Create distance and turning radius vectors once for usage later */ - deltaPlan = global_plan_tf_[idx_path].inverseTimes(global_plan_tf_[idx_path + 1]); + deltaPlan = global_plan_tf_[idx_path - 1].inverseTimes(global_plan_tf_[idx_path]); double dpX = deltaPlan.getOrigin().x(); double dpY = deltaPlan.getOrigin().y(); iterative_dist_to_goal += hypot(dpX, dpY); - distance_to_goal_vector_[idx_path] = iterative_dist_to_goal; + distance_to_goal_vector_[idx_path - 1] = iterative_dist_to_goal; // compute turning radius based on trigonometric analysis // radius such that next pose is connected from current pose with a semi-circle double dpXY2 = dpY*dpY + dpX*dpX; if (dpXY2 < FLT_EPSILON) { - turning_radius_inv_vector_[idx_path] = std::numeric_limits::infinity(); + turning_radius_inv_vector_[idx_path - 1] = std::numeric_limits::infinity(); } else { - // 0.5*dpY*( 1 + dpX*dpX/(dpY*dPY) ); - // turning_radius_vector[idx_path] = 0.5*(1/dpY)*( dpY*dpY + dpX*dpX ); - turning_radius_inv_vector_[idx_path] = 2*dpY/dpXY2; + turning_radius_inv_vector_[idx_path - 1] = 2*dpY/dpXY2; } - ROS_DEBUG("turning_radius_inv_vector[%d] = %f", idx_path, turning_radius_inv_vector_[idx_path]); + ROS_DEBUG("turning_radius_inv_vector[%zu] = %f", idx_path - 1, turning_radius_inv_vector_[idx_path - 1]); } // Set initial velocity @@ -354,9 +352,9 @@ tf2::Transform Controller::findPositionOnPlan(const geometry_msgs::Transform cur } // Then look backwards - for (int idx_path = controller_state_ptr->current_global_plan_index - 1; idx_path >= 0; idx_path--) + for (auto idx_path = controller_state_ptr->current_global_plan_index; idx_path > 0; --idx_path) { - error = current_tf2.inverseTimes(global_plan_tf_[idx_path]); + error = current_tf2.inverseTimes(global_plan_tf_[idx_path - 1]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! distance_to_path = hypot(error.getOrigin().x(), error.getOrigin().y(), error.getOrigin().z()); @@ -364,7 +362,7 @@ tf2::Transform Controller::findPositionOnPlan(const geometry_msgs::Transform cur if (distance_to_path < minimum_distance_to_path) { minimum_distance_to_path = distance_to_path; - controller_state_ptr->current_global_plan_index = idx_path; + controller_state_ptr->current_global_plan_index = idx_path - 1; } else { From 1c32dda882424bee334a8c5bb67db50eb96ecfe2 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 16 Feb 2022 14:10:35 +0000 Subject: [PATCH 060/156] Reworked to keep old behavior. --- src/controller.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index f81f4092..83ee6979 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -185,36 +185,38 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi tf2::Transform deltaPlan; // We define segment0 to be the segment connecting pose0 and pose1. // Hence, when picking the starting path's pose, we mean to start at the segment connecting that and the next pose. - for (auto idx_path = global_plan_tf_.size() - 1; idx_path > 0; --idx_path) + for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ - dist_to_segment = distToSegmentSquared(current_tf2, global_plan_tf_[idx_path - 1], global_plan_tf_[idx_path]); + dist_to_segment = distToSegmentSquared(current_tf2, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! if (dist_to_segment < minimum_distance_to_path) { minimum_distance_to_path = dist_to_segment; - controller_state_.current_global_plan_index = idx_path - 1; + controller_state_.current_global_plan_index = idx_path; } /* Create distance and turning radius vectors once for usage later */ - deltaPlan = global_plan_tf_[idx_path - 1].inverseTimes(global_plan_tf_[idx_path]); + deltaPlan = global_plan_tf_[idx_path].inverseTimes(global_plan_tf_[idx_path + 1]); double dpX = deltaPlan.getOrigin().x(); double dpY = deltaPlan.getOrigin().y(); iterative_dist_to_goal += hypot(dpX, dpY); - distance_to_goal_vector_[idx_path - 1] = iterative_dist_to_goal; + distance_to_goal_vector_[idx_path] = iterative_dist_to_goal; // compute turning radius based on trigonometric analysis // radius such that next pose is connected from current pose with a semi-circle double dpXY2 = dpY*dpY + dpX*dpX; if (dpXY2 < FLT_EPSILON) { - turning_radius_inv_vector_[idx_path - 1] = std::numeric_limits::infinity(); + turning_radius_inv_vector_[idx_path] = std::numeric_limits::infinity(); } else { - turning_radius_inv_vector_[idx_path - 1] = 2*dpY/dpXY2; + // 0.5*dpY*( 1 + dpX*dpX/(dpY*dPY) ); + // turning_radius_vector[idx_path] = 0.5*(1/dpY)*( dpY*dpY + dpX*dpX ); + turning_radius_inv_vector_[idx_path] = 2*dpY/dpXY2; } - ROS_DEBUG("turning_radius_inv_vector[%zu] = %f", idx_path - 1, turning_radius_inv_vector_[idx_path - 1]); + ROS_DEBUG("turning_radius_inv_vector[%d] = %f", idx_path, turning_radius_inv_vector_[idx_path]); } // Set initial velocity From 6027dbfb6bf1ea77f4197af6c5e2c10324a3839c Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 10 Feb 2022 08:37:48 +0000 Subject: [PATCH 061/156] Ensure all members are initialized. --- include/path_tracking_pid/controller.hpp | 30 +++++++++---------- .../path_tracking_pid_local_planner.hpp | 6 ++-- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index d8e708e0..d077b819 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -227,13 +227,13 @@ class Controller : private boost::noncopyable void printParameters(); path_tracking_pid::PidConfig local_config_; - ControllerState controller_state_ = ControllerState(); + ControllerState controller_state_; // Global Plan variables std::vector global_plan_tf_; // Global plan vector std::vector distance_to_goal_vector_; // Vector with distances to goal std::vector turning_radius_inv_vector_; // Vector with computed turning radius inverse - double distance_to_goal_; + double distance_to_goal_ = 0; tf2::Transform current_goal_; tf2::Transform current_pos_on_plan_; tf2::Transform current_with_carrot_; @@ -260,13 +260,13 @@ class Controller : private boost::noncopyable // tricycle model bool use_tricycle_model_ = false; geometry_msgs::Transform tf_base_to_steered_wheel_; - double max_steering_angle_; - double max_steering_x_vel_; - double max_steering_x_acc_; - double max_steering_yaw_vel_; - double max_steering_yaw_acc_; - std::array, 2> inverse_kinematics_matrix_; - std::array, 2> forward_kinematics_matrix_; + double max_steering_angle_ = 0; + double max_steering_x_vel_ = 0; + double max_steering_x_acc_ = 0; + double max_steering_yaw_vel_ = 0; + double max_steering_yaw_acc_ = 0; + std::array, 2> inverse_kinematics_matrix_{}; + std::array, 2> forward_kinematics_matrix_{}; bool debug_enabled_ = false; @@ -293,11 +293,11 @@ class Controller : private boost::noncopyable double vel_max_obstacle_ = INFINITY; // Can be zero if lethal obstacles are detected // MPC settings - int mpc_max_fwd_iter_; // Define # of steps that you look into the future with MPC [-] - int mpc_max_vel_optimization_iter_; // Set maximum # of velocity bisection iterations - // (maximum total iterations = max_opt_iter*max_iter) [-] - double mpc_simulation_sample_time_; // Define timestep [s] - double mpc_max_error_lat_; // Maximum allowed lateral error [m] - double mpc_min_x_vel_; // Minimum forward x velocity [m/s] + int mpc_max_fwd_iter_ = 0; // Define # of steps that you look into the future with MPC [-] + int mpc_max_vel_optimization_iter_ = 0; // Set maximum # of velocity bisection iterations + // (maximum total iterations = max_opt_iter*max_iter) [-] + double mpc_simulation_sample_time_ = 0; // Define timestep [s] + double mpc_max_error_lat_ = 0; // Maximum allowed lateral error [m] + double mpc_min_x_vel_ = 0; // Minimum forward x velocity [m/s] }; } // namespace path_tracking_pid diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index d3bf47fe..053ac169 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -129,7 +129,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co nav_msgs::Path received_path_; // Obstacle collision detection - costmap_2d::Costmap2DROS* costmap_; + costmap_2d::Costmap2DROS* costmap_ = nullptr; // Cancel flags (multi threaded, so atomic bools) std::atomic active_goal_{false}; @@ -140,7 +140,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co boost::recursive_mutex config_mutex_; std::unique_ptr> pid_server_; - tf2_ros::Buffer* tf_; + tf2_ros::Buffer* tf_ = nullptr; geometry_msgs::TransformStamped tfCurPoseStamped_; ros::Publisher debug_pub_; // Debugging of controller internal parameters @@ -160,7 +160,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co bool initialized_ = false; // Used for tricycle model - bool use_tricycle_model_; + bool use_tricycle_model_ = false; std::string steered_wheel_frame_; geometry_msgs::TransformStamped tf_base_to_steered_wheel_stamped_; From 4dfa41067d389f4c7b079e29a4a9d1f8ba4e029f Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 17 Feb 2022 13:53:00 +0000 Subject: [PATCH 062/156] Reworked to different defaults. --- include/path_tracking_pid/controller.hpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index d077b819..70b4f01c 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -233,7 +233,7 @@ class Controller : private boost::noncopyable std::vector global_plan_tf_; // Global plan vector std::vector distance_to_goal_vector_; // Vector with distances to goal std::vector turning_radius_inv_vector_; // Vector with computed turning radius inverse - double distance_to_goal_ = 0; + double distance_to_goal_ = NAN; tf2::Transform current_goal_; tf2::Transform current_pos_on_plan_; tf2::Transform current_with_carrot_; @@ -260,11 +260,11 @@ class Controller : private boost::noncopyable // tricycle model bool use_tricycle_model_ = false; geometry_msgs::Transform tf_base_to_steered_wheel_; - double max_steering_angle_ = 0; - double max_steering_x_vel_ = 0; - double max_steering_x_acc_ = 0; - double max_steering_yaw_vel_ = 0; - double max_steering_yaw_acc_ = 0; + double max_steering_angle_ = NAN; + double max_steering_x_vel_ = NAN; + double max_steering_x_acc_ = NAN; + double max_steering_yaw_vel_ = NAN; + double max_steering_yaw_acc_ = NAN; std::array, 2> inverse_kinematics_matrix_{}; std::array, 2> forward_kinematics_matrix_{}; @@ -293,11 +293,11 @@ class Controller : private boost::noncopyable double vel_max_obstacle_ = INFINITY; // Can be zero if lethal obstacles are detected // MPC settings - int mpc_max_fwd_iter_ = 0; // Define # of steps that you look into the future with MPC [-] - int mpc_max_vel_optimization_iter_ = 0; // Set maximum # of velocity bisection iterations - // (maximum total iterations = max_opt_iter*max_iter) [-] - double mpc_simulation_sample_time_ = 0; // Define timestep [s] - double mpc_max_error_lat_ = 0; // Maximum allowed lateral error [m] - double mpc_min_x_vel_ = 0; // Minimum forward x velocity [m/s] + int mpc_max_fwd_iter_ = -1; // Define # of steps that you look into the future with MPC [-] + int mpc_max_vel_optimization_iter_ = -1; // Set maximum # of velocity bisection iterations + // (maximum total iterations = max_opt_iter*max_iter) [-] + double mpc_simulation_sample_time_ = NAN; // Define timestep [s] + double mpc_max_error_lat_ = NAN; // Maximum allowed lateral error [m] + double mpc_min_x_vel_ = NAN; // Minimum forward x velocity [m/s] }; } // namespace path_tracking_pid From 17e5ddd9a62a03dc684275deba26e84201b54674 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Thu, 17 Feb 2022 16:19:34 +0100 Subject: [PATCH 063/156] Fix possible unsigned underflow When the global plan is empty, this comparison could underflow. Fix #48 --- src/controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 83ee6979..9e4e0880 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -135,7 +135,7 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi // For now do not allow repeated points or in-place rotation // To allow that the way the progress is checked and the interpolation is done needs to be changed // Also check if points suddenly go in the opposite direction, this could lead to deadlocks - for (std::vector::size_type pose_idx = 1; pose_idx < global_plan.size() - 1; ++pose_idx) + for (int pose_idx = 1; pose_idx < static_cast(global_plan.size()) - 1; ++pose_idx) { auto prev_pose = global_plan[pose_idx - 1].pose; auto pose = global_plan[pose_idx].pose; @@ -152,7 +152,7 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi } else { - ROS_WARN("Pose %zu of path is not used since it is not in the expected direction of the path!", pose_idx); + ROS_WARN("Pose %i of path is not used since it is not in the expected direction of the path!", pose_idx); } } // Add last pose as we didn't evaluate that one From 3e85956efd537460bef0794c7a40798b25d52951 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 16 Feb 2022 14:48:46 +0000 Subject: [PATCH 064/156] Added clang-tidy and fixed all warnings. --- .clang-tidy | 12 ++++ include/path_tracking_pid/controller.hpp | 36 +++++------ .../path_tracking_pid_local_planner.hpp | 2 +- include/path_tracking_pid/visualization.hpp | 4 +- src/controller.cpp | 59 +++++++++++-------- src/path_tracking_pid_local_planner.cpp | 15 +++-- src/visualization.cpp | 4 +- 7 files changed, 78 insertions(+), 54 deletions(-) create mode 100644 .clang-tidy diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 00000000..def4d06c --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,12 @@ +Checks: 'boost-*, + bugprone-*, + cert-*,-cert-err58-cpp, + clang-*,-clang-analyzer-core.uninitialized.Assign, + concurrency-*, + cppcoreguidelines-*,-cppcoreguidelines-pro-type-vararg,-cppcoreguidelines-pro-bounds-array-to-pointer-decay,-cppcoreguidelines-avoid-magic-numbers, + misc-*, + modernize-*,-modernize-use-trailing-return-type,-modernize-use-nodiscard, + performance-*, + portability-*, + readability-*,-readability-magic-numbers' +FormatStyle: file diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 70b4f01c..7979a0a2 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -98,11 +98,10 @@ class Controller : private boost::noncopyable * @return tf of found position on plan * @return index of current path-pose if requested */ - tf2::Transform findPositionOnPlan(const geometry_msgs::Transform current_tf, - ControllerState* controller_state_ptr, - size_t &path_pose_idx); + tf2::Transform findPositionOnPlan(geometry_msgs::Transform current_tf, ControllerState* controller_state_ptr, + size_t& path_pose_idx); // Overloaded function definition for users that don't require the segment index - tf2::Transform findPositionOnPlan(const geometry_msgs::Transform current_tf, + tf2::Transform findPositionOnPlan(geometry_msgs::Transform current_tf, ControllerState* controller_state_ptr) { size_t path_pose_idx; @@ -120,12 +119,9 @@ class Controller : private boost::noncopyable * @return progress Progress along the path [0,1] * @return pid_debug Variable with information to debug the controller */ - geometry_msgs::Twist update(const double target_x_vel, - const double target_end_x_vel, - const geometry_msgs::Transform current_tf, - const geometry_msgs::Twist odom_twist, - const ros::Duration dt, - double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug); + geometry_msgs::Twist update(double target_x_vel, double target_end_x_vel, geometry_msgs::Transform current_tf, + geometry_msgs::Twist odom_twist, ros::Duration dt, double* eda, double* progress, + path_tracking_pid::PidDebug* pid_debug); /** * Run one iteration of a PID controller with velocity limits applied @@ -137,10 +133,9 @@ class Controller : private boost::noncopyable * @return progress Progress along the path [0,1] * @return pid_debug Variable with information to debug the controller */ - geometry_msgs::Twist update_with_limits(const geometry_msgs::Transform current_tf, - const geometry_msgs::Twist odom_twist, - const ros::Duration dt, - double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug); + geometry_msgs::Twist update_with_limits(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, + ros::Duration dt, double* eda, double* progress, + path_tracking_pid::PidDebug* pid_debug); /** * Perform prediction steps on the lateral error and return a reduced velocity that stays within bounds @@ -148,8 +143,7 @@ class Controller : private boost::noncopyable * @param odom_twist Robot odometry * @return Velocity command */ - double mpc_based_max_vel(const double target_x_vel, geometry_msgs::Transform current_tf, - geometry_msgs::Twist odom_twist); + double mpc_based_max_vel(double target_x_vel, geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist); /** * Set dynamic parameters for the PID controller @@ -204,13 +198,13 @@ class Controller : private boost::noncopyable double getVelMaxObstacle() const; private: - double distSquared(const tf2::Transform& pose_v, const tf2::Transform& pose_w) const; - double distSquared(const geometry_msgs::Pose& pose_v, const geometry_msgs::Pose& pose_w) const; + static double distSquared(const tf2::Transform& pose_v, const tf2::Transform& pose_w); + static double distSquared(const geometry_msgs::Pose& pose_v, const geometry_msgs::Pose& pose_w); void distToSegmentSquared(const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w, - tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w); + tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w) const; // Overloaded function for callers that don't need the additional results - double distToSegmentSquared(const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w) + double distToSegmentSquared(const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w) const { tf2::Transform dummy_tf; double dummy_double; @@ -224,7 +218,7 @@ class Controller : private boost::noncopyable /** * Output some debug information about the current parameters */ - void printParameters(); + void printParameters() const; path_tracking_pid::PidConfig local_config_; ControllerState controller_state_; diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 053ac169..1d42ec78 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -33,7 +33,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co private: typedef boost::geometry::model::ring polygon_t; - inline polygon_t union_(polygon_t polygon1, polygon_t polygon2) + static inline polygon_t union_(const polygon_t& polygon1, const polygon_t& polygon2) { std::vector output_vec; boost::geometry::union_(polygon1, polygon2, output_vec); diff --git a/include/path_tracking_pid/visualization.hpp b/include/path_tracking_pid/visualization.hpp index 1f9233f3..09040e93 100644 --- a/include/path_tracking_pid/visualization.hpp +++ b/include/path_tracking_pid/visualization.hpp @@ -19,10 +19,10 @@ class Visualization private: void publishSphere( - const std_msgs::Header & header, std::string ns, int id, const tf2::Transform & pose, + const std_msgs::Header & header, const std::string& ns, int id, const tf2::Transform & pose, const std_msgs::ColorRGBA & color); void publishSphere( - const std_msgs::Header & header, std::string ns, int id, geometry_msgs::Pose pose, + const std_msgs::Header & header, const std::string& ns, int id, geometry_msgs::Pose pose, const std_msgs::ColorRGBA & color); ros::Publisher marker_pub_; diff --git a/src/controller.cpp b/src/controller.cpp index 83ee6979..e0730c1b 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -258,14 +258,14 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi // https://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment // TODO(Cesar): expand to 3 dimensions -double Controller::distSquared(const tf2::Transform& pose_v, const tf2::Transform& pose_w) const +double Controller::distSquared(const tf2::Transform& pose_v, const tf2::Transform& pose_w) { // Returns square distance between 2 points return (pose_v.getOrigin().x() - pose_w.getOrigin().x()) * (pose_v.getOrigin().x() - pose_w.getOrigin().x()) + (pose_v.getOrigin().y() - pose_w.getOrigin().y()) * (pose_v.getOrigin().y() - pose_w.getOrigin().y()); } -double Controller::distSquared(const geometry_msgs::Pose& pose_v, const geometry_msgs::Pose& pose_w) const +double Controller::distSquared(const geometry_msgs::Pose& pose_v, const geometry_msgs::Pose& pose_w) { // Returns square distance between 2 points return (pose_v.position.x - pose_w.position.x) * (pose_v.position.x - pose_w.position.x) + @@ -274,7 +274,7 @@ double Controller::distSquared(const geometry_msgs::Pose& pose_v, const geometry void Controller::distToSegmentSquared( const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w, - tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w) + tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w) const { double l2 = distSquared(pose_v, pose_w); if (l2 == 0) @@ -312,7 +312,7 @@ void Controller::distToSegmentSquared( } } -tf2::Transform Controller::findPositionOnPlan(const geometry_msgs::Transform current_tf, +tf2::Transform Controller::findPositionOnPlan(geometry_msgs::Transform current_tf, ControllerState* controller_state_ptr, size_t &path_pose_idx) { @@ -430,11 +430,11 @@ tf2::Transform Controller::findPositionOnPlan(const geometry_msgs::Transform cur return current_goal_local; } -geometry_msgs::Twist Controller::update(const double target_x_vel, - const double target_end_x_vel, - const geometry_msgs::Transform current_tf, - const geometry_msgs::Twist odom_twist, - const ros::Duration dt, +geometry_msgs::Twist Controller::update(double target_x_vel, + double target_end_x_vel, + geometry_msgs::Transform current_tf, + geometry_msgs::Twist odom_twist, + ros::Duration dt, double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug) { double current_x_vel = controller_state_.current_x_vel; @@ -480,10 +480,14 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, //***** Feedback control *****// if (!((Kp_lat_ <= 0. && Ki_lat_ <= 0. && Kd_lat_ <= 0.) || (Kp_lat_ >= 0. && Ki_lat_ >= 0. && Kd_lat_ >= 0.))) // All 3 gains should have the same sign + { ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); + } if (!((Kp_ang_ <= 0. && Ki_ang_ <= 0. && Kd_ang_ <= 0.) || (Kp_ang_ >= 0. && Ki_ang_ >= 0. && Kd_ang_ >= 0.))) // All 3 gains should have the same sign + { ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); + } controller_state_.error_lat.at(2) = controller_state_.error_lat.at(1); controller_state_.error_lat.at(1) = controller_state_.error_lat.at(0); @@ -575,7 +579,6 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, /***** Compute forward velocity *****/ // Apply acceleration limits and end velocity - double current_target_acc; double acc; double t_end_phase_current; double d_end_phase; @@ -632,7 +635,7 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, } // Determine if we need to accelerate, decelerate or maintain speed - current_target_acc = 0; // Assume maintaining speed + double current_target_acc = 0; // Assume maintaining speed if (fabs(current_target_x_vel_) <= VELOCITY_EPS) // Zero velocity requested { if (current_x_vel > current_target_x_vel_) @@ -728,10 +731,13 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, control_effort_ang_ = 0.0; if (feedback_lat_enabled_) + { control_effort_lat_ = proportional_lat + integral_lat + derivative_lat; + } if (feedback_ang_enabled_) + { control_effort_ang_ = proportional_ang + integral_ang + derivative_ang; - + } //***** Feedforward control *****// if (feedforward_lat_enabled_) @@ -740,7 +746,9 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, control_effort_lat_ = control_effort_lat_ + feedforward_lat_; } else + { feedforward_lat_ = 0.0; + } if (feedforward_ang_enabled_) { @@ -751,8 +759,9 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, control_effort_ang_ = control_effort_ang_ + feedforward_ang_; } else + { feedforward_ang_ = 0.0; - + } // Apply saturation limits control_effort_lat_ = std::clamp(control_effort_lat_, lat_lower_limit, lat_upper_limit); @@ -836,9 +845,13 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, { steering_cmd.speed = - steering_cmd.speed; if (steering_cmd.steering_angle > 0) + { steering_cmd.steering_angle = steering_cmd.steering_angle - M_PI; + } else + { steering_cmd.steering_angle = steering_cmd.steering_angle + M_PI; + } } // Apply limits to steering commands steering_cmd.steering_angle = std::clamp(steering_cmd.steering_angle, @@ -885,9 +898,9 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, return output_combined; } -geometry_msgs::Twist Controller::update_with_limits(const geometry_msgs::Transform current_tf, - const geometry_msgs::Twist odom_twist, - const ros::Duration dt, +geometry_msgs::Twist Controller::update_with_limits(geometry_msgs::Transform current_tf, + geometry_msgs::Twist odom_twist, + ros::Duration dt, double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug) { @@ -938,7 +951,7 @@ geometry_msgs::Twist Controller::update_with_limits(const geometry_msgs::Transfo // output updated velocity command: (Current position, current measured velocity, closest point index, estimated // duration of arrival, debug info) -double Controller::mpc_based_max_vel(const double target_x_vel, geometry_msgs::Transform current_tf, +double Controller::mpc_based_max_vel(double target_x_vel, geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist) { // Temporary save global data @@ -957,7 +970,6 @@ double Controller::mpc_based_max_vel(const double target_x_vel, geometry_msgs::T geometry_msgs::Twist pred_twist = odom_twist; double new_nominal_x_vel = target_x_vel; // Start off from the current velocity - double mpc_vel_limit = new_nominal_x_vel; // Loop MPC while (mpc_fwd_iter < mpc_max_fwd_iter_ && mpc_vel_optimization_iter <= mpc_max_vel_optimization_iter_) @@ -1019,7 +1031,8 @@ double Controller::mpc_based_max_vel(const double target_x_vel, geometry_msgs::T // Run controller // Output: pred_twist.[linear.x, linear.y, linear.z, angular.x, angular.y, angular.z] path_tracking_pid::PidDebug pid_debug_unused; - double eda_unused, progress_unused; + double eda_unused; + double progress_unused; pred_twist = Controller::update(new_nominal_x_vel, target_end_x_vel_, predicted_tf, pred_twist, ros::Duration(mpc_simulation_sample_time_), &eda_unused, &progress_unused, &pid_debug_unused); @@ -1034,9 +1047,7 @@ double Controller::mpc_based_max_vel(const double target_x_vel, geometry_msgs::T } } // Apply limits to the velocity - mpc_vel_limit = copysign(1.0, target_x_vel) * - fmin(fabs(target_x_vel), fabs(new_nominal_x_vel)); - mpc_vel_limit = copysign(1.0, new_nominal_x_vel) * + double mpc_vel_limit = copysign(1.0, new_nominal_x_vel) * fmax(fabs(new_nominal_x_vel), mpc_min_x_vel_); // Revert global variables @@ -1045,7 +1056,7 @@ double Controller::mpc_based_max_vel(const double target_x_vel, geometry_msgs::T return std::abs(mpc_vel_limit); } -void Controller::printParameters() +void Controller::printParameters() const { ROS_INFO("CONTROLLER PARAMETERS"); ROS_INFO("-----------------------------------------"); @@ -1189,7 +1200,9 @@ void Controller::setVelMaxExternal(double value) return; } if (value < 0.1) + { ROS_WARN_THROTTLE(1.0, "External velocity limit is very small (%f), this could result in standstill", value); + } vel_max_external_ = value; } diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 51d7d437..be134237 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -53,7 +53,7 @@ void TrackingPidLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, ROS_DEBUG("TrackingPidLocalPlanner::initialize(%s, ..., ...)", name.c_str()); // setup dynamic reconfigure pid_server_ = std::make_unique>(config_mutex_, nh); - pid_server_->setCallback([this](auto& config, auto) { this->reconfigure_pid(config); }); + pid_server_->setCallback([this](auto& config, auto /*unused*/) { this->reconfigure_pid(config); }); pid_controller_.setEnabled(false); bool holonomic_robot; @@ -101,14 +101,16 @@ bool TrackingPidLocalPlanner::setPlan(const std::vectorlookupTransform(map_frame_, path_frame, ros::Time(0)); // Check alignment, when path-frame is severly mis-aligned show error - double yaw, pitch, roll; + double yaw; + double pitch; + double roll; tf2::getEulerYPR(tf_transform.transform.rotation, yaw, pitch, roll); if (std::fabs(pitch) > MAP_PARALLEL_THRESH || std::fabs(roll) > MAP_PARALLEL_THRESH) { @@ -202,7 +204,7 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_ cmd_vel.angular.z = pid_controller_.getControllerState().current_yaw_vel; return true; // False is no use: https://github.com/magazino/move_base_flex/issues/195 } - else if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) + if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) { ROS_ERROR("Invalid time increment: %f. Aborting", dt.toSec()); return false; @@ -446,7 +448,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Convert to map coordinates for (const auto& point : collision_polygon_hull) { - int map_x, map_y; + int map_x; + int map_y; costmap2d->worldToMapEnforceBounds(point.x, point.y, map_x, map_y); costmap_2d::MapLocation map_point{static_cast(map_x), static_cast(map_y)}; collision_polygon_hull_map.push_back(map_point); @@ -554,7 +557,9 @@ uint32_t TrackingPidLocalPlanner::computeVelocityCommands(const geometry_msgs::P } if (isGoalReached()) + { active_goal_ = false; + } return mbf_msgs::ExePathResult::SUCCESS; } diff --git a/src/visualization.cpp b/src/visualization.cpp index a380725e..41f61b42 100644 --- a/src/visualization.cpp +++ b/src/visualization.cpp @@ -49,7 +49,7 @@ void Visualization::publishPlanPoint(const std_msgs::Header & header, const tf2: } void Visualization::publishSphere( - const std_msgs::Header & header, std::string ns, int id, const tf2::Transform & pose, + const std_msgs::Header & header, const std::string& ns, int id, const tf2::Transform & pose, const std_msgs::ColorRGBA & color) { geometry_msgs::Pose msg; @@ -58,7 +58,7 @@ void Visualization::publishSphere( } void Visualization::publishSphere( - const std_msgs::Header & header, std::string ns, int id, geometry_msgs::Pose pose, + const std_msgs::Header & header, const std::string& ns, int id, geometry_msgs::Pose pose, const std_msgs::ColorRGBA & color) { visualization_msgs::Marker marker; From c97e6975be210bc4dc35fad619c9e44ba1cffac8 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 17 Feb 2022 14:13:36 +0000 Subject: [PATCH 065/156] Reworked to use const& for big types. --- include/path_tracking_pid/controller.hpp | 22 +++++++++++----------- src/controller.cpp | 24 ++++++++++++------------ 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 7979a0a2..6658e79e 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -69,7 +69,7 @@ class Controller : private boost::noncopyable * @param tricycle_model_enabled If tricycle model should be used * @param estimate_pose_angle The transformation from base to steered wheel */ - void setTricycleModel(bool tricycle_model_enabled, geometry_msgs::Transform tf_base_to_steered_wheel); + void setTricycleModel(bool tricycle_model_enabled, const geometry_msgs::Transform& tf_base_to_steered_wheel); /** * Set plan @@ -77,7 +77,7 @@ class Controller : private boost::noncopyable * @param odom_twist Robot odometry * @param global_plan Plan to follow */ - void setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, + void setPlan(const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist, const std::vector& global_plan); /** @@ -88,8 +88,8 @@ class Controller : private boost::noncopyable * @param steering_odom_twist Steered wheel odometry * @param global_plan Plan to follow */ - void setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, - geometry_msgs::Transform tf_base_to_steered_wheel, geometry_msgs::Twist steering_odom_twist, + void setPlan(const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist, + const geometry_msgs::Transform& tf_base_to_steered_wheel, const geometry_msgs::Twist& steering_odom_twist, const std::vector& global_plan); /** * Find position on plan by looking at the surroundings of last known pose. @@ -98,10 +98,10 @@ class Controller : private boost::noncopyable * @return tf of found position on plan * @return index of current path-pose if requested */ - tf2::Transform findPositionOnPlan(geometry_msgs::Transform current_tf, ControllerState* controller_state_ptr, + tf2::Transform findPositionOnPlan(const geometry_msgs::Transform& current_tf, ControllerState* controller_state_ptr, size_t& path_pose_idx); // Overloaded function definition for users that don't require the segment index - tf2::Transform findPositionOnPlan(geometry_msgs::Transform current_tf, + tf2::Transform findPositionOnPlan(const geometry_msgs::Transform& current_tf, ControllerState* controller_state_ptr) { size_t path_pose_idx; @@ -119,8 +119,8 @@ class Controller : private boost::noncopyable * @return progress Progress along the path [0,1] * @return pid_debug Variable with information to debug the controller */ - geometry_msgs::Twist update(double target_x_vel, double target_end_x_vel, geometry_msgs::Transform current_tf, - geometry_msgs::Twist odom_twist, ros::Duration dt, double* eda, double* progress, + geometry_msgs::Twist update(double target_x_vel, double target_end_x_vel, const geometry_msgs::Transform& current_tf, + const geometry_msgs::Twist& odom_twist, ros::Duration dt, double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug); /** @@ -133,7 +133,7 @@ class Controller : private boost::noncopyable * @return progress Progress along the path [0,1] * @return pid_debug Variable with information to debug the controller */ - geometry_msgs::Twist update_with_limits(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, + geometry_msgs::Twist update_with_limits(const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist, ros::Duration dt, double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug); @@ -143,7 +143,7 @@ class Controller : private boost::noncopyable * @param odom_twist Robot odometry * @return Velocity command */ - double mpc_based_max_vel(double target_x_vel, geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist); + double mpc_based_max_vel(double target_x_vel, const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist); /** * Set dynamic parameters for the PID controller @@ -214,7 +214,7 @@ class Controller : private boost::noncopyable } geometry_msgs::Twist computeTricycleModelForwardKinematics(double x_vel, double steering_angle); - TricycleSteeringCmdVel computeTricycleModelInverseKinematics(geometry_msgs::Twist cmd_vel); + TricycleSteeringCmdVel computeTricycleModelInverseKinematics(const geometry_msgs::Twist& cmd_vel); /** * Output some debug information about the current parameters */ diff --git a/src/controller.cpp b/src/controller.cpp index e0730c1b..3e4c7bce 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -57,7 +57,7 @@ void Controller::setEstimatePoseAngle(bool estimate_pose_angle) estimate_pose_angle_enabled_ = estimate_pose_angle; } -void Controller::setTricycleModel(bool tricycle_model_enabled, geometry_msgs::Transform tf_base_to_steered_wheel) +void Controller::setTricycleModel(bool tricycle_model_enabled, const geometry_msgs::Transform& tf_base_to_steered_wheel) { // Set tricycle model use_tricycle_model_ = tricycle_model_enabled; @@ -104,7 +104,7 @@ geometry_msgs::Twist Controller::computeTricycleModelForwardKinematics(double x_ return estimated_base_twist; } -TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics(geometry_msgs::Twist cmd_vel) +TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics(const geometry_msgs::Twist& cmd_vel) { TricycleSteeringCmdVel steering_cmd_vel; double x_alpha = inverse_kinematics_matrix_[0][0]*cmd_vel.linear.x @@ -118,7 +118,7 @@ TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics(geometr return steering_cmd_vel; } -void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, +void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist, const std::vector& global_plan) { ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%d)", (int)global_plan.size()); @@ -247,8 +247,8 @@ void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twi controller_state_.end_reached = false; } -void Controller::setPlan(geometry_msgs::Transform current_tf, geometry_msgs::Twist odom_twist, - geometry_msgs::Transform tf_base_to_steered_wheel, geometry_msgs::Twist /* steering_odom_twist */, +void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist, + const geometry_msgs::Transform& tf_base_to_steered_wheel, const geometry_msgs::Twist& /* steering_odom_twist */, const std::vector& global_plan) { setPlan(current_tf, odom_twist, global_plan); @@ -312,7 +312,7 @@ void Controller::distToSegmentSquared( } } -tf2::Transform Controller::findPositionOnPlan(geometry_msgs::Transform current_tf, +tf2::Transform Controller::findPositionOnPlan(const geometry_msgs::Transform& current_tf, ControllerState* controller_state_ptr, size_t &path_pose_idx) { @@ -432,8 +432,8 @@ tf2::Transform Controller::findPositionOnPlan(geometry_msgs::Transform current_t geometry_msgs::Twist Controller::update(double target_x_vel, double target_end_x_vel, - geometry_msgs::Transform current_tf, - geometry_msgs::Twist odom_twist, + const geometry_msgs::Transform& current_tf, + const geometry_msgs::Twist& odom_twist, ros::Duration dt, double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug) { @@ -898,8 +898,8 @@ geometry_msgs::Twist Controller::update(double target_x_vel, return output_combined; } -geometry_msgs::Twist Controller::update_with_limits(geometry_msgs::Transform current_tf, - geometry_msgs::Twist odom_twist, +geometry_msgs::Twist Controller::update_with_limits(const geometry_msgs::Transform& current_tf, + const geometry_msgs::Twist& odom_twist, ros::Duration dt, double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug) @@ -951,8 +951,8 @@ geometry_msgs::Twist Controller::update_with_limits(geometry_msgs::Transform cur // output updated velocity command: (Current position, current measured velocity, closest point index, estimated // duration of arrival, debug info) -double Controller::mpc_based_max_vel(double target_x_vel, geometry_msgs::Transform current_tf, - geometry_msgs::Twist odom_twist) +double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::Transform& current_tf, + const geometry_msgs::Twist& odom_twist) { // Temporary save global data ControllerState controller_state_saved; From 3867198b7b4b0f46896f2177eccda1ef6490b49f Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 17 Feb 2022 17:02:22 +0000 Subject: [PATCH 066/156] Reworked to exclude uninitialized variables. --- .clang-tidy | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.clang-tidy b/.clang-tidy index def4d06c..3b3df6d7 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -3,7 +3,7 @@ Checks: 'boost-*, cert-*,-cert-err58-cpp, clang-*,-clang-analyzer-core.uninitialized.Assign, concurrency-*, - cppcoreguidelines-*,-cppcoreguidelines-pro-type-vararg,-cppcoreguidelines-pro-bounds-array-to-pointer-decay,-cppcoreguidelines-avoid-magic-numbers, + cppcoreguidelines-*,-cppcoreguidelines-pro-type-vararg,-cppcoreguidelines-pro-bounds-array-to-pointer-decay,-cppcoreguidelines-avoid-magic-numbers,-cppcoreguidelines-init-variables, misc-*, modernize-*,-modernize-use-trailing-return-type,-modernize-use-nodiscard, performance-*, From a9b2340d96d9d6f96f45b0637d3cbf289371f86b Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 14 Feb 2022 08:48:41 +0000 Subject: [PATCH 067/156] Use std::exchange(). --- src/controller.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 3e4c7bce..5062b0d9 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -990,10 +990,9 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T } // Increase speed - double prev_save = new_nominal_x_vel; - new_nominal_x_vel = copysign(1.0, new_nominal_x_vel) - * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + new_nominal_x_vel; - target_x_vel_prev = prev_save; + target_x_vel_prev = std::exchange( + new_nominal_x_vel, + copysign(1.0, new_nominal_x_vel) * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + new_nominal_x_vel); // Reset variables controller_state_ = controller_state_saved; @@ -1008,10 +1007,9 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T mpc_vel_optimization_iter += 1; // Lower speed - double prev_save = new_nominal_x_vel; - new_nominal_x_vel = -copysign(1.0, new_nominal_x_vel) - * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + new_nominal_x_vel; - target_x_vel_prev = prev_save; + target_x_vel_prev = std::exchange( + new_nominal_x_vel, + -copysign(1.0, new_nominal_x_vel) * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + new_nominal_x_vel); // Reset variables controller_state_ = controller_state_saved; From d9afec6562183fa565876432cdeb37ed269fbd97 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 10 Feb 2022 14:54:48 +0000 Subject: [PATCH 068/156] Added FifoArray abstraction. --- include/path_tracking_pid/controller.hpp | 17 +-- .../path_tracking_pid/details/fifo_array.hpp | 46 +++++++ src/controller.cpp | 121 ++++++++---------- 3 files changed, 107 insertions(+), 77 deletions(-) create mode 100644 include/path_tracking_pid/details/fifo_array.hpp diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 6658e79e..e3a271bd 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -39,14 +40,14 @@ struct ControllerState double tracking_error_lat = 0.0; double tracking_error_ang = 0.0; // Errors with little history - std::array error_lat = {0.0, 0.0, 0.0}; - std::array filtered_error_lat = {0.0, 0.0, 0.0}; - std::array error_deriv_lat = {0.0, 0.0, 0.0}; - std::array filtered_error_deriv_lat = {0.0, 0.0, 0.0}; - std::array error_ang = {0.0, 0.0, 0.0}; - std::array filtered_error_ang = {0.0, 0.0, 0.0}; - std::array error_deriv_ang = {0.0, 0.0, 0.0}; - std::array filtered_error_deriv_ang = {0.0, 0.0, 0.0}; + details::FifoArray error_lat; + details::FifoArray filtered_error_lat; + details::FifoArray error_deriv_lat; + details::FifoArray filtered_error_deriv_lat; + details::FifoArray error_ang; + details::FifoArray filtered_error_ang; + details::FifoArray error_deriv_ang; + details::FifoArray filtered_error_deriv_ang; }; class Controller : private boost::noncopyable diff --git a/include/path_tracking_pid/details/fifo_array.hpp b/include/path_tracking_pid/details/fifo_array.hpp new file mode 100644 index 00000000..178996e8 --- /dev/null +++ b/include/path_tracking_pid/details/fifo_array.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include +#include + +namespace path_tracking_pid::details +{ + +// Fixed-size array-like FIFO buffer. All elements are value initialized upon construction. +template +class FifoArray +{ +public: + // Pushes the given value to the front of the buffer (index = 0). All elements already in the buffer are moved + // towards the end of the buffer (index = size - 1). The last element is removed from the buffer. + constexpr void push(const value_type& value) + { + std::copy_backward(data_.begin(), std::prev(data_.end()), data_.end()); + data_[0] = value; + } + + // Value initializes all elements in the buffer. + constexpr void reset() + { + data_ = {}; + } + + // Read-only access to the element at the given index. + constexpr const value_type& operator[](std::size_t index) const + { + return data_[index]; + } + + // Read-only access to the element at the given index (with compile-time range check). + template + constexpr const value_type& at() const + { + static_assert(index < size); + return data_[index]; + } + +private: + std::array data_{}; +}; + +} // namespace path_tracking_pid::details diff --git a/src/controller.cpp b/src/controller.cpp index 5062b0d9..f40f0452 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -489,13 +489,8 @@ geometry_msgs::Twist Controller::update(double target_x_vel, ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); } - controller_state_.error_lat.at(2) = controller_state_.error_lat.at(1); - controller_state_.error_lat.at(1) = controller_state_.error_lat.at(0); - controller_state_.error_lat.at(0) = error.getOrigin().y(); // Current error goes to slot 0 - controller_state_.error_ang.at(2) = controller_state_.error_ang.at(1); - controller_state_.error_ang.at(1) = controller_state_.error_ang.at(0); - controller_state_.error_ang.at(0) = - angles::normalize_angle(tf2::getYaw(error.getRotation())); // Current error goes to slot 0 + controller_state_.error_lat.push(error.getOrigin().y()); + controller_state_.error_ang.push(angles::normalize_angle(tf2::getYaw(error.getRotation()))); // tracking error for diagnostic purposes // Transform current pose into local-path-frame to get tracked-frame-error @@ -514,67 +509,55 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // trackin_error here represents the error between tracked link and position on plan controller_state_.tracking_error_lat = current_tracking_err.y(); - controller_state_.tracking_error_ang = controller_state_.error_ang.at(0); + controller_state_.tracking_error_ang = controller_state_.error_ang.at<0>(); // integrate the error - controller_state_.error_integral_lat += controller_state_.error_lat.at(0) * dt.toSec(); - controller_state_.error_integral_ang += controller_state_.error_ang.at(0) * dt.toSec(); + controller_state_.error_integral_lat += controller_state_.error_lat.at<0>() * dt.toSec(); + controller_state_.error_integral_ang += controller_state_.error_ang.at<0>() * dt.toSec(); // Apply windup limit to limit the size of the integral term controller_state_.error_integral_lat = std::clamp(controller_state_.error_integral_lat, -windup_limit, windup_limit); controller_state_.error_integral_ang = std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit); - controller_state_.filtered_error_lat.at(2) = controller_state_.filtered_error_lat.at(1); - controller_state_.filtered_error_lat.at(1) = controller_state_.filtered_error_lat.at(0); - controller_state_.filtered_error_lat.at(0) = + controller_state_.filtered_error_lat.push( (1 / (1 + c_lat * c_lat + M_SQRT2 * c_lat)) * - (controller_state_.error_lat.at(2) + 2 * controller_state_.error_lat.at(1) + controller_state_.error_lat.at(0) - - (c_lat * c_lat - M_SQRT2 * c_lat + 1) * controller_state_.filtered_error_lat.at(2) - - (-2 * c_lat * c_lat + 2) * controller_state_.filtered_error_lat.at(1)); + (controller_state_.error_lat.at<2>() + 2 * controller_state_.error_lat.at<1>() + controller_state_.error_lat.at<0>() - + (c_lat * c_lat - M_SQRT2 * c_lat + 1) * controller_state_.filtered_error_lat.at<1>() - + (-2 * c_lat * c_lat + 2) * controller_state_.filtered_error_lat.at<0>())); - controller_state_.filtered_error_ang.at(2) = controller_state_.filtered_error_ang.at(1); - controller_state_.filtered_error_ang.at(1) = controller_state_.filtered_error_ang.at(0); - controller_state_.filtered_error_ang.at(0) = + controller_state_.filtered_error_ang.push( (1 / (1 + c_ang * c_ang + M_SQRT2 * c_ang)) * - (controller_state_.error_ang.at(2) + 2 * controller_state_.error_ang.at(1) + controller_state_.error_ang.at(0) - - (c_ang * c_ang - M_SQRT2 * c_ang + 1) * controller_state_.filtered_error_ang.at(2) - - (-2 * c_ang * c_ang + 2) * controller_state_.filtered_error_ang.at(1)); + (controller_state_.error_ang.at<2>() + 2 * controller_state_.error_ang.at<1>() + controller_state_.error_ang.at<0>() - + (c_ang * c_ang - M_SQRT2 * c_ang + 1) * controller_state_.filtered_error_ang.at<1>() - + (-2 * c_ang * c_ang + 2) * controller_state_.filtered_error_ang.at<0>())); // Take derivative of error, first the raw unfiltered data: - controller_state_.error_deriv_lat.at(2) = controller_state_.error_deriv_lat.at(1); - controller_state_.error_deriv_lat.at(1) = controller_state_.error_deriv_lat.at(0); - controller_state_.error_deriv_lat.at(0) = - (controller_state_.error_lat.at(0) - controller_state_.error_lat.at(1)) / dt.toSec(); - controller_state_.filtered_error_deriv_lat.at(2) = controller_state_.filtered_error_deriv_lat.at(1); - controller_state_.filtered_error_deriv_lat.at(1) = controller_state_.filtered_error_deriv_lat.at(0); - controller_state_.filtered_error_deriv_lat.at(0) = + controller_state_.error_deriv_lat.push( + (controller_state_.error_lat.at<0>() - controller_state_.error_lat.at<1>()) / dt.toSec()); + controller_state_.filtered_error_deriv_lat.push( (1 / (1 + c_lat * c_lat + M_SQRT2 * c_lat)) * - (controller_state_.error_deriv_lat.at(2) + 2 * controller_state_.error_deriv_lat.at(1) + - controller_state_.error_deriv_lat.at(0) - - (c_lat * c_lat - M_SQRT2 * c_lat + 1) * controller_state_.filtered_error_deriv_lat.at(2) - - (-2 * c_lat * c_lat + 2) * controller_state_.filtered_error_deriv_lat.at(1)); - - controller_state_.error_deriv_ang.at(2) = controller_state_.error_deriv_ang.at(1); - controller_state_.error_deriv_ang.at(1) = controller_state_.error_deriv_ang.at(0); - controller_state_.error_deriv_ang.at(0) = - (controller_state_.error_ang.at(0) - controller_state_.error_ang.at(1)) / dt.toSec(); - controller_state_.filtered_error_deriv_ang.at(2) = controller_state_.filtered_error_deriv_ang.at(1); - controller_state_.filtered_error_deriv_ang.at(1) = controller_state_.filtered_error_deriv_ang.at(0); - controller_state_.filtered_error_deriv_ang.at(0) = + (controller_state_.error_deriv_lat.at<2>() + 2 * controller_state_.error_deriv_lat.at<1>() + + controller_state_.error_deriv_lat.at<0>() - + (c_lat * c_lat - M_SQRT2 * c_lat + 1) * controller_state_.filtered_error_deriv_lat.at<1>() - + (-2 * c_lat * c_lat + 2) * controller_state_.filtered_error_deriv_lat.at<0>())); + + controller_state_.error_deriv_ang.push( + (controller_state_.error_ang.at<0>() - controller_state_.error_ang.at<1>()) / dt.toSec()); + controller_state_.filtered_error_deriv_ang.push( (1 / (1 + c_ang * c_ang + M_SQRT2 * c_ang)) * - (controller_state_.error_deriv_ang.at(2) + 2 * controller_state_.error_deriv_ang.at(1) + - controller_state_.error_deriv_ang.at(0) - - (c_ang * c_ang - M_SQRT2 * c_ang + 1) * controller_state_.filtered_error_deriv_ang.at(2) - - (-2 * c_ang * c_ang + 2) * controller_state_.filtered_error_deriv_ang.at(1)); + (controller_state_.error_deriv_ang.at<2>() + 2 * controller_state_.error_deriv_ang.at<1>() + + controller_state_.error_deriv_ang.at<0>() - + (c_ang * c_ang - M_SQRT2 * c_ang + 1) * controller_state_.filtered_error_deriv_ang.at<1>() - + (-2 * c_ang * c_ang + 2) * controller_state_.filtered_error_deriv_ang.at<0>())); // calculate the control effort - const auto proportional_lat = Kp_lat_ * controller_state_.filtered_error_lat.at(0); + const auto proportional_lat = Kp_lat_ * controller_state_.filtered_error_lat.at<0>(); const auto integral_lat = Ki_lat_ * controller_state_.error_integral_lat; - const auto derivative_lat = Kd_lat_ * controller_state_.filtered_error_deriv_lat.at(0); + const auto derivative_lat = Kd_lat_ * controller_state_.filtered_error_deriv_lat.at<0>(); - const auto proportional_ang = Kp_ang_ * controller_state_.filtered_error_ang.at(0); + const auto proportional_ang = Kp_ang_ * controller_state_.filtered_error_ang.at<0>(); const auto integral_ang = Ki_ang_ * controller_state_.error_integral_ang; - const auto derivative_ang = Kd_ang_ * controller_state_.filtered_error_deriv_ang.at(0); + const auto derivative_ang = Kd_ang_ * controller_state_.filtered_error_deriv_ang.at<0>(); /***** Compute forward velocity *****/ @@ -770,8 +753,8 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // Populate debug output // Error topic containing the 'control' error on which the PID acts pid_debug->control_error.linear.x = 0.0; - pid_debug->control_error.linear.y = controller_state_.error_lat.at(0); - pid_debug->control_error.angular.z = controller_state_.error_ang.at(0); + pid_debug->control_error.linear.y = controller_state_.error_lat.at<0>(); + pid_debug->control_error.angular.z = controller_state_.error_ang.at<0>(); // Error topic containing the 'tracking' error, i.e. the real error between path and tracked link pid_debug->tracking_error.linear.x = 0.0; pid_debug->tracking_error.linear.y = controller_state_.tracking_error_lat; @@ -978,7 +961,7 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T // Check if robot stays within bounds for all iterations, if the new_nominal_x_vel is smaller than // max_target_x_vel we can increase it - if (mpc_fwd_iter == mpc_max_fwd_iter_ && fabs(controller_state_.error_lat.at(0)) <= mpc_max_error_lat_ && + if (mpc_fwd_iter == mpc_max_fwd_iter_ && fabs(controller_state_.error_lat.at<0>()) <= mpc_max_error_lat_ && fabs(new_nominal_x_vel) < abs(target_x_vel)) { mpc_vel_optimization_iter += 1; @@ -1002,7 +985,7 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T mpc_fwd_iter = 0; } // If the robot gets out of bounds earlier we decrease the velocity - else if (abs(controller_state_.error_lat.at(0)) >= mpc_max_error_lat_) + else if (abs(controller_state_.error_lat.at<0>()) >= mpc_max_error_lat_) { mpc_vel_optimization_iter += 1; @@ -1083,15 +1066,15 @@ void Controller::configure(path_tracking_pid::PidConfig& config) { // Erase all queues when config changes - std::fill(controller_state_.error_lat.begin(), controller_state_.error_lat.end(), 0); - std::fill(controller_state_.filtered_error_lat.begin(), controller_state_.filtered_error_lat.end(), 0); - std::fill(controller_state_.error_deriv_lat.begin(), controller_state_.error_deriv_lat.end(), 0); - std::fill(controller_state_.filtered_error_deriv_lat.begin(), controller_state_.filtered_error_deriv_lat.end(), 0); + controller_state_.error_lat.reset(); + controller_state_.filtered_error_lat.reset(); + controller_state_.error_deriv_lat.reset(); + controller_state_.filtered_error_deriv_lat.reset(); - std::fill(controller_state_.error_ang.begin(), controller_state_.error_ang.end(), 0); - std::fill(controller_state_.filtered_error_ang.begin(), controller_state_.filtered_error_ang.end(), 0); - std::fill(controller_state_.error_deriv_ang.begin(), controller_state_.error_deriv_ang.end(), 0); - std::fill(controller_state_.filtered_error_deriv_ang.begin(), controller_state_.filtered_error_deriv_ang.end(), 0); + controller_state_.error_ang.reset(); + controller_state_.filtered_error_ang.reset(); + controller_state_.error_deriv_ang.reset(); + controller_state_.filtered_error_deriv_ang.reset(); Kp_lat_ = config.Kp_lat; Ki_lat_ = config.Ki_lat; @@ -1180,14 +1163,14 @@ void Controller::reset() controller_state_.previous_steering_x_vel = 0.0; controller_state_.error_integral_lat = 0.0; controller_state_.error_integral_ang = 0.0; - controller_state_.error_lat = {0.0, 0.0, 0.0}; - controller_state_.filtered_error_lat = {0.0, 0.0, 0.0}; - controller_state_.error_deriv_lat = {0.0, 0.0, 0.0}; - controller_state_.filtered_error_deriv_lat = {0.0, 0.0, 0.0}; - controller_state_.error_ang = {0.0, 0.0, 0.0}; - controller_state_.filtered_error_ang = {0.0, 0.0, 0.0}; - controller_state_.error_deriv_ang = {0.0, 0.0, 0.0}; - controller_state_.filtered_error_deriv_ang = {0.0, 0.0, 0.0}; + controller_state_.error_lat.reset(); + controller_state_.filtered_error_lat.reset(); + controller_state_.error_deriv_lat.reset(); + controller_state_.filtered_error_deriv_lat.reset(); + controller_state_.error_ang.reset(); + controller_state_.filtered_error_ang.reset(); + controller_state_.error_deriv_ang.reset(); + controller_state_.filtered_error_deriv_ang.reset(); } void Controller::setVelMaxExternal(double value) From 5c3178f755f14fba0394357873015a8cbf3f3690 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 10 Feb 2022 15:57:36 +0000 Subject: [PATCH 069/156] Added filtered error tracker abstraction. --- CMakeLists.txt | 1 + include/path_tracking_pid/controller.hpp | 13 ++-- .../details/filtered_error_tracker.hpp | 30 +++++++++ src/controller.cpp | 64 ++++--------------- src/details/filtered_error_tracker.cpp | 41 ++++++++++++ 5 files changed, 90 insertions(+), 59 deletions(-) create mode 100644 include/path_tracking_pid/details/filtered_error_tracker.hpp create mode 100644 src/details/filtered_error_tracker.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0b915db1..9b9a5cde 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,6 +65,7 @@ add_library(${PROJECT_NAME} src/${PROJECT_NAME}_local_planner.cpp src/controller.cpp src/visualization.cpp + src/details/filtered_error_tracker.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index e3a271bd..d1aacea0 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -40,14 +41,10 @@ struct ControllerState double tracking_error_lat = 0.0; double tracking_error_ang = 0.0; // Errors with little history - details::FifoArray error_lat; - details::FifoArray filtered_error_lat; - details::FifoArray error_deriv_lat; - details::FifoArray filtered_error_deriv_lat; - details::FifoArray error_ang; - details::FifoArray filtered_error_ang; - details::FifoArray error_deriv_ang; - details::FifoArray filtered_error_deriv_ang; + details::FilteredErrorTracker error_lat; + details::FilteredErrorTracker error_deriv_lat; + details::FilteredErrorTracker error_ang; + details::FilteredErrorTracker error_deriv_ang; }; class Controller : private boost::noncopyable diff --git a/include/path_tracking_pid/details/filtered_error_tracker.hpp b/include/path_tracking_pid/details/filtered_error_tracker.hpp new file mode 100644 index 00000000..e408558e --- /dev/null +++ b/include/path_tracking_pid/details/filtered_error_tracker.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include + +namespace path_tracking_pid::details +{ + +// Error tracker for the last 3 error and filtered error values. +class FilteredErrorTracker +{ +public: + // Pushes the given value to the errors FIFO buffer. A corresponding filtered error value is calculated and pushed + // to the filtered errors FIFO buffer. + void push(double value); + + // Resets both errors and filtered errors FIFO buffers. + void reset(); + + // Read-only access to the errors FIFO buffer. + const FifoArray& errors() const; + + // Read-only access to the filtered errors FIFO buffer. + const FifoArray& filtered_errors() const; + +private: + FifoArray errors_; + FifoArray filtered_errors_; +}; + +} // namespace path_tracking_pid::details diff --git a/src/controller.cpp b/src/controller.cpp index f40f0452..2a8691b9 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -30,11 +30,6 @@ constexpr double ang_lower_limit = -100.0; // Anti-windup term. Limits the absolute value of the integral term. constexpr double windup_limit = 1000.0; -// Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at -// 1/4 of the sample rate. -constexpr double c_lat = 1.; -constexpr double c_ang = 1.; - // Typesafe sign implementation with signum: // https://stackoverflow.com/a/4609795 @@ -509,55 +504,30 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // trackin_error here represents the error between tracked link and position on plan controller_state_.tracking_error_lat = current_tracking_err.y(); - controller_state_.tracking_error_ang = controller_state_.error_ang.at<0>(); + controller_state_.tracking_error_ang = controller_state_.error_ang.errors().at<0>(); // integrate the error - controller_state_.error_integral_lat += controller_state_.error_lat.at<0>() * dt.toSec(); - controller_state_.error_integral_ang += controller_state_.error_ang.at<0>() * dt.toSec(); + controller_state_.error_integral_lat += controller_state_.error_lat.errors().at<0>() * dt.toSec(); + controller_state_.error_integral_ang += controller_state_.error_ang.errors().at<0>() * dt.toSec(); // Apply windup limit to limit the size of the integral term controller_state_.error_integral_lat = std::clamp(controller_state_.error_integral_lat, -windup_limit, windup_limit); controller_state_.error_integral_ang = std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit); - controller_state_.filtered_error_lat.push( - (1 / (1 + c_lat * c_lat + M_SQRT2 * c_lat)) * - (controller_state_.error_lat.at<2>() + 2 * controller_state_.error_lat.at<1>() + controller_state_.error_lat.at<0>() - - (c_lat * c_lat - M_SQRT2 * c_lat + 1) * controller_state_.filtered_error_lat.at<1>() - - (-2 * c_lat * c_lat + 2) * controller_state_.filtered_error_lat.at<0>())); - - controller_state_.filtered_error_ang.push( - (1 / (1 + c_ang * c_ang + M_SQRT2 * c_ang)) * - (controller_state_.error_ang.at<2>() + 2 * controller_state_.error_ang.at<1>() + controller_state_.error_ang.at<0>() - - (c_ang * c_ang - M_SQRT2 * c_ang + 1) * controller_state_.filtered_error_ang.at<1>() - - (-2 * c_ang * c_ang + 2) * controller_state_.filtered_error_ang.at<0>())); - // Take derivative of error, first the raw unfiltered data: controller_state_.error_deriv_lat.push( - (controller_state_.error_lat.at<0>() - controller_state_.error_lat.at<1>()) / dt.toSec()); - controller_state_.filtered_error_deriv_lat.push( - (1 / (1 + c_lat * c_lat + M_SQRT2 * c_lat)) * - (controller_state_.error_deriv_lat.at<2>() + 2 * controller_state_.error_deriv_lat.at<1>() + - controller_state_.error_deriv_lat.at<0>() - - (c_lat * c_lat - M_SQRT2 * c_lat + 1) * controller_state_.filtered_error_deriv_lat.at<1>() - - (-2 * c_lat * c_lat + 2) * controller_state_.filtered_error_deriv_lat.at<0>())); - + (controller_state_.error_lat.errors().at<0>() - controller_state_.error_lat.errors().at<1>()) / dt.toSec()); controller_state_.error_deriv_ang.push( - (controller_state_.error_ang.at<0>() - controller_state_.error_ang.at<1>()) / dt.toSec()); - controller_state_.filtered_error_deriv_ang.push( - (1 / (1 + c_ang * c_ang + M_SQRT2 * c_ang)) * - (controller_state_.error_deriv_ang.at<2>() + 2 * controller_state_.error_deriv_ang.at<1>() + - controller_state_.error_deriv_ang.at<0>() - - (c_ang * c_ang - M_SQRT2 * c_ang + 1) * controller_state_.filtered_error_deriv_ang.at<1>() - - (-2 * c_ang * c_ang + 2) * controller_state_.filtered_error_deriv_ang.at<0>())); + (controller_state_.error_ang.errors().at<0>() - controller_state_.error_ang.errors().at<1>()) / dt.toSec()); // calculate the control effort - const auto proportional_lat = Kp_lat_ * controller_state_.filtered_error_lat.at<0>(); + const auto proportional_lat = Kp_lat_ * controller_state_.error_lat.filtered_errors().at<0>(); const auto integral_lat = Ki_lat_ * controller_state_.error_integral_lat; - const auto derivative_lat = Kd_lat_ * controller_state_.filtered_error_deriv_lat.at<0>(); + const auto derivative_lat = Kd_lat_ * controller_state_.error_deriv_lat.filtered_errors().at<0>(); - const auto proportional_ang = Kp_ang_ * controller_state_.filtered_error_ang.at<0>(); + const auto proportional_ang = Kp_ang_ * controller_state_.error_ang.filtered_errors().at<0>(); const auto integral_ang = Ki_ang_ * controller_state_.error_integral_ang; - const auto derivative_ang = Kd_ang_ * controller_state_.filtered_error_deriv_ang.at<0>(); + const auto derivative_ang = Kd_ang_ * controller_state_.error_deriv_ang.filtered_errors().at<0>(); /***** Compute forward velocity *****/ @@ -753,8 +723,8 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // Populate debug output // Error topic containing the 'control' error on which the PID acts pid_debug->control_error.linear.x = 0.0; - pid_debug->control_error.linear.y = controller_state_.error_lat.at<0>(); - pid_debug->control_error.angular.z = controller_state_.error_ang.at<0>(); + pid_debug->control_error.linear.y = controller_state_.error_lat.errors().at<0>(); + pid_debug->control_error.angular.z = controller_state_.error_ang.errors().at<0>(); // Error topic containing the 'tracking' error, i.e. the real error between path and tracked link pid_debug->tracking_error.linear.x = 0.0; pid_debug->tracking_error.linear.y = controller_state_.tracking_error_lat; @@ -961,7 +931,7 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T // Check if robot stays within bounds for all iterations, if the new_nominal_x_vel is smaller than // max_target_x_vel we can increase it - if (mpc_fwd_iter == mpc_max_fwd_iter_ && fabs(controller_state_.error_lat.at<0>()) <= mpc_max_error_lat_ && + if (mpc_fwd_iter == mpc_max_fwd_iter_ && fabs(controller_state_.error_lat.errors().at<0>()) <= mpc_max_error_lat_ && fabs(new_nominal_x_vel) < abs(target_x_vel)) { mpc_vel_optimization_iter += 1; @@ -985,7 +955,7 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T mpc_fwd_iter = 0; } // If the robot gets out of bounds earlier we decrease the velocity - else if (abs(controller_state_.error_lat.at<0>()) >= mpc_max_error_lat_) + else if (abs(controller_state_.error_lat.errors().at<0>()) >= mpc_max_error_lat_) { mpc_vel_optimization_iter += 1; @@ -1067,14 +1037,10 @@ void Controller::configure(path_tracking_pid::PidConfig& config) // Erase all queues when config changes controller_state_.error_lat.reset(); - controller_state_.filtered_error_lat.reset(); controller_state_.error_deriv_lat.reset(); - controller_state_.filtered_error_deriv_lat.reset(); controller_state_.error_ang.reset(); - controller_state_.filtered_error_ang.reset(); controller_state_.error_deriv_ang.reset(); - controller_state_.filtered_error_deriv_ang.reset(); Kp_lat_ = config.Kp_lat; Ki_lat_ = config.Ki_lat; @@ -1164,13 +1130,9 @@ void Controller::reset() controller_state_.error_integral_lat = 0.0; controller_state_.error_integral_ang = 0.0; controller_state_.error_lat.reset(); - controller_state_.filtered_error_lat.reset(); controller_state_.error_deriv_lat.reset(); - controller_state_.filtered_error_deriv_lat.reset(); controller_state_.error_ang.reset(); - controller_state_.filtered_error_ang.reset(); controller_state_.error_deriv_ang.reset(); - controller_state_.filtered_error_deriv_ang.reset(); } void Controller::setVelMaxExternal(double value) diff --git a/src/details/filtered_error_tracker.cpp b/src/details/filtered_error_tracker.cpp new file mode 100644 index 00000000..ffab9f9a --- /dev/null +++ b/src/details/filtered_error_tracker.cpp @@ -0,0 +1,41 @@ +#include + +#include + +namespace path_tracking_pid::details +{ + +namespace +{ + +// Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at 1/4 of the sample rate. +constexpr double cutoff = 1.; + +} // namespace + +void FilteredErrorTracker::push(double value) +{ + errors_.push(value); + + filtered_errors_.push((1 / (1 + cutoff * cutoff + M_SQRT2 * cutoff)) * + (errors_.at<2>() + 2 * errors_.at<1>() + errors_.at<0>() - + (cutoff * cutoff - M_SQRT2 * cutoff + 1) * filtered_errors_.at<1>() - + (-2 * cutoff * cutoff + 2) * filtered_errors_.at<0>())); +} + +void FilteredErrorTracker::reset() +{ + errors_.reset(); + filtered_errors_.reset(); +} + +const FifoArray& FilteredErrorTracker::errors() const +{ + return errors_; +} +const FifoArray& FilteredErrorTracker::filtered_errors() const +{ + return filtered_errors_; +} + +} // namespace path_tracking_pid::details From 1f7a6746ea712b61b8cd285447032f4acb4e06e4 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 17 Feb 2022 17:17:22 +0000 Subject: [PATCH 070/156] Reworked by renaming to SecondOrderLowpass. --- CMakeLists.txt | 2 +- include/path_tracking_pid/controller.hpp | 10 +++++----- ...ered_error_tracker.hpp => second_order_lowpass.hpp} | 2 +- ...ered_error_tracker.cpp => second_order_lowpass.cpp} | 10 +++++----- 4 files changed, 12 insertions(+), 12 deletions(-) rename include/path_tracking_pid/details/{filtered_error_tracker.hpp => second_order_lowpass.hpp} (96%) rename src/details/{filtered_error_tracker.cpp => second_order_lowpass.cpp} (72%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9b9a5cde..da83a8f0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,7 +65,7 @@ add_library(${PROJECT_NAME} src/${PROJECT_NAME}_local_planner.cpp src/controller.cpp src/visualization.cpp - src/details/filtered_error_tracker.cpp + src/details/second_order_lowpass.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index d1aacea0..ddf162d7 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include #include @@ -41,10 +41,10 @@ struct ControllerState double tracking_error_lat = 0.0; double tracking_error_ang = 0.0; // Errors with little history - details::FilteredErrorTracker error_lat; - details::FilteredErrorTracker error_deriv_lat; - details::FilteredErrorTracker error_ang; - details::FilteredErrorTracker error_deriv_ang; + details::SecondOrderLowpass error_lat; + details::SecondOrderLowpass error_deriv_lat; + details::SecondOrderLowpass error_ang; + details::SecondOrderLowpass error_deriv_ang; }; class Controller : private boost::noncopyable diff --git a/include/path_tracking_pid/details/filtered_error_tracker.hpp b/include/path_tracking_pid/details/second_order_lowpass.hpp similarity index 96% rename from include/path_tracking_pid/details/filtered_error_tracker.hpp rename to include/path_tracking_pid/details/second_order_lowpass.hpp index e408558e..692ea60f 100644 --- a/include/path_tracking_pid/details/filtered_error_tracker.hpp +++ b/include/path_tracking_pid/details/second_order_lowpass.hpp @@ -6,7 +6,7 @@ namespace path_tracking_pid::details { // Error tracker for the last 3 error and filtered error values. -class FilteredErrorTracker +class SecondOrderLowpass { public: // Pushes the given value to the errors FIFO buffer. A corresponding filtered error value is calculated and pushed diff --git a/src/details/filtered_error_tracker.cpp b/src/details/second_order_lowpass.cpp similarity index 72% rename from src/details/filtered_error_tracker.cpp rename to src/details/second_order_lowpass.cpp index ffab9f9a..7806c5b7 100644 --- a/src/details/filtered_error_tracker.cpp +++ b/src/details/second_order_lowpass.cpp @@ -1,4 +1,4 @@ -#include +#include #include @@ -13,7 +13,7 @@ constexpr double cutoff = 1.; } // namespace -void FilteredErrorTracker::push(double value) +void SecondOrderLowpass::push(double value) { errors_.push(value); @@ -23,17 +23,17 @@ void FilteredErrorTracker::push(double value) (-2 * cutoff * cutoff + 2) * filtered_errors_.at<0>())); } -void FilteredErrorTracker::reset() +void SecondOrderLowpass::reset() { errors_.reset(); filtered_errors_.reset(); } -const FifoArray& FilteredErrorTracker::errors() const +const FifoArray& SecondOrderLowpass::errors() const { return errors_; } -const FifoArray& FilteredErrorTracker::filtered_errors() const +const FifoArray& SecondOrderLowpass::filtered_errors() const { return filtered_errors_; } From ee40fe01a82f9521e9e3f573003d0da98282f546 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Fri, 11 Feb 2022 14:26:05 +0000 Subject: [PATCH 071/156] Const locals when possible. --- src/controller.cpp | 71 +++++++++++++++++++++++----------------------- 1 file changed, 35 insertions(+), 36 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 2a8691b9..6f3a6a1b 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -57,19 +57,19 @@ void Controller::setTricycleModel(bool tricycle_model_enabled, const geometry_ms // Set tricycle model use_tricycle_model_ = tricycle_model_enabled; tf_base_to_steered_wheel_ = tf_base_to_steered_wheel; - double wheel_x = tf_base_to_steered_wheel_.translation.x; - double wheel_y = tf_base_to_steered_wheel_.translation.y; + const double wheel_x = tf_base_to_steered_wheel_.translation.x; + const double wheel_y = tf_base_to_steered_wheel_.translation.y; - double distance_base_to_steered_wheel = hypot(wheel_x, wheel_y); - double wheel_theta = atan2(wheel_y, wheel_x); + const double distance_base_to_steered_wheel = hypot(wheel_x, wheel_y); + const double wheel_theta = atan2(wheel_y, wheel_x); inverse_kinematics_matrix_[0][0] = 1; inverse_kinematics_matrix_[0][1] = - distance_base_to_steered_wheel * sin(wheel_theta); inverse_kinematics_matrix_[1][0] = 0; inverse_kinematics_matrix_[1][1] = - distance_base_to_steered_wheel * cos(wheel_theta); - double determinant = inverse_kinematics_matrix_[0][0] * inverse_kinematics_matrix_[1][1] - - inverse_kinematics_matrix_[0][1] * inverse_kinematics_matrix_[1][0]; + const double determinant = inverse_kinematics_matrix_[0][0] * inverse_kinematics_matrix_[1][1] + - inverse_kinematics_matrix_[0][1] * inverse_kinematics_matrix_[1][0]; if (determinant == 0) { @@ -89,8 +89,8 @@ void Controller::setTricycleModel(bool tricycle_model_enabled, const geometry_ms geometry_msgs::Twist Controller::computeTricycleModelForwardKinematics(double x_vel, double steering_angle) { geometry_msgs::Twist estimated_base_twist; - double x_alpha = x_vel*cos(steering_angle); - double y_alpha = x_vel*sin(steering_angle); + const double x_alpha = x_vel*cos(steering_angle); + const double y_alpha = x_vel*sin(steering_angle); estimated_base_twist.linear.x = forward_kinematics_matrix_[0][0]*x_alpha + forward_kinematics_matrix_[0][1]*y_alpha; estimated_base_twist.angular.z = @@ -102,10 +102,10 @@ geometry_msgs::Twist Controller::computeTricycleModelForwardKinematics(double x_ TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics(const geometry_msgs::Twist& cmd_vel) { TricycleSteeringCmdVel steering_cmd_vel; - double x_alpha = inverse_kinematics_matrix_[0][0]*cmd_vel.linear.x - + inverse_kinematics_matrix_[0][1]*cmd_vel.angular.z; - double y_alpha = inverse_kinematics_matrix_[1][0]*cmd_vel.linear.x - + inverse_kinematics_matrix_[1][1]*cmd_vel.angular.z; + const double x_alpha = inverse_kinematics_matrix_[0][0]*cmd_vel.linear.x + + inverse_kinematics_matrix_[0][1]*cmd_vel.angular.z; + const double y_alpha = inverse_kinematics_matrix_[1][0]*cmd_vel.linear.x + + inverse_kinematics_matrix_[1][1]*cmd_vel.angular.z; steering_cmd_vel.steering_angle = atan2(y_alpha, x_alpha); steering_cmd_vel.speed = hypot(x_alpha, y_alpha); @@ -132,14 +132,14 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome // Also check if points suddenly go in the opposite direction, this could lead to deadlocks for (std::vector::size_type pose_idx = 1; pose_idx < global_plan.size() - 1; ++pose_idx) { - auto prev_pose = global_plan[pose_idx - 1].pose; - auto pose = global_plan[pose_idx].pose; - auto next_pose = global_plan[pose_idx + 1].pose; + const auto prev_pose = global_plan[pose_idx - 1].pose; + const auto pose = global_plan[pose_idx].pose; + const auto next_pose = global_plan[pose_idx + 1].pose; // Check if the angle of the pose is obtuse, otherwise warn and ignore this pose // We check using Pythagorean theorem: if c*c > (a*a + b*b) it is an obtuse angle and thus we can follow it - double a_squared = distSquared(prev_pose, pose); - double b_squared = distSquared(pose, next_pose); - double c_squared = distSquared(prev_pose, next_pose); + const double a_squared = distSquared(prev_pose, pose); + const double b_squared = distSquared(pose, next_pose); + const double c_squared = distSquared(prev_pose, next_pose); if (c_squared > (a_squared + b_squared)) { tf2::fromMsg(pose, transform); @@ -194,13 +194,13 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome /* Create distance and turning radius vectors once for usage later */ deltaPlan = global_plan_tf_[idx_path].inverseTimes(global_plan_tf_[idx_path + 1]); - double dpX = deltaPlan.getOrigin().x(); - double dpY = deltaPlan.getOrigin().y(); + const double dpX = deltaPlan.getOrigin().x(); + const double dpY = deltaPlan.getOrigin().y(); iterative_dist_to_goal += hypot(dpX, dpY); distance_to_goal_vector_[idx_path] = iterative_dist_to_goal; // compute turning radius based on trigonometric analysis // radius such that next pose is connected from current pose with a semi-circle - double dpXY2 = dpY*dpY + dpX*dpX; + const double dpXY2 = dpY * dpY + dpX * dpX; if (dpXY2 < FLT_EPSILON) { turning_radius_inv_vector_[idx_path] = std::numeric_limits::infinity(); @@ -271,7 +271,7 @@ void Controller::distToSegmentSquared( const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w, tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w) const { - double l2 = distSquared(pose_v, pose_w); + const double l2 = distSquared(pose_v, pose_w); if (l2 == 0) { pose_projection = pose_w; @@ -432,11 +432,11 @@ geometry_msgs::Twist Controller::update(double target_x_vel, ros::Duration dt, double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug) { - double current_x_vel = controller_state_.current_x_vel; + const double current_x_vel = controller_state_.current_x_vel; const double current_yaw_vel = controller_state_.current_yaw_vel; // Compute location of the point to be controlled - double theda_rp = tf2::getYaw(current_tf.rotation); + const double theda_rp = tf2::getYaw(current_tf.rotation); tf2::Vector3 current_with_carrot_origin; current_with_carrot_origin.setX(current_tf.translation.x + l_ * cos(theda_rp)); current_with_carrot_origin.setY(current_tf.translation.y + l_ * sin(theda_rp)); @@ -532,7 +532,6 @@ geometry_msgs::Twist Controller::update(double target_x_vel, /***** Compute forward velocity *****/ // Apply acceleration limits and end velocity - double acc; double t_end_phase_current; double d_end_phase; @@ -565,7 +564,7 @@ geometry_msgs::Twist Controller::update(double target_x_vel, tf2::Transform robot_pose; tf2::convert(current_tf, robot_pose); tf2::Transform base_to_goal = robot_pose.inverseTimes(current_goal_); - double angle_to_goal = atan2(base_to_goal.getOrigin().x(), -base_to_goal.getOrigin().y()); + const double angle_to_goal = atan2(base_to_goal.getOrigin().x(), -base_to_goal.getOrigin().y()); // If we are as close to our goal or closer then we need to reach end velocity, enable end_phase. // However, if robot is not facing to the same direction as the local velocity target vector, don't enable end_phase. @@ -623,9 +622,9 @@ geometry_msgs::Twist Controller::update(double target_x_vel, } } - double acc_desired = (current_target_x_vel_ - current_x_vel) / dt.toSec(); - double acc_abs = fmin(fabs(acc_desired), fabs(current_target_acc)); - acc = copysign(acc_abs, current_target_acc); + const double acc_desired = (current_target_x_vel_ - current_x_vel) / dt.toSec(); + const double acc_abs = fmin(fabs(acc_desired), fabs(current_target_acc)); + const auto acc = copysign(acc_abs, current_target_acc); double new_x_vel = current_x_vel + acc * dt.toSec(); @@ -666,7 +665,7 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // eda (Estimated duration of arrival) estimation if (fabs(target_x_vel) > VELOCITY_EPS) { - double t_const = (copysign(distance_to_goal_, target_x_vel) - d_end_phase) / target_x_vel; + const double t_const = (copysign(distance_to_goal_, target_x_vel) - d_end_phase) / target_x_vel; *eda = fmin(fmax(t_end_phase_current, 0.0) + fmax(t_const, 0.0), LONG_DURATION); } else @@ -784,9 +783,9 @@ geometry_msgs::Twist Controller::update(double target_x_vel, output_combined.angular.z = std::clamp(output_combined.angular.z, -max_ang_twist_tr, max_ang_twist_tr); } // Apply max acceleration limit to yaw - double yaw_acc = std::clamp((output_combined.angular.z - current_yaw_vel) / dt.toSec(), - -max_yaw_acc_, max_yaw_acc_); - double new_yaw_vel = current_yaw_vel + (yaw_acc * dt.toSec()); + const double yaw_acc = std::clamp((output_combined.angular.z - current_yaw_vel) / dt.toSec(), + -max_yaw_acc_, max_yaw_acc_); + const double new_yaw_vel = current_yaw_vel + (yaw_acc * dt.toSec()); output_combined.angular.z = new_yaw_vel; // Transform velocity commands at base_link to steer when using tricycle model @@ -809,9 +808,9 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // Apply limits to steering commands steering_cmd.steering_angle = std::clamp(steering_cmd.steering_angle, -max_steering_angle_, max_steering_angle_); - double steering_yaw_vel = std::clamp((steering_cmd.steering_angle - controller_state_.previous_steering_angle) + const double steering_yaw_vel = std::clamp((steering_cmd.steering_angle - controller_state_.previous_steering_angle) / dt.toSec(), -max_steering_yaw_vel_, max_steering_yaw_vel_); - double steering_angle_acc = std::clamp((steering_yaw_vel - controller_state_.previous_steering_yaw_vel)/ dt.toSec(), + const double steering_angle_acc = std::clamp((steering_yaw_vel - controller_state_.previous_steering_yaw_vel)/ dt.toSec(), -max_steering_yaw_acc_, max_steering_yaw_acc_); steering_cmd.steering_angle_velocity = controller_state_.previous_steering_yaw_vel + (steering_angle_acc * dt.toSec()); @@ -989,7 +988,7 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T &eda_unused, &progress_unused, &pid_debug_unused); // Run plant model - double theta = tf2::getYaw(predicted_tf.rotation); + const double theta = tf2::getYaw(predicted_tf.rotation); predicted_tf.translation.x += pred_twist.linear.x * cos(theta) * mpc_simulation_sample_time_; predicted_tf.translation.y += pred_twist.linear.x * sin(theta) * mpc_simulation_sample_time_; tf2::Quaternion q; From 367d68f95494e67e129cba814cae31de9fabb427 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 16 Feb 2022 17:59:10 +0000 Subject: [PATCH 072/156] Replaced helper function sign(). --- src/controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 6f3a6a1b..d381733c 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -31,10 +31,11 @@ constexpr double ang_lower_limit = -100.0; constexpr double windup_limit = 1000.0; -// Typesafe sign implementation with signum: -// https://stackoverflow.com/a/4609795 -template int sign(T val) { - return (T(0) < val) - (val < T(0)); +// Indicates if both values have the same sign. +template +bool have_same_sign(T val1, T val2) +{ + return std::signbit(val1) == std::signbit(val2); } } // namespace anonymous @@ -569,8 +570,7 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // If we are as close to our goal or closer then we need to reach end velocity, enable end_phase. // However, if robot is not facing to the same direction as the local velocity target vector, don't enable end_phase. // This is to avoid skipping paths that start with opposite velocity. - if (distance_to_goal_ <= fabs(d_end_phase) - && sign(target_x_vel) == sign(angle_to_goal)) + if ((distance_to_goal_ <= fabs(d_end_phase)) && have_same_sign(target_x_vel, angle_to_goal)) { // This state will be remebered to avoid jittering on target_x_vel controller_state_.end_phase_enabled = true; From 41ac8bbe6c24433754e6514398ef12c3810802a4 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 17 Feb 2022 08:35:04 +0000 Subject: [PATCH 073/156] Added to_transform() helper. --- src/controller.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index d381733c..142e0efc 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -38,6 +38,14 @@ bool have_same_sign(T val1, T val2) return std::signbit(val1) == std::signbit(val2); } +// Converts a pose to the corresponding transform. +tf2::Transform to_transform(const geometry_msgs::Pose & pose) +{ + tf2::Transform result; + tf2::fromMsg(pose, result); + return result; +} + } // namespace anonymous void Controller::setHolonomic(bool holonomic) @@ -125,9 +133,7 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome /* Minimal sanity check */ global_plan_tf_.clear(); - tf2::Transform transform; - tf2::fromMsg(global_plan[0].pose, transform); - global_plan_tf_.push_back(transform); + global_plan_tf_.push_back(to_transform(global_plan[0].pose)); // For now do not allow repeated points or in-place rotation // To allow that the way the progress is checked and the interpolation is done needs to be changed // Also check if points suddenly go in the opposite direction, this could lead to deadlocks @@ -143,8 +149,7 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome const double c_squared = distSquared(prev_pose, next_pose); if (c_squared > (a_squared + b_squared)) { - tf2::fromMsg(pose, transform); - global_plan_tf_.push_back(transform); + global_plan_tf_.push_back(to_transform(pose)); } else { @@ -152,8 +157,7 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome } } // Add last pose as we didn't evaluate that one - tf2::Transform last_transform; - tf2::fromMsg(global_plan.back().pose, last_transform); + const auto last_transform = to_transform(global_plan.back().pose); global_plan_tf_.push_back(last_transform); if (!track_base_link_enabled_) From 4d80e5026e01eb709b965eed49721778752f780c Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 17 Feb 2022 08:46:32 +0000 Subject: [PATCH 074/156] Moved distSquared() helpers. --- include/path_tracking_pid/controller.hpp | 2 -- src/controller.cpp | 30 +++++++++++------------- 2 files changed, 14 insertions(+), 18 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index ddf162d7..607f1bbd 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -196,8 +196,6 @@ class Controller : private boost::noncopyable double getVelMaxObstacle() const; private: - static double distSquared(const tf2::Transform& pose_v, const tf2::Transform& pose_w); - static double distSquared(const geometry_msgs::Pose& pose_v, const geometry_msgs::Pose& pose_w); void distToSegmentSquared(const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w, tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w) const; diff --git a/src/controller.cpp b/src/controller.cpp index 90784569..1d61a53b 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -46,6 +46,20 @@ tf2::Transform to_transform(const geometry_msgs::Pose & pose) return result; } +// Returns the square distance between two points +double distSquared(const tf2::Transform & a, const tf2::Transform & b) +{ + return std::pow(a.getOrigin().x() - b.getOrigin().x(), 2) + + std::pow(a.getOrigin().y() - b.getOrigin().y(), 2); +} + +// Return the square distance between two points. +double distSquared(const geometry_msgs::Pose & a, const geometry_msgs::Pose & b) +{ + return std::pow(a.position.x - b.position.x, 2) + + std::pow(a.position.y - b.position.y, 2); +} + } // namespace anonymous void Controller::setHolonomic(bool holonomic) @@ -256,22 +270,6 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome // TODO(clopez) use steering_odom_twist to check if setpoint is being followed } -// https://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment -// TODO(Cesar): expand to 3 dimensions -double Controller::distSquared(const tf2::Transform& pose_v, const tf2::Transform& pose_w) -{ - // Returns square distance between 2 points - return (pose_v.getOrigin().x() - pose_w.getOrigin().x()) * (pose_v.getOrigin().x() - pose_w.getOrigin().x()) + - (pose_v.getOrigin().y() - pose_w.getOrigin().y()) * (pose_v.getOrigin().y() - pose_w.getOrigin().y()); -} - -double Controller::distSquared(const geometry_msgs::Pose& pose_v, const geometry_msgs::Pose& pose_w) -{ - // Returns square distance between 2 points - return (pose_v.position.x - pose_w.position.x) * (pose_v.position.x - pose_w.position.x) + - (pose_v.position.y - pose_w.position.y) * (pose_v.position.y - pose_w.position.y); -} - void Controller::distToSegmentSquared( const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w, tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w) const From f267837f0e140f2a745f826e27ed32e48ce111df Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 21 Feb 2022 10:14:11 +0000 Subject: [PATCH 075/156] Reworked by switching to distance2(). --- src/controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 1d61a53b..edf22d1a 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -49,8 +49,7 @@ tf2::Transform to_transform(const geometry_msgs::Pose & pose) // Returns the square distance between two points double distSquared(const tf2::Transform & a, const tf2::Transform & b) { - return std::pow(a.getOrigin().x() - b.getOrigin().x(), 2) + - std::pow(a.getOrigin().y() - b.getOrigin().y(), 2); + return a.getOrigin().distance2(b.getOrigin()); } // Return the square distance between two points. From 5224fa1dcabf0fe126913e0a700596cbe528f456 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 17 Feb 2022 09:53:41 +0000 Subject: [PATCH 076/156] Added is_pose_angle_obtuse() helper. --- src/controller.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index edf22d1a..13131439 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -59,7 +59,15 @@ double distSquared(const geometry_msgs::Pose & a, const geometry_msgs::Pose & b) std::pow(a.position.y - b.position.y, 2); } -} // namespace anonymous +// Indicates if the angle of the cur pose is obtuse (with respect to the prev and next poses). +bool is_pose_angle_obtuse( + const geometry_msgs::Pose & prev, const geometry_msgs::Pose & cur, + const geometry_msgs::Pose & next) +{ + return distSquared(prev, next) > (distSquared(prev, cur) + distSquared(cur, next)); +} + +} // namespace anonymous void Controller::setHolonomic(bool holonomic) { @@ -155,12 +163,7 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome const auto prev_pose = global_plan[pose_idx - 1].pose; const auto pose = global_plan[pose_idx].pose; const auto next_pose = global_plan[pose_idx + 1].pose; - // Check if the angle of the pose is obtuse, otherwise warn and ignore this pose - // We check using Pythagorean theorem: if c*c > (a*a + b*b) it is an obtuse angle and thus we can follow it - const double a_squared = distSquared(prev_pose, pose); - const double b_squared = distSquared(pose, next_pose); - const double c_squared = distSquared(prev_pose, next_pose); - if (c_squared > (a_squared + b_squared)) + if (is_pose_angle_obtuse(prev_pose, pose, next_pose)) { global_plan_tf_.push_back(to_transform(pose)); } From c033dc87fe628d74c0f6a1fef0dbd78cb50d3318 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 14 Feb 2022 14:07:18 +0000 Subject: [PATCH 077/156] Removed config duplicate members. --- include/path_tracking_pid/controller.hpp | 41 +---- src/controller.cpp | 221 +++++++++-------------- 2 files changed, 90 insertions(+), 172 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 607f1bbd..ed08d9e6 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -216,7 +216,7 @@ class Controller : private boost::noncopyable */ void printParameters() const; - path_tracking_pid::PidConfig local_config_; + path_tracking_pid::PidConfig config_; ControllerState controller_state_; // Global Plan variables @@ -235,12 +235,7 @@ class Controller : private boost::noncopyable double control_effort_ang_ = 0.0; // output of pid controller bool enabled_ = true; - bool feedback_lat_enabled_ = false; - bool feedback_ang_enabled_ = false; - bool feedforward_lat_enabled_ = false; - bool feedforward_ang_enabled_ = false; bool holonomic_robot_enable_ = false; - bool track_base_link_enabled_ = false; bool estimate_pose_angle_enabled_ = false; // feedforward controller @@ -250,44 +245,12 @@ class Controller : private boost::noncopyable // tricycle model bool use_tricycle_model_ = false; geometry_msgs::Transform tf_base_to_steered_wheel_; - double max_steering_angle_ = NAN; - double max_steering_x_vel_ = NAN; - double max_steering_x_acc_ = NAN; - double max_steering_yaw_vel_ = NAN; - double max_steering_yaw_acc_ = NAN; std::array, 2> inverse_kinematics_matrix_{}; std::array, 2> forward_kinematics_matrix_{}; - bool debug_enabled_ = false; - - // Primary feedback controller parameters - double Kp_lat_ = 0.0; - double Ki_lat_ = 0.0; - double Kd_lat_ = 0.0; - double Kp_ang_ = 0.0; - double Ki_ang_ = 0.0; - double Kd_ang_ = 0.0; - double l_ = 0.0; - double target_x_vel_ = 0.0; - double target_end_x_vel_ = 0.0; - double target_x_acc_ = 0.0; - double target_x_decc_ = 0.0; - double max_error_x_vel_ = 0.0; - double abs_minimum_x_vel_ = 0.0; - double max_yaw_vel_ = 0.0; - double max_yaw_acc_ = 0.0; - double minimum_turning_radius_ = FLT_EPSILON; - // Velocity limits that can be active external to the pid controller: double vel_max_external_ = INFINITY; // Dynamic external max velocity requirement (e.g. no more power available) double vel_max_obstacle_ = INFINITY; // Can be zero if lethal obstacles are detected - - // MPC settings - int mpc_max_fwd_iter_ = -1; // Define # of steps that you look into the future with MPC [-] - int mpc_max_vel_optimization_iter_ = -1; // Set maximum # of velocity bisection iterations - // (maximum total iterations = max_opt_iter*max_iter) [-] - double mpc_simulation_sample_time_ = NAN; // Define timestep [s] - double mpc_max_error_lat_ = NAN; // Maximum allowed lateral error [m] - double mpc_min_x_vel_ = NAN; // Minimum forward x velocity [m/s] }; + } // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index 13131439..3a27961c 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -176,11 +176,11 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome const auto last_transform = to_transform(global_plan.back().pose); global_plan_tf_.push_back(last_transform); - if (!track_base_link_enabled_) + if (!config_.track_base_link) { // Add carrot length to plan using goal pose (we assume the last pose contains correct angle) tf2::Transform carrotTF(tf2::Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), - tf2::Vector3(l_, 0.0, 0.0)); + tf2::Vector3(config_.l, 0.0, 0.0)); global_plan_tf_.push_back(last_transform * carrotTF); } @@ -236,7 +236,7 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome } // Set initial velocity - switch (local_config_.init_vel_method) + switch (config_.init_vel_method) { case Pid_Zero: reset(); @@ -253,7 +253,7 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome } // When velocity error is too big reset current_x_vel - if (fabs(odom_twist.linear.x - controller_state_.current_x_vel) > max_error_x_vel_) + if (fabs(odom_twist.linear.x - controller_state_.current_x_vel) > config_.max_error_x_vel) { // TODO(clopez/mcfurry/nobleo): Give feedback to higher level software here ROS_WARN("Large control error. Current_x_vel %f / odometry %f", @@ -443,8 +443,8 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // Compute location of the point to be controlled const double theda_rp = tf2::getYaw(current_tf.rotation); tf2::Vector3 current_with_carrot_origin; - current_with_carrot_origin.setX(current_tf.translation.x + l_ * cos(theda_rp)); - current_with_carrot_origin.setY(current_tf.translation.y + l_ * sin(theda_rp)); + current_with_carrot_origin.setX(current_tf.translation.x + config_.l * cos(theda_rp)); + current_with_carrot_origin.setY(current_tf.translation.y + config_.l * sin(theda_rp)); current_with_carrot_origin.setZ(0); current_with_carrot_.setOrigin(current_with_carrot_origin); @@ -452,15 +452,15 @@ geometry_msgs::Twist Controller::update(double target_x_vel, current_with_carrot_.setRotation(cur_rot); size_t path_pose_idx; - if (track_base_link_enabled_) + if (config_.track_base_link) { // Find closes robot position to path and then project carrot on goal current_pos_on_plan_ = current_goal_ = findPositionOnPlan(current_tf, &controller_state_, path_pose_idx); // To track the base link the goal is then transform to the control point goal double theda_rp = tf2::getYaw(current_goal_.getRotation()); tf2::Vector3 newControlOrigin; - newControlOrigin.setX(current_goal_.getOrigin().x() + l_ * cos(theda_rp)); - newControlOrigin.setY(current_goal_.getOrigin().y() + l_ * sin(theda_rp)); + newControlOrigin.setX(current_goal_.getOrigin().x() + config_.l * cos(theda_rp)); + newControlOrigin.setY(current_goal_.getOrigin().y() + config_.l * sin(theda_rp)); newControlOrigin.setZ(0); current_goal_.setOrigin(newControlOrigin); } @@ -478,13 +478,13 @@ geometry_msgs::Twist Controller::update(double target_x_vel, tf2::Transform error = current_with_carrot_.inverseTimes(current_goal_); //***** Feedback control *****// - if (!((Kp_lat_ <= 0. && Ki_lat_ <= 0. && Kd_lat_ <= 0.) || - (Kp_lat_ >= 0. && Ki_lat_ >= 0. && Kd_lat_ >= 0.))) // All 3 gains should have the same sign + if (!((config_.Kp_lat <= 0. && config_.Ki_lat <= 0. && config_.Kd_lat <= 0.) || + (config_.Kp_lat >= 0. && config_.Ki_lat >= 0. && config_.Kd_lat >= 0.))) // All 3 gains should have the same sign { ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); } - if (!((Kp_ang_ <= 0. && Ki_ang_ <= 0. && Kd_ang_ <= 0.) || - (Kp_ang_ >= 0. && Ki_ang_ >= 0. && Kd_ang_ >= 0.))) // All 3 gains should have the same sign + if (!((config_.Kp_ang <= 0. && config_.Ki_ang <= 0. && config_.Kd_ang <= 0.) || + (config_.Kp_ang >= 0. && config_.Ki_ang >= 0. && config_.Kd_ang >= 0.))) // All 3 gains should have the same sign { ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); } @@ -526,13 +526,13 @@ geometry_msgs::Twist Controller::update(double target_x_vel, (controller_state_.error_ang.errors().at<0>() - controller_state_.error_ang.errors().at<1>()) / dt.toSec()); // calculate the control effort - const auto proportional_lat = Kp_lat_ * controller_state_.error_lat.filtered_errors().at<0>(); - const auto integral_lat = Ki_lat_ * controller_state_.error_integral_lat; - const auto derivative_lat = Kd_lat_ * controller_state_.error_deriv_lat.filtered_errors().at<0>(); + const auto proportional_lat = config_.Kp_lat * controller_state_.error_lat.filtered_errors().at<0>(); + const auto integral_lat = config_.Ki_lat * controller_state_.error_integral_lat; + const auto derivative_lat = config_.Kd_lat * controller_state_.error_deriv_lat.filtered_errors().at<0>(); - const auto proportional_ang = Kp_ang_ * controller_state_.error_ang.filtered_errors().at<0>(); - const auto integral_ang = Ki_ang_ * controller_state_.error_integral_ang; - const auto derivative_ang = Kd_ang_ * controller_state_.error_deriv_ang.filtered_errors().at<0>(); + const auto proportional_ang = config_.Kp_ang * controller_state_.error_ang.filtered_errors().at<0>(); + const auto integral_ang = config_.Ki_ang * controller_state_.error_integral_ang; + const auto derivative_ang = config_.Kd_ang * controller_state_.error_deriv_ang.filtered_errors().at<0>(); /***** Compute forward velocity *****/ @@ -549,16 +549,16 @@ geometry_msgs::Twist Controller::update(double target_x_vel, if ((current_target_x_vel_ > 0.0 && current_x_vel > target_end_x_vel) || (current_target_x_vel_ < 0.0 && current_x_vel < target_end_x_vel)) { - t_end_phase_current = (target_end_x_vel - current_x_vel) / (-target_x_decc_); + t_end_phase_current = (target_end_x_vel - current_x_vel) / (-config_.target_x_decc); d_end_phase = current_x_vel * t_end_phase_current - - 0.5 * (target_x_decc_) * t_end_phase_current * t_end_phase_current + - 0.5 * (config_.target_x_decc) * t_end_phase_current * t_end_phase_current + target_x_vel * 2.0 * dt.toSec(); } else { - t_end_phase_current = (target_end_x_vel - current_x_vel) / (target_x_acc_); + t_end_phase_current = (target_end_x_vel - current_x_vel) / (config_.target_x_acc); d_end_phase = current_x_vel * t_end_phase_current - + 0.5 * (target_x_acc_) * t_end_phase_current * t_end_phase_current + + 0.5 * (config_.target_x_acc) * t_end_phase_current * t_end_phase_current + target_x_vel * 2.0 * dt.toSec(); } ROS_DEBUG("t_end_phase_current: %f", t_end_phase_current); @@ -596,33 +596,33 @@ geometry_msgs::Twist Controller::update(double target_x_vel, { if (current_x_vel > current_target_x_vel_) { - current_target_acc = -target_x_decc_; + current_target_acc = -config_.target_x_decc; } else { - current_target_acc = target_x_decc_; + current_target_acc = config_.target_x_decc; } } else if (current_target_x_vel_ > 0) // Positive velocity requested { if (current_x_vel > current_target_x_vel_) { - current_target_acc = -target_x_decc_; + current_target_acc = -config_.target_x_decc; } else { - current_target_acc = target_x_acc_; + current_target_acc = config_.target_x_acc; } } else // Negative velocity requested { if (current_x_vel > current_target_x_vel_) { - current_target_acc = -target_x_acc_; + current_target_acc = -config_.target_x_acc; } else { - current_target_acc = target_x_decc_; + current_target_acc = config_.target_x_decc; } } @@ -633,7 +633,7 @@ geometry_msgs::Twist Controller::update(double target_x_vel, double new_x_vel = current_x_vel + acc * dt.toSec(); // For low target_end_x_vel we have a minimum velocity to ensure the goal is reached - double min_vel = copysign(1.0, l_) * abs_minimum_x_vel_; + double min_vel = copysign(1.0, config_.l) * config_.abs_minimum_x_vel; if (!controller_state_.end_reached && controller_state_.end_phase_enabled && fabs(target_end_x_vel) <= fabs(min_vel) + VELOCITY_EPS && fabs(new_x_vel) <= fabs(min_vel) + VELOCITY_EPS) @@ -643,7 +643,7 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // When velocity error is too big reset current_x_vel if (fabs(odom_twist.linear.x) < fabs(current_target_x_vel_) && - fabs(odom_twist.linear.x - new_x_vel) > max_error_x_vel_) + fabs(odom_twist.linear.x - new_x_vel) > config_.max_error_x_vel) { // TODO(clopez/mcfurry/nobleo): Give feedback to higher level software here ROS_WARN_THROTTLE(1.0, "Large tracking error. Current_x_vel %f / odometry %f", @@ -686,17 +686,17 @@ geometry_msgs::Twist Controller::update(double target_x_vel, control_effort_lat_ = 0.0; control_effort_ang_ = 0.0; - if (feedback_lat_enabled_) + if (config_.feedback_lat) { control_effort_lat_ = proportional_lat + integral_lat + derivative_lat; } - if (feedback_ang_enabled_) + if (config_.feedback_ang) { control_effort_ang_ = proportional_ang + integral_ang + derivative_ang; } //***** Feedforward control *****// - if (feedforward_lat_enabled_) + if (config_.feedforward_lat) { feedforward_lat_ = 0.0; // Not implemented control_effort_lat_ = control_effort_lat_ + feedforward_lat_; @@ -706,7 +706,7 @@ geometry_msgs::Twist Controller::update(double target_x_vel, feedforward_lat_ = 0.0; } - if (feedforward_ang_enabled_) + if (config_.feedforward_ang) { feedforward_ang_ = turning_radius_inv_vector_[controller_state_.last_visited_pose_index]*control_effort_long_; ROS_DEBUG("turning_radius_inv_vector[%lu] = %f", @@ -759,7 +759,7 @@ geometry_msgs::Twist Controller::update(double target_x_vel, output_combined.angular.x = 0; output_combined.angular.y = 0; output_combined.angular.z = control_effort_ang_; - output_combined.angular.z = std::clamp(output_combined.angular.z, -max_yaw_vel_, max_yaw_vel_); + output_combined.angular.z = std::clamp(output_combined.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); } else { @@ -768,12 +768,12 @@ geometry_msgs::Twist Controller::update(double target_x_vel, output_combined.linear.z = 0; output_combined.angular.x = 0; output_combined.angular.y = 0; - output_combined.angular.z = copysign(1.0, l_) * control_effort_lat_ + + output_combined.angular.z = copysign(1.0, config_.l) * control_effort_lat_ + control_effort_ang_; // Take the sign of l for the lateral control effort - output_combined.angular.z = std::clamp(output_combined.angular.z, -max_yaw_vel_, max_yaw_vel_); + output_combined.angular.z = std::clamp(output_combined.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); // For non-holonomic robots apply saturation based on minimum turning radius double max_ang_twist_tr; - if (minimum_turning_radius_ < RADIUS_EPS) + if (config_.min_turning_radius < RADIUS_EPS) { // Rotation in place is allowed // minimum_turning_radius = RADIUS_EPS; // This variable is not used anymore so it does not matter @@ -782,13 +782,13 @@ geometry_msgs::Twist Controller::update(double target_x_vel, } else { - max_ang_twist_tr = fabs(output_combined.linear.x / minimum_turning_radius_); + max_ang_twist_tr = fabs(output_combined.linear.x / config_.min_turning_radius); } output_combined.angular.z = std::clamp(output_combined.angular.z, -max_ang_twist_tr, max_ang_twist_tr); } // Apply max acceleration limit to yaw const double yaw_acc = std::clamp((output_combined.angular.z - current_yaw_vel) / dt.toSec(), - -max_yaw_acc_, max_yaw_acc_); + -config_.max_yaw_acc, config_.max_yaw_acc); const double new_yaw_vel = current_yaw_vel + (yaw_acc * dt.toSec()); output_combined.angular.z = new_yaw_vel; @@ -811,19 +811,19 @@ geometry_msgs::Twist Controller::update(double target_x_vel, } // Apply limits to steering commands steering_cmd.steering_angle = std::clamp(steering_cmd.steering_angle, - -max_steering_angle_, max_steering_angle_); + -config_.max_steering_angle, config_.max_steering_angle); const double steering_yaw_vel = std::clamp((steering_cmd.steering_angle - controller_state_.previous_steering_angle) - / dt.toSec(), -max_steering_yaw_vel_, max_steering_yaw_vel_); + / dt.toSec(), -config_.max_steering_yaw_vel, config_.max_steering_yaw_vel); const double steering_angle_acc = std::clamp((steering_yaw_vel - controller_state_.previous_steering_yaw_vel)/ dt.toSec(), - -max_steering_yaw_acc_, max_steering_yaw_acc_); + -config_.max_steering_yaw_acc, config_.max_steering_yaw_acc); steering_cmd.steering_angle_velocity = controller_state_.previous_steering_yaw_vel + (steering_angle_acc * dt.toSec()); steering_cmd.steering_angle = controller_state_.previous_steering_angle + (steering_cmd.steering_angle_velocity * dt.toSec()); - steering_cmd.speed = std::clamp(steering_cmd.speed, -max_steering_x_vel_, max_steering_x_vel_); + steering_cmd.speed = std::clamp(steering_cmd.speed, -config_.max_steering_x_vel, config_.max_steering_x_vel); steering_cmd.acceleration = std::clamp((steering_cmd.speed - controller_state_.previous_steering_x_vel)/ dt.toSec(), - -max_steering_x_acc_, max_steering_x_acc_); + -config_.max_steering_x_acc, config_.max_steering_x_acc); steering_cmd.speed = controller_state_.previous_steering_x_vel + (steering_cmd.acceleration * dt.toSec()); controller_state_.previous_steering_angle = steering_cmd.steering_angle; @@ -861,7 +861,7 @@ geometry_msgs::Twist Controller::update_with_limits(const geometry_msgs::Transfo path_tracking_pid::PidDebug* pid_debug) { // All limits are absolute - double max_x_vel = std::abs(target_x_vel_); + double max_x_vel = std::abs(config_.target_x_vel); // Apply external limit max_x_vel = std::min(max_x_vel, vel_max_external_); @@ -871,16 +871,16 @@ geometry_msgs::Twist Controller::update_with_limits(const geometry_msgs::Transfo // Apply mpc limit (last because less iterations required if max vel is already limited) double vel_max_mpc = std::numeric_limits::infinity(); - if (local_config_.use_mpc) + if (config_.use_mpc) { - vel_max_mpc = std::abs(mpc_based_max_vel(std::copysign(max_x_vel, target_x_vel_), current_tf, odom_twist)); + vel_max_mpc = std::abs(mpc_based_max_vel(std::copysign(max_x_vel, config_.target_x_vel), current_tf, odom_twist)); max_x_vel = std::min(max_x_vel, vel_max_mpc); } // Some logging: ROS_DEBUG("max_x_vel=%.3f, target_x_vel=%.3f, vel_max_external=%.3f, vel_max_obstacle=%.3f, vel_max_mpc=%.3f", - max_x_vel, target_x_vel_, vel_max_external_, vel_max_obstacle_, vel_max_mpc); - if (max_x_vel != target_x_vel_) + max_x_vel, config_.target_x_vel, vel_max_external_, vel_max_obstacle_, vel_max_mpc); + if (max_x_vel != config_.target_x_vel) { if (max_x_vel == vel_max_external_) { @@ -897,11 +897,11 @@ geometry_msgs::Twist Controller::update_with_limits(const geometry_msgs::Transfo } // The end velocity is bound by the same limits to avoid accelerating above the limit in the end phase - double max_end_x_vel = std::min({std::abs(target_end_x_vel_), vel_max_external_, vel_max_obstacle_, vel_max_mpc}); // NOLINT - max_end_x_vel = std::copysign(max_end_x_vel, target_end_x_vel_); + double max_end_x_vel = std::min({std::abs(config_.target_end_x_vel), vel_max_external_, vel_max_obstacle_, vel_max_mpc}); + max_end_x_vel = std::copysign(max_end_x_vel, config_.target_end_x_vel); // Update the controller with the new setting - max_x_vel = std::copysign(max_x_vel, target_x_vel_); + max_x_vel = std::copysign(max_x_vel, config_.target_x_vel); return update(max_x_vel, max_end_x_vel, current_tf, odom_twist, dt, eda, progress, pid_debug); } @@ -928,19 +928,19 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T double new_nominal_x_vel = target_x_vel; // Start off from the current velocity // Loop MPC - while (mpc_fwd_iter < mpc_max_fwd_iter_ && mpc_vel_optimization_iter <= mpc_max_vel_optimization_iter_) + while (mpc_fwd_iter < config_.mpc_max_fwd_iterations && mpc_vel_optimization_iter <= config_.mpc_max_vel_optimization_iterations) { mpc_fwd_iter += 1; // Check if robot stays within bounds for all iterations, if the new_nominal_x_vel is smaller than // max_target_x_vel we can increase it - if (mpc_fwd_iter == mpc_max_fwd_iter_ && fabs(controller_state_.error_lat.errors().at<0>()) <= mpc_max_error_lat_ && + if (mpc_fwd_iter == config_.mpc_max_fwd_iterations && fabs(controller_state_.error_lat.errors().at<0>()) <= config_.mpc_max_error_lat && fabs(new_nominal_x_vel) < abs(target_x_vel)) { mpc_vel_optimization_iter += 1; // When we reach the maximum allowed mpc optimization iterations, do not change velocity anymore - if (mpc_vel_optimization_iter > mpc_max_vel_optimization_iter_) + if (mpc_vel_optimization_iter > config_.mpc_max_vel_optimization_iterations) { break; } @@ -958,7 +958,7 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T mpc_fwd_iter = 0; } // If the robot gets out of bounds earlier we decrease the velocity - else if (abs(controller_state_.error_lat.errors().at<0>()) >= mpc_max_error_lat_) + else if (abs(controller_state_.error_lat.errors().at<0>()) >= config_.mpc_max_error_lat) { mpc_vel_optimization_iter += 1; @@ -980,29 +980,29 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T ROS_WARN_THROTTLE(5.0, "Lowering velocity did not decrease the lateral error enough."); } } - else if (mpc_fwd_iter != mpc_max_fwd_iter_) + else if (mpc_fwd_iter != config_.mpc_max_fwd_iterations) { // Run controller // Output: pred_twist.[linear.x, linear.y, linear.z, angular.x, angular.y, angular.z] path_tracking_pid::PidDebug pid_debug_unused; double eda_unused; double progress_unused; - pred_twist = Controller::update(new_nominal_x_vel, target_end_x_vel_, predicted_tf, pred_twist, - ros::Duration(mpc_simulation_sample_time_), + pred_twist = Controller::update(new_nominal_x_vel, config_.target_end_x_vel, predicted_tf, pred_twist, + ros::Duration(config_.mpc_simulation_sample_time), &eda_unused, &progress_unused, &pid_debug_unused); // Run plant model const double theta = tf2::getYaw(predicted_tf.rotation); - predicted_tf.translation.x += pred_twist.linear.x * cos(theta) * mpc_simulation_sample_time_; - predicted_tf.translation.y += pred_twist.linear.x * sin(theta) * mpc_simulation_sample_time_; + predicted_tf.translation.x += pred_twist.linear.x * cos(theta) * config_.mpc_simulation_sample_time; + predicted_tf.translation.y += pred_twist.linear.x * sin(theta) * config_.mpc_simulation_sample_time; tf2::Quaternion q; - q.setRPY(0, 0, theta + pred_twist.angular.z * mpc_simulation_sample_time_); + q.setRPY(0, 0, theta + pred_twist.angular.z * config_.mpc_simulation_sample_time); predicted_tf.rotation = tf2::toMsg(q); } } // Apply limits to the velocity double mpc_vel_limit = copysign(1.0, new_nominal_x_vel) * - fmax(fabs(new_nominal_x_vel), mpc_min_x_vel_); + fmax(fabs(new_nominal_x_vel), config_.mpc_min_x_vel); // Revert global variables controller_state_ = controller_state_saved; @@ -1015,17 +1015,17 @@ void Controller::printParameters() const ROS_INFO("CONTROLLER PARAMETERS"); ROS_INFO("-----------------------------------------"); ROS_INFO("Controller enabled: %i", enabled_); - ROS_INFO("Controller DEBUG enabled: %i", debug_enabled_); - ROS_INFO("Distance L: %f", l_); - ROS_INFO("Track base_link enabled?: %i", track_base_link_enabled_); - - ROS_INFO("Target forward velocities (xv: %f, xv_end,: %f)", target_x_vel_, target_end_x_vel_); - ROS_INFO("Target forward (de)accelerations (xacc: %f, xdecc,: %f)", target_x_acc_, target_x_decc_); - ROS_INFO("Maximum allowed forward velocity error: %f", max_error_x_vel_); - ROS_INFO("Feedback (lat, ang): ( %i, %i)", feedback_lat_enabled_, feedback_ang_enabled_); - ROS_INFO("Feedforward (lat, ang): (%i, %i)", feedforward_lat_enabled_, feedforward_ang_enabled_); - ROS_INFO("Lateral gains: (Kp: %f, Ki, %f, Kd, %f)", Kp_lat_, Ki_lat_, Kd_lat_); - ROS_INFO("Angular gains: (Kp: %f, Ki, %f, Kd, %f)", Kp_ang_, Ki_ang_, Kd_ang_); + ROS_INFO("Controller DEBUG enabled: %i", config_.controller_debug_enabled); + ROS_INFO("Distance L: %f", config_.l); + ROS_INFO("Track base_link enabled?: %i", config_.track_base_link); + + ROS_INFO("Target forward velocities (xv: %f, xv_end,: %f)", config_.target_x_vel, config_.target_end_x_vel); + ROS_INFO("Target forward (de)accelerations (xacc: %f, xdecc,: %f)", config_.target_x_acc, config_.target_x_decc); + ROS_INFO("Maximum allowed forward velocity error: %f", config_.max_error_x_vel); + ROS_INFO("Feedback (lat, ang): ( %i, %i)", config_.feedback_lat, config_.feedback_ang); + ROS_INFO("Feedforward (lat, ang): (%i, %i)", config_.feedforward_lat, config_.feedforward_ang); + ROS_INFO("Lateral gains: (Kp: %f, Ki, %f, Kd, %f)", config_.Kp_lat, config_.Ki_lat, config_.Kd_lat); + ROS_INFO("Angular gains: (Kp: %f, Ki, %f, Kd, %f)", config_.Kp_ang, config_.Ki_ang, config_.Kd_ang); ROS_INFO("Robot type (holonomic): (%i)", holonomic_robot_enable_); @@ -1038,83 +1038,38 @@ void Controller::printParameters() const void Controller::configure(path_tracking_pid::PidConfig& config) { // Erase all queues when config changes - controller_state_.error_lat.reset(); controller_state_.error_deriv_lat.reset(); controller_state_.error_ang.reset(); controller_state_.error_deriv_ang.reset(); - Kp_lat_ = config.Kp_lat; - Ki_lat_ = config.Ki_lat; - Kd_lat_ = config.Kd_lat; - Kp_ang_ = config.Kp_ang; - Ki_ang_ = config.Ki_ang; - Kd_ang_ = config.Kd_ang; - - - track_base_link_enabled_ = config.track_base_link; - ROS_DEBUG("Track base_link? Then global path poses are needed! '%d'", (int)track_base_link_enabled_); - - l_ = config.l; - target_x_vel_ = config.target_x_vel; - l_ = copysign(1.0, target_x_vel_) * fabs(l_); + config.l = copysign(config.l, config.target_x_vel); if (controller_state_.end_phase_enabled) { - ROS_WARN_COND(abs(config.target_end_x_vel - target_end_x_vel_) > 1e-3, "Won't change end velocity in end phase"); - ROS_WARN_COND(abs(config.target_x_acc - target_x_acc_) > 1e-3, "Won't change accelerations in end phase"); - ROS_WARN_COND(abs(config.target_x_decc - target_x_decc_) > 1e-3, "Won't change accelerations in end phase"); - config.target_end_x_vel = target_end_x_vel_; - config.target_x_acc = target_x_acc_; - config.target_x_decc = target_x_decc_; + ROS_WARN_COND(abs(config.target_end_x_vel - config_.target_end_x_vel) > 1e-3, "Won't change end velocity in end phase"); + ROS_WARN_COND(abs(config.target_x_acc - config_.target_x_acc) > 1e-3, "Won't change accelerations in end phase"); + ROS_WARN_COND(abs(config.target_x_decc - config_.target_x_decc) > 1e-3, "Won't change accelerations in end phase"); + config.target_end_x_vel = config_.target_end_x_vel; + config.target_x_acc = config_.target_x_acc; + config.target_x_decc = config_.target_x_decc; } - else - { - target_end_x_vel_ = config.target_end_x_vel; - target_x_acc_ = config.target_x_acc; - target_x_decc_ = config.target_x_decc; - } - abs_minimum_x_vel_ = config.abs_minimum_x_vel; - max_yaw_vel_ = config.max_yaw_vel; - max_yaw_acc_ = config.max_yaw_acc; - - max_error_x_vel_ = config.max_error_x_vel; - - minimum_turning_radius_ = config.min_turning_radius; - debug_enabled_ = config.controller_debug_enabled; - feedforward_lat_enabled_ = config.feedforward_lat; - feedforward_ang_enabled_ = config.feedforward_ang; - feedback_lat_enabled_ = config.feedback_lat; - feedback_ang_enabled_ = config.feedback_ang; - - // MPC config.groups.mpc_group.state = config.use_mpc; // Hide config options if disabled - mpc_max_fwd_iter_ = config.mpc_max_fwd_iterations; - mpc_max_vel_optimization_iter_ = config.mpc_max_vel_optimization_iterations; - mpc_simulation_sample_time_ = config.mpc_simulation_sample_time; - mpc_max_error_lat_ = config.mpc_max_error_lat; - mpc_min_x_vel_ = config.mpc_min_x_vel; - mpc_min_x_vel_ = fmin(mpc_min_x_vel_, fabs(target_x_vel_)); - - // Obstacle speed reduction + config.mpc_min_x_vel = fmin(config.mpc_min_x_vel, fabs(config.target_x_vel)); + config.groups.collision_group.state = config.anti_collision; // Hide config options if disabled - // Tricycle model - max_steering_angle_ = config.max_steering_angle; - max_steering_x_vel_ = config.max_steering_x_vel; - max_steering_x_acc_ = config.max_steering_x_acc; - max_steering_yaw_vel_ = config.max_steering_yaw_vel; - max_steering_yaw_acc_ = config.max_steering_yaw_acc; + config_ = config; - local_config_ = config; + ROS_DEBUG("Track base_link? Then global path poses are needed! '%d'", (int)config_.track_base_link); // printParameters(); } path_tracking_pid::PidConfig Controller::getConfig() { - return local_config_; + return config_; } void Controller::setEnabled(bool value) From 35a32502fd8ad4f10f6b0397c316b4c0650c5251 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 17 Feb 2022 14:22:04 +0000 Subject: [PATCH 078/156] Used ROS2 clang-format on all files. --- include/path_tracking_pid/controller.hpp | 93 ++- .../path_tracking_pid/details/fifo_array.hpp | 15 +- .../details/second_order_lowpass.hpp | 5 +- .../path_tracking_pid_local_planner.hpp | 38 +- include/path_tracking_pid/visualization.hpp | 4 +- src/common.hpp | 1 - src/controller.cpp | 720 +++++++++--------- src/details/second_order_lowpass.cpp | 21 +- src/path_tracking_pid_local_planner.cpp | 284 ++++--- src/visualization.cpp | 4 +- 10 files changed, 558 insertions(+), 627 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index ed08d9e6..4ccfefe0 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -1,9 +1,8 @@ #pragma once +#include #include #include - -#include #include #include "boost/noncopyable.hpp" @@ -16,7 +15,6 @@ namespace path_tracking_pid { - struct TricycleSteeringCmdVel { double steering_angle = 0.0; @@ -56,18 +54,19 @@ class Controller : private boost::noncopyable */ void setHolonomic(bool holonomic); - /** + /** * Enable estimation of pose angles by looking at consecutive path points * @param estimate_pose_angle */ void setEstimatePoseAngle(bool estimate_pose_angle); - /** + /** * Set static configuration of the controller * @param tricycle_model_enabled If tricycle model should be used * @param estimate_pose_angle The transformation from base to steered wheel */ - void setTricycleModel(bool tricycle_model_enabled, const geometry_msgs::Transform& tf_base_to_steered_wheel); + void setTricycleModel( + bool tricycle_model_enabled, const geometry_msgs::Transform & tf_base_to_steered_wheel); /** * Set plan @@ -75,8 +74,9 @@ class Controller : private boost::noncopyable * @param odom_twist Robot odometry * @param global_plan Plan to follow */ - void setPlan(const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist, - const std::vector& global_plan); + void setPlan( + const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const std::vector & global_plan); /** * Set plan @@ -86,9 +86,11 @@ class Controller : private boost::noncopyable * @param steering_odom_twist Steered wheel odometry * @param global_plan Plan to follow */ - void setPlan(const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist, - const geometry_msgs::Transform& tf_base_to_steered_wheel, const geometry_msgs::Twist& steering_odom_twist, - const std::vector& global_plan); + void setPlan( + const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const geometry_msgs::Transform & tf_base_to_steered_wheel, + const geometry_msgs::Twist & steering_odom_twist, + const std::vector & global_plan); /** * Find position on plan by looking at the surroundings of last known pose. * @param current Where is the robot now? @@ -96,11 +98,12 @@ class Controller : private boost::noncopyable * @return tf of found position on plan * @return index of current path-pose if requested */ - tf2::Transform findPositionOnPlan(const geometry_msgs::Transform& current_tf, ControllerState* controller_state_ptr, - size_t& path_pose_idx); + tf2::Transform findPositionOnPlan( + const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr, + size_t & path_pose_idx); // Overloaded function definition for users that don't require the segment index - tf2::Transform findPositionOnPlan(const geometry_msgs::Transform& current_tf, - ControllerState* controller_state_ptr) + tf2::Transform findPositionOnPlan( + const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr) { size_t path_pose_idx; return findPositionOnPlan(current_tf, controller_state_ptr, path_pose_idx); @@ -117,9 +120,10 @@ class Controller : private boost::noncopyable * @return progress Progress along the path [0,1] * @return pid_debug Variable with information to debug the controller */ - geometry_msgs::Twist update(double target_x_vel, double target_end_x_vel, const geometry_msgs::Transform& current_tf, - const geometry_msgs::Twist& odom_twist, ros::Duration dt, double* eda, double* progress, - path_tracking_pid::PidDebug* pid_debug); + geometry_msgs::Twist update( + double target_x_vel, double target_end_x_vel, const geometry_msgs::Transform & current_tf, + const geometry_msgs::Twist & odom_twist, ros::Duration dt, double * eda, double * progress, + path_tracking_pid::PidDebug * pid_debug); /** * Run one iteration of a PID controller with velocity limits applied @@ -131,9 +135,9 @@ class Controller : private boost::noncopyable * @return progress Progress along the path [0,1] * @return pid_debug Variable with information to debug the controller */ - geometry_msgs::Twist update_with_limits(const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist, - ros::Duration dt, double* eda, double* progress, - path_tracking_pid::PidDebug* pid_debug); + geometry_msgs::Twist update_with_limits( + const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + ros::Duration dt, double * eda, double * progress, path_tracking_pid::PidDebug * pid_debug); /** * Perform prediction steps on the lateral error and return a reduced velocity that stays within bounds @@ -141,13 +145,15 @@ class Controller : private boost::noncopyable * @param odom_twist Robot odometry * @return Velocity command */ - double mpc_based_max_vel(double target_x_vel, const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist); + double mpc_based_max_vel( + double target_x_vel, const geometry_msgs::Transform & current_tf, + const geometry_msgs::Twist & odom_twist); /** * Set dynamic parameters for the PID controller * @param config */ - void configure(path_tracking_pid::PidConfig& config); + void configure(path_tracking_pid::PidConfig & config); /** * Set whether the controller is enabled @@ -167,24 +173,12 @@ class Controller : private boost::noncopyable path_tracking_pid::PidConfig getConfig(); // Inline get-functions for transforms - tf2::Transform getCurrentGoal() const - { - return current_goal_; - } - tf2::Transform getCurrentWithCarrot() const - { - return current_with_carrot_; - } - tf2::Transform getCurrentPosOnPlan() const - { - return current_pos_on_plan_; - } + tf2::Transform getCurrentGoal() const { return current_goal_; } + tf2::Transform getCurrentWithCarrot() const { return current_with_carrot_; } + tf2::Transform getCurrentPosOnPlan() const { return current_pos_on_plan_; } // Inline get-function for controller-state - ControllerState getControllerState() const - { - return controller_state_; - } + ControllerState getControllerState() const { return controller_state_; } // Set new vel_max_external value void setVelMaxExternal(double value); @@ -196,21 +190,25 @@ class Controller : private boost::noncopyable double getVelMaxObstacle() const; private: - void distToSegmentSquared(const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w, - tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w) const; + void distToSegmentSquared( + const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, + tf2::Transform & pose_projection, double & distance_to_p, double & distance_to_w) const; // Overloaded function for callers that don't need the additional results - double distToSegmentSquared(const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w) const + double distToSegmentSquared( + const tf2::Transform & pose_p, const tf2::Transform & pose_v, + const tf2::Transform & pose_w) const { tf2::Transform dummy_tf; double dummy_double; double result; - distToSegmentSquared(pose_p,pose_v, pose_w, dummy_tf, result, dummy_double); + distToSegmentSquared(pose_p, pose_v, pose_w, dummy_tf, result, dummy_double); return result; } geometry_msgs::Twist computeTricycleModelForwardKinematics(double x_vel, double steering_angle); - TricycleSteeringCmdVel computeTricycleModelInverseKinematics(const geometry_msgs::Twist& cmd_vel); + TricycleSteeringCmdVel computeTricycleModelInverseKinematics( + const geometry_msgs::Twist & cmd_vel); /** * Output some debug information about the current parameters */ @@ -220,8 +218,8 @@ class Controller : private boost::noncopyable ControllerState controller_state_; // Global Plan variables - std::vector global_plan_tf_; // Global plan vector - std::vector distance_to_goal_vector_; // Vector with distances to goal + std::vector global_plan_tf_; // Global plan vector + std::vector distance_to_goal_vector_; // Vector with distances to goal std::vector turning_radius_inv_vector_; // Vector with computed turning radius inverse double distance_to_goal_ = NAN; tf2::Transform current_goal_; @@ -249,7 +247,8 @@ class Controller : private boost::noncopyable std::array, 2> forward_kinematics_matrix_{}; // Velocity limits that can be active external to the pid controller: - double vel_max_external_ = INFINITY; // Dynamic external max velocity requirement (e.g. no more power available) + double vel_max_external_ = + INFINITY; // Dynamic external max velocity requirement (e.g. no more power available) double vel_max_obstacle_ = INFINITY; // Can be zero if lethal obstacles are detected }; diff --git a/include/path_tracking_pid/details/fifo_array.hpp b/include/path_tracking_pid/details/fifo_array.hpp index 178996e8..9ded1e1f 100644 --- a/include/path_tracking_pid/details/fifo_array.hpp +++ b/include/path_tracking_pid/details/fifo_array.hpp @@ -5,7 +5,6 @@ namespace path_tracking_pid::details { - // Fixed-size array-like FIFO buffer. All elements are value initialized upon construction. template class FifoArray @@ -13,27 +12,21 @@ class FifoArray public: // Pushes the given value to the front of the buffer (index = 0). All elements already in the buffer are moved // towards the end of the buffer (index = size - 1). The last element is removed from the buffer. - constexpr void push(const value_type& value) + constexpr void push(const value_type & value) { std::copy_backward(data_.begin(), std::prev(data_.end()), data_.end()); data_[0] = value; } // Value initializes all elements in the buffer. - constexpr void reset() - { - data_ = {}; - } + constexpr void reset() { data_ = {}; } // Read-only access to the element at the given index. - constexpr const value_type& operator[](std::size_t index) const - { - return data_[index]; - } + constexpr const value_type & operator[](std::size_t index) const { return data_[index]; } // Read-only access to the element at the given index (with compile-time range check). template - constexpr const value_type& at() const + constexpr const value_type & at() const { static_assert(index < size); return data_[index]; diff --git a/include/path_tracking_pid/details/second_order_lowpass.hpp b/include/path_tracking_pid/details/second_order_lowpass.hpp index 692ea60f..857dadcb 100644 --- a/include/path_tracking_pid/details/second_order_lowpass.hpp +++ b/include/path_tracking_pid/details/second_order_lowpass.hpp @@ -4,7 +4,6 @@ namespace path_tracking_pid::details { - // Error tracker for the last 3 error and filtered error values. class SecondOrderLowpass { @@ -17,10 +16,10 @@ class SecondOrderLowpass void reset(); // Read-only access to the errors FIFO buffer. - const FifoArray& errors() const; + const FifoArray & errors() const; // Read-only access to the filtered errors FIFO buffer. - const FifoArray& filtered_errors() const; + const FifoArray & filtered_errors() const; private: FifoArray errors_; diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 1d42ec78..6f8855f6 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -1,6 +1,5 @@ #pragma once - #include #include #include @@ -18,22 +17,23 @@ #include "nav_msgs/Odometry.h" #include "nav_msgs/Path.h" #include "path_tracking_pid/PidConfig.h" +#include "path_tracking_pid/visualization.hpp" #include "std_msgs/Bool.h" #include "std_msgs/Float64.h" #include "tf2_ros/buffer.h" -#include "path_tracking_pid/visualization.hpp" - BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x, y) namespace path_tracking_pid { -class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController, private boost::noncopyable +class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, + public mbf_costmap_core::CostmapController, + private boost::noncopyable { private: typedef boost::geometry::model::ring polygon_t; - static inline polygon_t union_(const polygon_t& polygon1, const polygon_t& polygon2) + static inline polygon_t union_(const polygon_t & polygon1, const polygon_t & polygon2) { std::vector output_vec; boost::geometry::union_(polygon1, polygon2, output_vec); @@ -47,14 +47,15 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co * @param tf a pointer to TransformListener in TF Buffer * @param costmap Costmap indicating free/occupied space */ - void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap) override; + void initialize( + std::string name, tf2_ros::Buffer * tf, costmap_2d::Costmap2DROS * costmap) override; /** * @brief Set the plan we should be following * @param global_plan Plan to follow as closely as we can * @return whether the plan was successfully updated or not */ - bool setPlan(const std::vector& global_plan) override; + bool setPlan(const std::vector & global_plan) override; /** * @brief Calculates the velocity command based on the current robot pose given by pose. See the interface in move @@ -62,7 +63,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co * @param cmd_vel Output the velocity command * @return true if succeded. */ - bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) override; // NOLINT + bool computeVelocityCommands(geometry_msgs::Twist & cmd_vel) override; // NOLINT /** * @brief Calculates the velocity command based on the current robot pose given by pose. The velocity @@ -73,8 +74,9 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co * @param message * @return a status code defined in the move base flex ExePath action. */ - uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, - geometry_msgs::TwistStamped& cmd_vel, std::string& message) override; // NOLINT + uint32_t computeVelocityCommands( + const geometry_msgs::PoseStamped & pose, const geometry_msgs::TwistStamped & velocity, + geometry_msgs::TwistStamped & cmd_vel, std::string & message) override; // NOLINT /** * @brief Returns true, if the goal is reached. Currently does not respect the parameters give @@ -99,24 +101,20 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co /** Enumeration for custom SUCCESS feedback codes. See default ones: * https://github.com/magazino/move_base_flex/blob/master/mbf_msgs/action/ExePath.action */ - enum class ComputeVelocityCommandsResult - { - SUCCESS = 0, - GRACEFULLY_CANCELLING = 1 - }; + enum class ComputeVelocityCommandsResult { SUCCESS = 0, GRACEFULLY_CANCELLING = 1 }; private: /** * Accept a new configuration for the PID controller * @param config the new configuration */ - void reconfigure_pid(path_tracking_pid::PidConfig& config); + void reconfigure_pid(path_tracking_pid::PidConfig & config); void pauseCallback(std_msgs::Bool pause); - void curOdomCallback(const nav_msgs::Odometry& odom_msg); + void curOdomCallback(const nav_msgs::Odometry & odom_msg); - void velMaxExternalCallback(const std_msgs::Float64& msg); + void velMaxExternalCallback(const std_msgs::Float64 & msg); uint8_t projectedCollisionCost(); @@ -129,7 +127,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co nav_msgs::Path received_path_; // Obstacle collision detection - costmap_2d::Costmap2DROS* costmap_ = nullptr; + costmap_2d::Costmap2DROS * costmap_ = nullptr; // Cancel flags (multi threaded, so atomic bools) std::atomic active_goal_{false}; @@ -140,7 +138,7 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, public mbf_co boost::recursive_mutex config_mutex_; std::unique_ptr> pid_server_; - tf2_ros::Buffer* tf_ = nullptr; + tf2_ros::Buffer * tf_ = nullptr; geometry_msgs::TransformStamped tfCurPoseStamped_; ros::Publisher debug_pub_; // Debugging of controller internal parameters diff --git a/include/path_tracking_pid/visualization.hpp b/include/path_tracking_pid/visualization.hpp index 09040e93..e563f9df 100644 --- a/include/path_tracking_pid/visualization.hpp +++ b/include/path_tracking_pid/visualization.hpp @@ -19,10 +19,10 @@ class Visualization private: void publishSphere( - const std_msgs::Header & header, const std::string& ns, int id, const tf2::Transform & pose, + const std_msgs::Header & header, const std::string & ns, int id, const tf2::Transform & pose, const std_msgs::ColorRGBA & color); void publishSphere( - const std_msgs::Header & header, const std::string& ns, int id, geometry_msgs::Pose pose, + const std_msgs::Header & header, const std::string & ns, int id, geometry_msgs::Pose pose, const std_msgs::ColorRGBA & color); ros::Publisher marker_pub_; diff --git a/src/common.hpp b/src/common.hpp index cc63d240..114cf44b 100644 --- a/src/common.hpp +++ b/src/common.hpp @@ -4,7 +4,6 @@ namespace path_tracking_pid { - inline constexpr double VELOCITY_EPS = 1e-3; // Neglegible velocity // Converts an enumeration to its underlying type. diff --git a/src/controller.cpp b/src/controller.cpp index 3a27961c..4013d3e0 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -4,19 +4,17 @@ #include "path_tracking_pid/controller.hpp" -#include "common.hpp" - #include #include #include "angles/angles.h" +#include "common.hpp" #include "tf2/utils.h" namespace path_tracking_pid { - -namespace { - +namespace +{ constexpr double RADIUS_EPS = 0.001; // Smallest relevant radius [m] constexpr double LONG_DURATION = 31556926; // A year (ros::Duration cannot be inf) @@ -30,7 +28,6 @@ constexpr double ang_lower_limit = -100.0; // Anti-windup term. Limits the absolute value of the integral term. constexpr double windup_limit = 1000.0; - // Indicates if both values have the same sign. template bool have_same_sign(T val1, T val2) @@ -55,8 +52,7 @@ double distSquared(const tf2::Transform & a, const tf2::Transform & b) // Return the square distance between two points. double distSquared(const geometry_msgs::Pose & a, const geometry_msgs::Pose & b) { - return std::pow(a.position.x - b.position.x, 2) + - std::pow(a.position.y - b.position.y, 2); + return std::pow(a.position.x - b.position.x, 2) + std::pow(a.position.y - b.position.y, 2); } // Indicates if the angle of the cur pose is obtuse (with respect to the prev and next poses). @@ -67,7 +63,7 @@ bool is_pose_angle_obtuse( return distSquared(prev, next) > (distSquared(prev, cur) + distSquared(cur, next)); } -} // namespace anonymous +} // namespace void Controller::setHolonomic(bool holonomic) { @@ -82,7 +78,8 @@ void Controller::setEstimatePoseAngle(bool estimate_pose_angle) estimate_pose_angle_enabled_ = estimate_pose_angle; } -void Controller::setTricycleModel(bool tricycle_model_enabled, const geometry_msgs::Transform& tf_base_to_steered_wheel) +void Controller::setTricycleModel( + bool tricycle_model_enabled, const geometry_msgs::Transform & tf_base_to_steered_wheel) { // Set tricycle model use_tricycle_model_ = tricycle_model_enabled; @@ -93,58 +90,60 @@ void Controller::setTricycleModel(bool tricycle_model_enabled, const geometry_ms const double distance_base_to_steered_wheel = hypot(wheel_x, wheel_y); const double wheel_theta = atan2(wheel_y, wheel_x); inverse_kinematics_matrix_[0][0] = 1; - inverse_kinematics_matrix_[0][1] = - distance_base_to_steered_wheel * sin(wheel_theta); + inverse_kinematics_matrix_[0][1] = -distance_base_to_steered_wheel * sin(wheel_theta); inverse_kinematics_matrix_[1][0] = 0; - inverse_kinematics_matrix_[1][1] = - distance_base_to_steered_wheel * cos(wheel_theta); - + inverse_kinematics_matrix_[1][1] = -distance_base_to_steered_wheel * cos(wheel_theta); - const double determinant = inverse_kinematics_matrix_[0][0] * inverse_kinematics_matrix_[1][1] - - inverse_kinematics_matrix_[0][1] * inverse_kinematics_matrix_[1][0]; + const double determinant = inverse_kinematics_matrix_[0][0] * inverse_kinematics_matrix_[1][1] - + inverse_kinematics_matrix_[0][1] * inverse_kinematics_matrix_[1][0]; - if (determinant == 0) - { + if (determinant == 0) { ROS_ERROR("Steered wheel at base_link. Invalid for tricycle model, it will be disabled."); use_tricycle_model_ = false; return; } - forward_kinematics_matrix_[0][0] = inverse_kinematics_matrix_[1][1] / determinant; + forward_kinematics_matrix_[0][0] = inverse_kinematics_matrix_[1][1] / determinant; forward_kinematics_matrix_[0][1] = -inverse_kinematics_matrix_[0][1] / determinant; forward_kinematics_matrix_[1][0] = -inverse_kinematics_matrix_[1][0] / determinant; - forward_kinematics_matrix_[1][1] = inverse_kinematics_matrix_[0][0] / determinant; + forward_kinematics_matrix_[1][1] = inverse_kinematics_matrix_[0][0] / determinant; controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel_.rotation); } -geometry_msgs::Twist Controller::computeTricycleModelForwardKinematics(double x_vel, double steering_angle) +geometry_msgs::Twist Controller::computeTricycleModelForwardKinematics( + double x_vel, double steering_angle) { - geometry_msgs::Twist estimated_base_twist; - const double x_alpha = x_vel*cos(steering_angle); - const double y_alpha = x_vel*sin(steering_angle); + geometry_msgs::Twist estimated_base_twist; + const double x_alpha = x_vel * cos(steering_angle); + const double y_alpha = x_vel * sin(steering_angle); - estimated_base_twist.linear.x = forward_kinematics_matrix_[0][0]*x_alpha + forward_kinematics_matrix_[0][1]*y_alpha; - estimated_base_twist.angular.z = - forward_kinematics_matrix_[1][0]*x_alpha + forward_kinematics_matrix_[1][1]*y_alpha; + estimated_base_twist.linear.x = + forward_kinematics_matrix_[0][0] * x_alpha + forward_kinematics_matrix_[0][1] * y_alpha; + estimated_base_twist.angular.z = + forward_kinematics_matrix_[1][0] * x_alpha + forward_kinematics_matrix_[1][1] * y_alpha; - return estimated_base_twist; + return estimated_base_twist; } -TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics(const geometry_msgs::Twist& cmd_vel) +TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics( + const geometry_msgs::Twist & cmd_vel) { - TricycleSteeringCmdVel steering_cmd_vel; - const double x_alpha = inverse_kinematics_matrix_[0][0]*cmd_vel.linear.x - + inverse_kinematics_matrix_[0][1]*cmd_vel.angular.z; - const double y_alpha = inverse_kinematics_matrix_[1][0]*cmd_vel.linear.x - + inverse_kinematics_matrix_[1][1]*cmd_vel.angular.z; + TricycleSteeringCmdVel steering_cmd_vel; + const double x_alpha = inverse_kinematics_matrix_[0][0] * cmd_vel.linear.x + + inverse_kinematics_matrix_[0][1] * cmd_vel.angular.z; + const double y_alpha = inverse_kinematics_matrix_[1][0] * cmd_vel.linear.x + + inverse_kinematics_matrix_[1][1] * cmd_vel.angular.z; - steering_cmd_vel.steering_angle = atan2(y_alpha, x_alpha); - steering_cmd_vel.speed = hypot(x_alpha, y_alpha); + steering_cmd_vel.steering_angle = atan2(y_alpha, x_alpha); + steering_cmd_vel.speed = hypot(x_alpha, y_alpha); - return steering_cmd_vel; + return steering_cmd_vel; } -void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist, - const std::vector& global_plan) +void Controller::setPlan( + const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const std::vector & global_plan) { ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%d)", (int)global_plan.size()); ROS_DEBUG("Plan is defined in frame '%s'", global_plan.at(0).header.frame_id.c_str()); @@ -158,29 +157,27 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome // For now do not allow repeated points or in-place rotation // To allow that the way the progress is checked and the interpolation is done needs to be changed // Also check if points suddenly go in the opposite direction, this could lead to deadlocks - for (int pose_idx = 1; pose_idx < static_cast(global_plan.size()) - 1; ++pose_idx) - { + for (int pose_idx = 1; pose_idx < static_cast(global_plan.size()) - 1; ++pose_idx) { const auto prev_pose = global_plan[pose_idx - 1].pose; const auto pose = global_plan[pose_idx].pose; const auto next_pose = global_plan[pose_idx + 1].pose; - if (is_pose_angle_obtuse(prev_pose, pose, next_pose)) - { + if (is_pose_angle_obtuse(prev_pose, pose, next_pose)) { global_plan_tf_.push_back(to_transform(pose)); - } - else - { - ROS_WARN("Pose %i of path is not used since it is not in the expected direction of the path!", pose_idx); + } else { + ROS_WARN( + "Pose %i of path is not used since it is not in the expected direction of the path!", + pose_idx); } } // Add last pose as we didn't evaluate that one const auto last_transform = to_transform(global_plan.back().pose); global_plan_tf_.push_back(last_transform); - if (!config_.track_base_link) - { + if (!config_.track_base_link) { // Add carrot length to plan using goal pose (we assume the last pose contains correct angle) - tf2::Transform carrotTF(tf2::Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), - tf2::Vector3(config_.l, 0.0, 0.0)); + tf2::Transform carrotTF( + tf2::Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), + tf2::Vector3(config_.l, 0.0, 0.0)); global_plan_tf_.push_back(last_transform * carrotTF); } @@ -201,14 +198,13 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome tf2::Transform deltaPlan; // We define segment0 to be the segment connecting pose0 and pose1. // Hence, when picking the starting path's pose, we mean to start at the segment connecting that and the next pose. - for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) - { + for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ - dist_to_segment = distToSegmentSquared(current_tf2, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]); + dist_to_segment = + distToSegmentSquared(current_tf2, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! - if (dist_to_segment < minimum_distance_to_path) - { + if (dist_to_segment < minimum_distance_to_path) { minimum_distance_to_path = dist_to_segment; controller_state_.current_global_plan_index = idx_path; } @@ -222,22 +218,18 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome // compute turning radius based on trigonometric analysis // radius such that next pose is connected from current pose with a semi-circle const double dpXY2 = dpY * dpY + dpX * dpX; - if (dpXY2 < FLT_EPSILON) - { + if (dpXY2 < FLT_EPSILON) { turning_radius_inv_vector_[idx_path] = std::numeric_limits::infinity(); - } - else - { + } else { // 0.5*dpY*( 1 + dpX*dpX/(dpY*dPY) ); // turning_radius_vector[idx_path] = 0.5*(1/dpY)*( dpY*dpY + dpX*dpX ); - turning_radius_inv_vector_[idx_path] = 2*dpY/dpXY2; + turning_radius_inv_vector_[idx_path] = 2 * dpY / dpXY2; } ROS_DEBUG("turning_radius_inv_vector[%d] = %f", idx_path, turning_radius_inv_vector_[idx_path]); } // Set initial velocity - switch (config_.init_vel_method) - { + switch (config_.init_vel_method) { case Pid_Zero: reset(); break; @@ -245,7 +237,8 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome reset(); controller_state_.current_x_vel = odom_twist.linear.x; controller_state_.current_yaw_vel = odom_twist.angular.z; - ROS_INFO("Resuming on odom velocity x: %f, yaw: %f", odom_twist.linear.x, odom_twist.angular.z); + ROS_INFO( + "Resuming on odom velocity x: %f, yaw: %f", odom_twist.linear.x, odom_twist.angular.z); break; default: ROS_DEBUG("Internal controller_state stays valid"); @@ -253,19 +246,21 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome } // When velocity error is too big reset current_x_vel - if (fabs(odom_twist.linear.x - controller_state_.current_x_vel) > config_.max_error_x_vel) - { + if (fabs(odom_twist.linear.x - controller_state_.current_x_vel) > config_.max_error_x_vel) { // TODO(clopez/mcfurry/nobleo): Give feedback to higher level software here - ROS_WARN("Large control error. Current_x_vel %f / odometry %f", - controller_state_.current_x_vel, odom_twist.linear.x); + ROS_WARN( + "Large control error. Current_x_vel %f / odometry %f", controller_state_.current_x_vel, + odom_twist.linear.x); } controller_state_.end_phase_enabled = false; controller_state_.end_reached = false; } -void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geometry_msgs::Twist& odom_twist, - const geometry_msgs::Transform& tf_base_to_steered_wheel, const geometry_msgs::Twist& /* steering_odom_twist */, - const std::vector& global_plan) +void Controller::setPlan( + const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const geometry_msgs::Transform & tf_base_to_steered_wheel, + const geometry_msgs::Twist & /* steering_odom_twist */, + const std::vector & global_plan) { setPlan(current_tf, odom_twist, global_plan); controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel.rotation); @@ -273,36 +268,35 @@ void Controller::setPlan(const geometry_msgs::Transform& current_tf, const geome } void Controller::distToSegmentSquared( - const tf2::Transform& pose_p, const tf2::Transform& pose_v, const tf2::Transform& pose_w, - tf2::Transform& pose_projection, double& distance_to_p, double& distance_to_w) const + const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, + tf2::Transform & pose_projection, double & distance_to_p, double & distance_to_w) const { const double l2 = distSquared(pose_v, pose_w); - if (l2 == 0) - { + if (l2 == 0) { pose_projection = pose_w; distance_to_w = 0.0; distance_to_p = distSquared(pose_p, pose_w); - } - else - { - double t = ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) * (pose_w.getOrigin().x() - pose_v.getOrigin().x()) + - (pose_p.getOrigin().y() - pose_v.getOrigin().y()) * (pose_w.getOrigin().y() - pose_v.getOrigin().y())) / + } else { + double t = ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) * + (pose_w.getOrigin().x() - pose_v.getOrigin().x()) + + (pose_p.getOrigin().y() - pose_v.getOrigin().y()) * + (pose_w.getOrigin().y() - pose_v.getOrigin().y())) / l2; t = fmax(0.0, fmin(1.0, t)); - pose_projection.setOrigin( - tf2::Vector3(pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()), - pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); + pose_projection.setOrigin(tf2::Vector3( + pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()), + pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); double yaw_projection = tf2::getYaw(pose_v.getRotation()); // get yaw of the first vector + t * - // (tf2::getYaw(pose_w.getRotation()) - - // tf2::getYaw(pose_v.getRotation())); + // (tf2::getYaw(pose_w.getRotation()) - + // tf2::getYaw(pose_v.getRotation())); tf2::Quaternion pose_quaternion; - if (estimate_pose_angle_enabled_) - { - pose_quaternion.setRPY(0.0, 0.0, atan2(pose_w.getOrigin().y() - pose_v.getOrigin().y(), - pose_w.getOrigin().x() - pose_v.getOrigin().x())); - } - else - { + if (estimate_pose_angle_enabled_) { + pose_quaternion.setRPY( + 0.0, 0.0, + atan2( + pose_w.getOrigin().y() - pose_v.getOrigin().y(), + pose_w.getOrigin().x() - pose_v.getOrigin().x())); + } else { pose_quaternion.setRPY(0.0, 0.0, yaw_projection); } @@ -312,9 +306,9 @@ void Controller::distToSegmentSquared( } } -tf2::Transform Controller::findPositionOnPlan(const geometry_msgs::Transform& current_tf, - ControllerState* controller_state_ptr, - size_t &path_pose_idx) +tf2::Transform Controller::findPositionOnPlan( + const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr, + size_t & path_pose_idx) { tf2::Transform current_tf2; tf2::convert(current_tf, current_tf2); @@ -335,43 +329,38 @@ tf2::Transform Controller::findPositionOnPlan(const geometry_msgs::Transform& cu // Hence, when idx_path==i we are currently tracking the section connection pose i and pose i+1 // First look in current position and in front - for (auto idx_path = controller_state_ptr->current_global_plan_index; idx_path < global_plan_tf_.size(); idx_path++) - { + for (auto idx_path = controller_state_ptr->current_global_plan_index; + idx_path < global_plan_tf_.size(); idx_path++) { error = current_tf2.inverseTimes(global_plan_tf_[idx_path]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! distance_to_path = hypot(error.getOrigin().x(), error.getOrigin().y(), error.getOrigin().z()); - if (distance_to_path <= minimum_distance_to_path) - { + if (distance_to_path <= minimum_distance_to_path) { minimum_distance_to_path = distance_to_path; controller_state_ptr->current_global_plan_index = idx_path; - } - else - { + } else { break; } } // Then look backwards - for (auto idx_path = controller_state_ptr->current_global_plan_index; idx_path > 0; --idx_path) - { + for (auto idx_path = controller_state_ptr->current_global_plan_index; idx_path > 0; --idx_path) { error = current_tf2.inverseTimes(global_plan_tf_[idx_path - 1]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! distance_to_path = hypot(error.getOrigin().x(), error.getOrigin().y(), error.getOrigin().z()); - if (distance_to_path < minimum_distance_to_path) - { + if (distance_to_path < minimum_distance_to_path) { minimum_distance_to_path = distance_to_path; controller_state_ptr->current_global_plan_index = idx_path - 1; - } - else - { + } else { break; } } - ROS_DEBUG("progress: %lu of %lu", controller_state_ptr->current_global_plan_index, global_plan_tf_.size()-1); + ROS_DEBUG( + "progress: %lu of %lu", controller_state_ptr->current_global_plan_index, + global_plan_tf_.size() - 1); // To finalize, compute the indexes of the start and end points of // the closest line segment to the current carrot tf2::Transform current_goal_local; @@ -383,59 +372,57 @@ tf2::Transform Controller::findPositionOnPlan(const geometry_msgs::Transform& cu double distance2_to_line_behind; double distance_to_end_line_ahead; double distance_to_end_line_behind; - if (controller_state_ptr->current_global_plan_index == 0) - { - distToSegmentSquared(current_tf2, global_plan_tf_[0], global_plan_tf_[1], - pose_projection_ahead, distance2_to_line_ahead, distance_to_end_line_ahead); + if (controller_state_ptr->current_global_plan_index == 0) { + distToSegmentSquared( + current_tf2, global_plan_tf_[0], global_plan_tf_[1], pose_projection_ahead, + distance2_to_line_ahead, distance_to_end_line_ahead); current_goal_local = pose_projection_ahead; distance_to_goal_ = distance_to_goal_vector_[1] + distance_to_end_line_ahead; controller_state_ptr->last_visited_pose_index = 0; path_pose_idx = controller_state_ptr->current_global_plan_index; - } - else if (controller_state_ptr->current_global_plan_index == global_plan_tf_.size() - 1) - { - distToSegmentSquared(current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], - global_plan_tf_[controller_state_ptr->current_global_plan_index], - pose_projection_behind, distance2_to_line_behind, distance_to_end_line_behind); + } else if (controller_state_ptr->current_global_plan_index == global_plan_tf_.size() - 1) { + distToSegmentSquared( + current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], + global_plan_tf_[controller_state_ptr->current_global_plan_index], pose_projection_behind, + distance2_to_line_behind, distance_to_end_line_behind); current_goal_local = pose_projection_behind; distance_to_goal_ = distance_to_end_line_behind; controller_state_ptr->last_visited_pose_index = global_plan_tf_.size() - 2; path_pose_idx = controller_state_ptr->current_global_plan_index - 1; - } - else - { - distToSegmentSquared(current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index], - global_plan_tf_[controller_state_ptr->current_global_plan_index + 1], - pose_projection_ahead, distance2_to_line_ahead, distance_to_end_line_ahead); - distToSegmentSquared(current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], - global_plan_tf_[controller_state_ptr->current_global_plan_index], - pose_projection_behind, distance2_to_line_behind, distance_to_end_line_behind); - - if (distance2_to_line_ahead < distance2_to_line_behind) - { + } else { + distToSegmentSquared( + current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index], + global_plan_tf_[controller_state_ptr->current_global_plan_index + 1], pose_projection_ahead, + distance2_to_line_ahead, distance_to_end_line_ahead); + distToSegmentSquared( + current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], + global_plan_tf_[controller_state_ptr->current_global_plan_index], pose_projection_behind, + distance2_to_line_behind, distance_to_end_line_behind); + + if (distance2_to_line_ahead < distance2_to_line_behind) { current_goal_local = pose_projection_ahead; distance_to_goal_ = - distance_to_goal_vector_[controller_state_ptr->current_global_plan_index + 1] + distance_to_end_line_ahead; - controller_state_ptr->last_visited_pose_index = controller_state_ptr->current_global_plan_index; - } - else - { + distance_to_goal_vector_[controller_state_ptr->current_global_plan_index + 1] + + distance_to_end_line_ahead; + controller_state_ptr->last_visited_pose_index = + controller_state_ptr->current_global_plan_index; + } else { current_goal_local = pose_projection_behind; distance_to_goal_ = - distance_to_goal_vector_[controller_state_ptr->current_global_plan_index] + distance_to_end_line_behind; - controller_state_ptr->last_visited_pose_index = controller_state_ptr->current_global_plan_index - 1; + distance_to_goal_vector_[controller_state_ptr->current_global_plan_index] + + distance_to_end_line_behind; + controller_state_ptr->last_visited_pose_index = + controller_state_ptr->current_global_plan_index - 1; } path_pose_idx = controller_state_ptr->current_global_plan_index; } return current_goal_local; } -geometry_msgs::Twist Controller::update(double target_x_vel, - double target_end_x_vel, - const geometry_msgs::Transform& current_tf, - const geometry_msgs::Twist& odom_twist, - ros::Duration dt, - double* eda, double* progress, path_tracking_pid::PidDebug* pid_debug) +geometry_msgs::Twist Controller::update( + double target_x_vel, double target_end_x_vel, const geometry_msgs::Transform & current_tf, + const geometry_msgs::Twist & odom_twist, ros::Duration dt, double * eda, double * progress, + path_tracking_pid::PidDebug * pid_debug) { const double current_x_vel = controller_state_.current_x_vel; const double current_yaw_vel = controller_state_.current_yaw_vel; @@ -448,14 +435,15 @@ geometry_msgs::Twist Controller::update(double target_x_vel, current_with_carrot_origin.setZ(0); current_with_carrot_.setOrigin(current_with_carrot_origin); - tf2::Quaternion cur_rot(current_tf.rotation.x, current_tf.rotation.y, current_tf.rotation.z, current_tf.rotation.w); + tf2::Quaternion cur_rot( + current_tf.rotation.x, current_tf.rotation.y, current_tf.rotation.z, current_tf.rotation.w); current_with_carrot_.setRotation(cur_rot); size_t path_pose_idx; - if (config_.track_base_link) - { + if (config_.track_base_link) { // Find closes robot position to path and then project carrot on goal - current_pos_on_plan_ = current_goal_ = findPositionOnPlan(current_tf, &controller_state_, path_pose_idx); + current_pos_on_plan_ = current_goal_ = + findPositionOnPlan(current_tf, &controller_state_, path_pose_idx); // To track the base link the goal is then transform to the control point goal double theda_rp = tf2::getYaw(current_goal_.getRotation()); tf2::Vector3 newControlOrigin; @@ -463,13 +451,12 @@ geometry_msgs::Twist Controller::update(double target_x_vel, newControlOrigin.setY(current_goal_.getOrigin().y() + config_.l * sin(theda_rp)); newControlOrigin.setZ(0); current_goal_.setOrigin(newControlOrigin); - } - else - { + } else { // find position of current position with projected carrot geometry_msgs::Transform current_with_carrot_g; tf2::convert(current_with_carrot_, current_with_carrot_g); - current_pos_on_plan_ = current_goal_ = findPositionOnPlan(current_with_carrot_g, &controller_state_, path_pose_idx); + current_pos_on_plan_ = current_goal_ = + findPositionOnPlan(current_with_carrot_g, &controller_state_, path_pose_idx); } *progress = 1.0 - distance_to_goal_ / distance_to_goal_vector_[0]; @@ -479,12 +466,14 @@ geometry_msgs::Twist Controller::update(double target_x_vel, //***** Feedback control *****// if (!((config_.Kp_lat <= 0. && config_.Ki_lat <= 0. && config_.Kd_lat <= 0.) || - (config_.Kp_lat >= 0. && config_.Ki_lat >= 0. && config_.Kd_lat >= 0.))) // All 3 gains should have the same sign + (config_.Kp_lat >= 0. && config_.Ki_lat >= 0. && + config_.Kd_lat >= 0.))) // All 3 gains should have the same sign { ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); } if (!((config_.Kp_ang <= 0. && config_.Ki_ang <= 0. && config_.Kd_ang <= 0.) || - (config_.Kp_ang >= 0. && config_.Ki_ang >= 0. && config_.Kd_ang >= 0.))) // All 3 gains should have the same sign + (config_.Kp_ang >= 0. && config_.Ki_ang >= 0. && + config_.Kd_ang >= 0.))) // All 3 gains should have the same sign { ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); } @@ -495,17 +484,22 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // tracking error for diagnostic purposes // Transform current pose into local-path-frame to get tracked-frame-error tf2::Quaternion path_quat; - path_quat.setEuler(0.0, 0.0, - atan2(global_plan_tf_[path_pose_idx+1].getOrigin().y() - global_plan_tf_[path_pose_idx].getOrigin().y(), - global_plan_tf_[path_pose_idx+1].getOrigin().x() - global_plan_tf_[path_pose_idx].getOrigin().x())); - tf2::Transform path_segmen_tf = tf2::Transform(path_quat, - tf2::Vector3(global_plan_tf_[path_pose_idx].getOrigin().x(), - global_plan_tf_[path_pose_idx].getOrigin().y(), - global_plan_tf_[path_pose_idx].getOrigin().z())); - - tf2::Vector3 current_tracking_err = - (path_segmen_tf.inverse() * tf2::Vector3(current_tf.translation.x, - current_tf.translation.y, - current_tf.translation.z)); + path_quat.setEuler( + 0.0, 0.0, + atan2( + global_plan_tf_[path_pose_idx + 1].getOrigin().y() - + global_plan_tf_[path_pose_idx].getOrigin().y(), + global_plan_tf_[path_pose_idx + 1].getOrigin().x() - + global_plan_tf_[path_pose_idx].getOrigin().x())); + tf2::Transform path_segmen_tf = tf2::Transform( + path_quat, tf2::Vector3( + global_plan_tf_[path_pose_idx].getOrigin().x(), + global_plan_tf_[path_pose_idx].getOrigin().y(), + global_plan_tf_[path_pose_idx].getOrigin().z())); + + tf2::Vector3 current_tracking_err = + -(path_segmen_tf.inverse() * + tf2::Vector3(current_tf.translation.x, current_tf.translation.y, current_tf.translation.z)); // trackin_error here represents the error between tracked link and position on plan controller_state_.tracking_error_lat = current_tracking_err.y(); @@ -516,24 +510,31 @@ geometry_msgs::Twist Controller::update(double target_x_vel, controller_state_.error_integral_ang += controller_state_.error_ang.errors().at<0>() * dt.toSec(); // Apply windup limit to limit the size of the integral term - controller_state_.error_integral_lat = std::clamp(controller_state_.error_integral_lat, -windup_limit, windup_limit); - controller_state_.error_integral_ang = std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit); + controller_state_.error_integral_lat = + std::clamp(controller_state_.error_integral_lat, -windup_limit, windup_limit); + controller_state_.error_integral_ang = + std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit); // Take derivative of error, first the raw unfiltered data: controller_state_.error_deriv_lat.push( - (controller_state_.error_lat.errors().at<0>() - controller_state_.error_lat.errors().at<1>()) / dt.toSec()); + (controller_state_.error_lat.errors().at<0>() - controller_state_.error_lat.errors().at<1>()) / + dt.toSec()); controller_state_.error_deriv_ang.push( - (controller_state_.error_ang.errors().at<0>() - controller_state_.error_ang.errors().at<1>()) / dt.toSec()); + (controller_state_.error_ang.errors().at<0>() - controller_state_.error_ang.errors().at<1>()) / + dt.toSec()); // calculate the control effort - const auto proportional_lat = config_.Kp_lat * controller_state_.error_lat.filtered_errors().at<0>(); + const auto proportional_lat = + config_.Kp_lat * controller_state_.error_lat.filtered_errors().at<0>(); const auto integral_lat = config_.Ki_lat * controller_state_.error_integral_lat; - const auto derivative_lat = config_.Kd_lat * controller_state_.error_deriv_lat.filtered_errors().at<0>(); + const auto derivative_lat = + config_.Kd_lat * controller_state_.error_deriv_lat.filtered_errors().at<0>(); - const auto proportional_ang = config_.Kp_ang * controller_state_.error_ang.filtered_errors().at<0>(); + const auto proportional_ang = + config_.Kp_ang * controller_state_.error_ang.filtered_errors().at<0>(); const auto integral_ang = config_.Ki_ang * controller_state_.error_integral_ang; - const auto derivative_ang = config_.Kd_ang * controller_state_.error_deriv_ang.filtered_errors().at<0>(); - + const auto derivative_ang = + config_.Kd_ang * controller_state_.error_deriv_ang.filtered_errors().at<0>(); /***** Compute forward velocity *****/ // Apply acceleration limits and end velocity @@ -546,20 +547,18 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // to de-accelerate and thus avoid abrupt velocity changes at the end of the trajectory // The sample time plays an important role on how good these estimates are. // Thus We add a distance to the end phase distance estimation depending on the sample time - if ((current_target_x_vel_ > 0.0 && current_x_vel > target_end_x_vel) || - (current_target_x_vel_ < 0.0 && current_x_vel < target_end_x_vel)) - { + if ( + (current_target_x_vel_ > 0.0 && current_x_vel > target_end_x_vel) || + (current_target_x_vel_ < 0.0 && current_x_vel < target_end_x_vel)) { t_end_phase_current = (target_end_x_vel - current_x_vel) / (-config_.target_x_decc); - d_end_phase = current_x_vel * t_end_phase_current - - 0.5 * (config_.target_x_decc) * t_end_phase_current * t_end_phase_current - + target_x_vel * 2.0 * dt.toSec(); - } - else - { + d_end_phase = current_x_vel * t_end_phase_current - + 0.5 * (config_.target_x_decc) * t_end_phase_current * t_end_phase_current + + target_x_vel * 2.0 * dt.toSec(); + } else { t_end_phase_current = (target_end_x_vel - current_x_vel) / (config_.target_x_acc); - d_end_phase = current_x_vel * t_end_phase_current - + 0.5 * (config_.target_x_acc) * t_end_phase_current * t_end_phase_current - + target_x_vel * 2.0 * dt.toSec(); + d_end_phase = current_x_vel * t_end_phase_current + + 0.5 * (config_.target_x_acc) * t_end_phase_current * t_end_phase_current + + target_x_vel * 2.0 * dt.toSec(); } ROS_DEBUG("t_end_phase_current: %f", t_end_phase_current); ROS_DEBUG("d_end_phase: %f", d_end_phase); @@ -574,54 +573,39 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // If we are as close to our goal or closer then we need to reach end velocity, enable end_phase. // However, if robot is not facing to the same direction as the local velocity target vector, don't enable end_phase. // This is to avoid skipping paths that start with opposite velocity. - if ((distance_to_goal_ <= fabs(d_end_phase)) && have_same_sign(target_x_vel, angle_to_goal)) - { + if ((distance_to_goal_ <= fabs(d_end_phase)) && have_same_sign(target_x_vel, angle_to_goal)) { // This state will be remebered to avoid jittering on target_x_vel controller_state_.end_phase_enabled = true; } - if (controller_state_.end_phase_enabled && fabs(target_x_vel) > VELOCITY_EPS) - { + if (controller_state_.end_phase_enabled && fabs(target_x_vel) > VELOCITY_EPS) { current_target_x_vel_ = target_end_x_vel; - } - else - { + } else { controller_state_.end_phase_enabled = false; current_target_x_vel_ = target_x_vel; } // Determine if we need to accelerate, decelerate or maintain speed - double current_target_acc = 0; // Assume maintaining speed + double current_target_acc = 0; // Assume maintaining speed if (fabs(current_target_x_vel_) <= VELOCITY_EPS) // Zero velocity requested { - if (current_x_vel > current_target_x_vel_) - { + if (current_x_vel > current_target_x_vel_) { current_target_acc = -config_.target_x_decc; - } - else - { + } else { current_target_acc = config_.target_x_decc; } - } - else if (current_target_x_vel_ > 0) // Positive velocity requested + } else if (current_target_x_vel_ > 0) // Positive velocity requested { - if (current_x_vel > current_target_x_vel_) - { + if (current_x_vel > current_target_x_vel_) { current_target_acc = -config_.target_x_decc; - } - else - { + } else { current_target_acc = config_.target_x_acc; } - } - else // Negative velocity requested + } else // Negative velocity requested { - if (current_x_vel > current_target_x_vel_) - { + if (current_x_vel > current_target_x_vel_) { current_target_acc = -config_.target_x_acc; - } - else - { + } else { current_target_acc = config_.target_x_decc; } } @@ -634,88 +618,77 @@ geometry_msgs::Twist Controller::update(double target_x_vel, // For low target_end_x_vel we have a minimum velocity to ensure the goal is reached double min_vel = copysign(1.0, config_.l) * config_.abs_minimum_x_vel; - if (!controller_state_.end_reached && controller_state_.end_phase_enabled - && fabs(target_end_x_vel) <= fabs(min_vel) + VELOCITY_EPS - && fabs(new_x_vel) <= fabs(min_vel) + VELOCITY_EPS) - { + if ( + !controller_state_.end_reached && controller_state_.end_phase_enabled && + fabs(target_end_x_vel) <= fabs(min_vel) + VELOCITY_EPS && + fabs(new_x_vel) <= fabs(min_vel) + VELOCITY_EPS) { new_x_vel = min_vel; } // When velocity error is too big reset current_x_vel - if (fabs(odom_twist.linear.x) < fabs(current_target_x_vel_) && - fabs(odom_twist.linear.x - new_x_vel) > config_.max_error_x_vel) - { + if ( + fabs(odom_twist.linear.x) < fabs(current_target_x_vel_) && + fabs(odom_twist.linear.x - new_x_vel) > config_.max_error_x_vel) { // TODO(clopez/mcfurry/nobleo): Give feedback to higher level software here - ROS_WARN_THROTTLE(1.0, "Large tracking error. Current_x_vel %f / odometry %f", - new_x_vel, odom_twist.linear.x); + ROS_WARN_THROTTLE( + 1.0, "Large tracking error. Current_x_vel %f / odometry %f", new_x_vel, odom_twist.linear.x); } // Force target_end_x_vel at the very end of the path // Or when the end velocity is reached. // Warning! If target_end_x_vel == 0 and min_vel = 0 then the robot might not reach end pose - if ((distance_to_goal_ == 0.0 && target_end_x_vel >= VELOCITY_EPS) - || (controller_state_.end_phase_enabled && new_x_vel >= target_end_x_vel - VELOCITY_EPS - && new_x_vel <= target_end_x_vel + VELOCITY_EPS)) - { + if ( + (distance_to_goal_ == 0.0 && target_end_x_vel >= VELOCITY_EPS) || + (controller_state_.end_phase_enabled && new_x_vel >= target_end_x_vel - VELOCITY_EPS && + new_x_vel <= target_end_x_vel + VELOCITY_EPS)) { controller_state_.end_reached = true; controller_state_.end_phase_enabled = false; *progress = 1.0; *eda = 0.0; enabled_ = false; - } - else - { + } else { controller_state_.end_reached = false; // eda (Estimated duration of arrival) estimation - if (fabs(target_x_vel) > VELOCITY_EPS) - { - const double t_const = (copysign(distance_to_goal_, target_x_vel) - d_end_phase) / target_x_vel; + if (fabs(target_x_vel) > VELOCITY_EPS) { + const double t_const = + (copysign(distance_to_goal_, target_x_vel) - d_end_phase) / target_x_vel; *eda = fmin(fmax(t_end_phase_current, 0.0) + fmax(t_const, 0.0), LONG_DURATION); - } - else - { + } else { *eda = LONG_DURATION; } } /******* end calculation of forward velocity ********/ - //***** Overall control *****// // Controller logic && overall control effort control_effort_long_ = new_x_vel; control_effort_lat_ = 0.0; control_effort_ang_ = 0.0; - if (config_.feedback_lat) - { + if (config_.feedback_lat) { control_effort_lat_ = proportional_lat + integral_lat + derivative_lat; } - if (config_.feedback_ang) - { + if (config_.feedback_ang) { control_effort_ang_ = proportional_ang + integral_ang + derivative_ang; } //***** Feedforward control *****// - if (config_.feedforward_lat) - { + if (config_.feedforward_lat) { feedforward_lat_ = 0.0; // Not implemented control_effort_lat_ = control_effort_lat_ + feedforward_lat_; - } - else - { + } else { feedforward_lat_ = 0.0; } - if (config_.feedforward_ang) - { - feedforward_ang_ = turning_radius_inv_vector_[controller_state_.last_visited_pose_index]*control_effort_long_; - ROS_DEBUG("turning_radius_inv_vector[%lu] = %f", - controller_state_.last_visited_pose_index, turning_radius_inv_vector_[controller_state_.last_visited_pose_index]); + if (config_.feedforward_ang) { + feedforward_ang_ = + turning_radius_inv_vector_[controller_state_.last_visited_pose_index] * control_effort_long_; + ROS_DEBUG( + "turning_radius_inv_vector[%lu] = %f", controller_state_.last_visited_pose_index, + turning_radius_inv_vector_[controller_state_.last_visited_pose_index]); control_effort_ang_ = control_effort_ang_ + feedforward_ang_; - } - else - { + } else { feedforward_ang_ = 0.0; } @@ -751,88 +724,88 @@ geometry_msgs::Twist Controller::update(double target_x_vel, geometry_msgs::Twist output_combined; // Generate twist message - if (holonomic_robot_enable_) - { + if (holonomic_robot_enable_) { output_combined.linear.x = control_effort_long_; output_combined.linear.y = control_effort_lat_; output_combined.linear.z = 0; output_combined.angular.x = 0; output_combined.angular.y = 0; output_combined.angular.z = control_effort_ang_; - output_combined.angular.z = std::clamp(output_combined.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); - } - else - { + output_combined.angular.z = + std::clamp(output_combined.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); + } else { output_combined.linear.x = control_effort_long_; output_combined.linear.y = 0; output_combined.linear.z = 0; output_combined.angular.x = 0; output_combined.angular.y = 0; - output_combined.angular.z = copysign(1.0, config_.l) * control_effort_lat_ + - control_effort_ang_; // Take the sign of l for the lateral control effort - output_combined.angular.z = std::clamp(output_combined.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); + output_combined.angular.z = + copysign(1.0, config_.l) * control_effort_lat_ + + control_effort_ang_; // Take the sign of l for the lateral control effort + output_combined.angular.z = + std::clamp(output_combined.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); // For non-holonomic robots apply saturation based on minimum turning radius double max_ang_twist_tr; - if (config_.min_turning_radius < RADIUS_EPS) - { + if (config_.min_turning_radius < RADIUS_EPS) { // Rotation in place is allowed // minimum_turning_radius = RADIUS_EPS; // This variable is not used anymore so it does not matter // do not restrict angular velocity. Thus use the biggets number possible max_ang_twist_tr = std::numeric_limits::infinity(); - } - else - { + } else { max_ang_twist_tr = fabs(output_combined.linear.x / config_.min_turning_radius); } - output_combined.angular.z = std::clamp(output_combined.angular.z, -max_ang_twist_tr, max_ang_twist_tr); + output_combined.angular.z = + std::clamp(output_combined.angular.z, -max_ang_twist_tr, max_ang_twist_tr); } // Apply max acceleration limit to yaw - const double yaw_acc = std::clamp((output_combined.angular.z - current_yaw_vel) / dt.toSec(), - -config_.max_yaw_acc, config_.max_yaw_acc); + const double yaw_acc = std::clamp( + (output_combined.angular.z - current_yaw_vel) / dt.toSec(), -config_.max_yaw_acc, + config_.max_yaw_acc); const double new_yaw_vel = current_yaw_vel + (yaw_acc * dt.toSec()); output_combined.angular.z = new_yaw_vel; // Transform velocity commands at base_link to steer when using tricycle model - if (use_tricycle_model_) - { + if (use_tricycle_model_) { geometry_msgs::Twist output_steering; TricycleSteeringCmdVel steering_cmd = computeTricycleModelInverseKinematics(output_combined); - if (output_combined.linear.x < 0.0 && steering_cmd.speed > 0.0) - { - steering_cmd.speed = - steering_cmd.speed; - if (steering_cmd.steering_angle > 0) - { + if (output_combined.linear.x < 0.0 && steering_cmd.speed > 0.0) { + steering_cmd.speed = -steering_cmd.speed; + if (steering_cmd.steering_angle > 0) { steering_cmd.steering_angle = steering_cmd.steering_angle - M_PI; - } - else - { + } else { steering_cmd.steering_angle = steering_cmd.steering_angle + M_PI; } } // Apply limits to steering commands - steering_cmd.steering_angle = std::clamp(steering_cmd.steering_angle, - -config_.max_steering_angle, config_.max_steering_angle); - const double steering_yaw_vel = std::clamp((steering_cmd.steering_angle - controller_state_.previous_steering_angle) - / dt.toSec(), -config_.max_steering_yaw_vel, config_.max_steering_yaw_vel); - const double steering_angle_acc = std::clamp((steering_yaw_vel - controller_state_.previous_steering_yaw_vel)/ dt.toSec(), - -config_.max_steering_yaw_acc, config_.max_steering_yaw_acc); - steering_cmd.steering_angle_velocity = controller_state_.previous_steering_yaw_vel - + (steering_angle_acc * dt.toSec()); - steering_cmd.steering_angle = controller_state_.previous_steering_angle - + (steering_cmd.steering_angle_velocity * dt.toSec()); - - steering_cmd.speed = std::clamp(steering_cmd.speed, -config_.max_steering_x_vel, config_.max_steering_x_vel); - steering_cmd.acceleration = std::clamp((steering_cmd.speed - controller_state_.previous_steering_x_vel)/ dt.toSec(), - -config_.max_steering_x_acc, config_.max_steering_x_acc); - steering_cmd.speed = controller_state_.previous_steering_x_vel + (steering_cmd.acceleration * dt.toSec()); + steering_cmd.steering_angle = std::clamp( + steering_cmd.steering_angle, -config_.max_steering_angle, config_.max_steering_angle); + const double steering_yaw_vel = std::clamp( + (steering_cmd.steering_angle - controller_state_.previous_steering_angle) / dt.toSec(), + -config_.max_steering_yaw_vel, config_.max_steering_yaw_vel); + const double steering_angle_acc = std::clamp( + (steering_yaw_vel - controller_state_.previous_steering_yaw_vel) / dt.toSec(), + -config_.max_steering_yaw_acc, config_.max_steering_yaw_acc); + steering_cmd.steering_angle_velocity = + controller_state_.previous_steering_yaw_vel + (steering_angle_acc * dt.toSec()); + steering_cmd.steering_angle = controller_state_.previous_steering_angle + + (steering_cmd.steering_angle_velocity * dt.toSec()); + + steering_cmd.speed = + std::clamp(steering_cmd.speed, -config_.max_steering_x_vel, config_.max_steering_x_vel); + steering_cmd.acceleration = std::clamp( + (steering_cmd.speed - controller_state_.previous_steering_x_vel) / dt.toSec(), + -config_.max_steering_x_acc, config_.max_steering_x_acc); + steering_cmd.speed = + controller_state_.previous_steering_x_vel + (steering_cmd.acceleration * dt.toSec()); controller_state_.previous_steering_angle = steering_cmd.steering_angle; controller_state_.previous_steering_yaw_vel = steering_cmd.steering_angle_velocity; controller_state_.previous_steering_x_vel = steering_cmd.speed; // Compute velocities back to base_link and update controller state - output_steering = computeTricycleModelForwardKinematics(steering_cmd.speed, steering_cmd.steering_angle); - controller_state_.current_x_vel = output_steering.linear.x; + output_steering = + computeTricycleModelForwardKinematics(steering_cmd.speed, steering_cmd.steering_angle); + controller_state_.current_x_vel = output_steering.linear.x; controller_state_.current_yaw_vel = output_steering.angular.z; pid_debug->steering_angle = steering_cmd.steering_angle; @@ -854,11 +827,9 @@ geometry_msgs::Twist Controller::update(double target_x_vel, return output_combined; } -geometry_msgs::Twist Controller::update_with_limits(const geometry_msgs::Transform& current_tf, - const geometry_msgs::Twist& odom_twist, - ros::Duration dt, - double* eda, double* progress, - path_tracking_pid::PidDebug* pid_debug) +geometry_msgs::Twist Controller::update_with_limits( + const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + ros::Duration dt, double * eda, double * progress, path_tracking_pid::PidDebug * pid_debug) { // All limits are absolute double max_x_vel = std::abs(config_.target_x_vel); @@ -871,33 +842,30 @@ geometry_msgs::Twist Controller::update_with_limits(const geometry_msgs::Transfo // Apply mpc limit (last because less iterations required if max vel is already limited) double vel_max_mpc = std::numeric_limits::infinity(); - if (config_.use_mpc) - { - vel_max_mpc = std::abs(mpc_based_max_vel(std::copysign(max_x_vel, config_.target_x_vel), current_tf, odom_twist)); + if (config_.use_mpc) { + vel_max_mpc = std::abs( + mpc_based_max_vel(std::copysign(max_x_vel, config_.target_x_vel), current_tf, odom_twist)); max_x_vel = std::min(max_x_vel, vel_max_mpc); } // Some logging: - ROS_DEBUG("max_x_vel=%.3f, target_x_vel=%.3f, vel_max_external=%.3f, vel_max_obstacle=%.3f, vel_max_mpc=%.3f", - max_x_vel, config_.target_x_vel, vel_max_external_, vel_max_obstacle_, vel_max_mpc); - if (max_x_vel != config_.target_x_vel) - { - if (max_x_vel == vel_max_external_) - { + ROS_DEBUG( + "max_x_vel=%.3f, target_x_vel=%.3f, vel_max_external=%.3f, vel_max_obstacle=%.3f, " + "vel_max_mpc=%.3f", + max_x_vel, config_.target_x_vel, vel_max_external_, vel_max_obstacle_, vel_max_mpc); + if (max_x_vel != config_.target_x_vel) { + if (max_x_vel == vel_max_external_) { ROS_WARN_THROTTLE(5.0, "External velocity limit active %.2fm/s", vel_max_external_); - } - else if (max_x_vel == vel_max_obstacle_) - { + } else if (max_x_vel == vel_max_obstacle_) { ROS_WARN_THROTTLE(5.0, "Obstacle velocity limit active %.2fm/s", vel_max_obstacle_); - } - else if (max_x_vel == vel_max_mpc) - { + } else if (max_x_vel == vel_max_mpc) { ROS_WARN_THROTTLE(5.0, "MPC velocity limit active %.2fm/s", vel_max_mpc); } } // The end velocity is bound by the same limits to avoid accelerating above the limit in the end phase - double max_end_x_vel = std::min({std::abs(config_.target_end_x_vel), vel_max_external_, vel_max_obstacle_, vel_max_mpc}); + double max_end_x_vel = std::min( + {std::abs(config_.target_end_x_vel), vel_max_external_, vel_max_obstacle_, vel_max_mpc}); max_end_x_vel = std::copysign(max_end_x_vel, config_.target_end_x_vel); // Update the controller with the new setting @@ -907,8 +875,9 @@ geometry_msgs::Twist Controller::update_with_limits(const geometry_msgs::Transfo // output updated velocity command: (Current position, current measured velocity, closest point index, estimated // duration of arrival, debug info) -double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::Transform& current_tf, - const geometry_msgs::Twist& odom_twist) +double Controller::mpc_based_max_vel( + double target_x_vel, const geometry_msgs::Transform & current_tf, + const geometry_msgs::Twist & odom_twist) { // Temporary save global data ControllerState controller_state_saved; @@ -919,7 +888,7 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T int mpc_vel_optimization_iter = 0; // MPC parameters - int mpc_fwd_iter = 0; // Reset MPC iterations + int mpc_fwd_iter = 0; // Reset MPC iterations // Create predicted position vector geometry_msgs::Transform predicted_tf = current_tf; @@ -928,27 +897,28 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T double new_nominal_x_vel = target_x_vel; // Start off from the current velocity // Loop MPC - while (mpc_fwd_iter < config_.mpc_max_fwd_iterations && mpc_vel_optimization_iter <= config_.mpc_max_vel_optimization_iterations) - { + while (mpc_fwd_iter < config_.mpc_max_fwd_iterations && + mpc_vel_optimization_iter <= config_.mpc_max_vel_optimization_iterations) { mpc_fwd_iter += 1; // Check if robot stays within bounds for all iterations, if the new_nominal_x_vel is smaller than // max_target_x_vel we can increase it - if (mpc_fwd_iter == config_.mpc_max_fwd_iterations && fabs(controller_state_.error_lat.errors().at<0>()) <= config_.mpc_max_error_lat && - fabs(new_nominal_x_vel) < abs(target_x_vel)) - { + if ( + mpc_fwd_iter == config_.mpc_max_fwd_iterations && + fabs(controller_state_.error_lat.errors().at<0>()) <= config_.mpc_max_error_lat && + fabs(new_nominal_x_vel) < abs(target_x_vel)) { mpc_vel_optimization_iter += 1; // When we reach the maximum allowed mpc optimization iterations, do not change velocity anymore - if (mpc_vel_optimization_iter > config_.mpc_max_vel_optimization_iterations) - { + if (mpc_vel_optimization_iter > config_.mpc_max_vel_optimization_iterations) { break; } // Increase speed target_x_vel_prev = std::exchange( - new_nominal_x_vel, - copysign(1.0, new_nominal_x_vel) * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + new_nominal_x_vel); + new_nominal_x_vel, + copysign(1.0, new_nominal_x_vel) * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + + new_nominal_x_vel); // Reset variables controller_state_ = controller_state_saved; @@ -958,14 +928,14 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T mpc_fwd_iter = 0; } // If the robot gets out of bounds earlier we decrease the velocity - else if (abs(controller_state_.error_lat.errors().at<0>()) >= config_.mpc_max_error_lat) - { + else if (abs(controller_state_.error_lat.errors().at<0>()) >= config_.mpc_max_error_lat) { mpc_vel_optimization_iter += 1; // Lower speed target_x_vel_prev = std::exchange( - new_nominal_x_vel, - -copysign(1.0, new_nominal_x_vel) * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + new_nominal_x_vel); + new_nominal_x_vel, + -copysign(1.0, new_nominal_x_vel) * abs(target_x_vel_prev - new_nominal_x_vel) / 2 + + new_nominal_x_vel); // Reset variables controller_state_ = controller_state_saved; @@ -975,34 +945,34 @@ double Controller::mpc_based_max_vel(double target_x_vel, const geometry_msgs::T mpc_fwd_iter = 0; // Warning if new_nominal_x_vel becomes really low - if (abs(new_nominal_x_vel) < 0.01) - { + if (abs(new_nominal_x_vel) < 0.01) { ROS_WARN_THROTTLE(5.0, "Lowering velocity did not decrease the lateral error enough."); } - } - else if (mpc_fwd_iter != config_.mpc_max_fwd_iterations) - { + } else if (mpc_fwd_iter != config_.mpc_max_fwd_iterations) { // Run controller // Output: pred_twist.[linear.x, linear.y, linear.z, angular.x, angular.y, angular.z] path_tracking_pid::PidDebug pid_debug_unused; double eda_unused; double progress_unused; - pred_twist = Controller::update(new_nominal_x_vel, config_.target_end_x_vel, predicted_tf, pred_twist, - ros::Duration(config_.mpc_simulation_sample_time), - &eda_unused, &progress_unused, &pid_debug_unused); + pred_twist = Controller::update( + new_nominal_x_vel, config_.target_end_x_vel, predicted_tf, pred_twist, + ros::Duration(config_.mpc_simulation_sample_time), &eda_unused, &progress_unused, + &pid_debug_unused); // Run plant model const double theta = tf2::getYaw(predicted_tf.rotation); - predicted_tf.translation.x += pred_twist.linear.x * cos(theta) * config_.mpc_simulation_sample_time; - predicted_tf.translation.y += pred_twist.linear.x * sin(theta) * config_.mpc_simulation_sample_time; + predicted_tf.translation.x += + pred_twist.linear.x * cos(theta) * config_.mpc_simulation_sample_time; + predicted_tf.translation.y += + pred_twist.linear.x * sin(theta) * config_.mpc_simulation_sample_time; tf2::Quaternion q; q.setRPY(0, 0, theta + pred_twist.angular.z * config_.mpc_simulation_sample_time); predicted_tf.rotation = tf2::toMsg(q); } } // Apply limits to the velocity - double mpc_vel_limit = copysign(1.0, new_nominal_x_vel) * - fmax(fabs(new_nominal_x_vel), config_.mpc_min_x_vel); + double mpc_vel_limit = + copysign(1.0, new_nominal_x_vel) * fmax(fabs(new_nominal_x_vel), config_.mpc_min_x_vel); // Revert global variables controller_state_ = controller_state_saved; @@ -1019,13 +989,19 @@ void Controller::printParameters() const ROS_INFO("Distance L: %f", config_.l); ROS_INFO("Track base_link enabled?: %i", config_.track_base_link); - ROS_INFO("Target forward velocities (xv: %f, xv_end,: %f)", config_.target_x_vel, config_.target_end_x_vel); - ROS_INFO("Target forward (de)accelerations (xacc: %f, xdecc,: %f)", config_.target_x_acc, config_.target_x_decc); + ROS_INFO( + "Target forward velocities (xv: %f, xv_end,: %f)", config_.target_x_vel, + config_.target_end_x_vel); + ROS_INFO( + "Target forward (de)accelerations (xacc: %f, xdecc,: %f)", config_.target_x_acc, + config_.target_x_decc); ROS_INFO("Maximum allowed forward velocity error: %f", config_.max_error_x_vel); ROS_INFO("Feedback (lat, ang): ( %i, %i)", config_.feedback_lat, config_.feedback_ang); ROS_INFO("Feedforward (lat, ang): (%i, %i)", config_.feedforward_lat, config_.feedforward_ang); - ROS_INFO("Lateral gains: (Kp: %f, Ki, %f, Kd, %f)", config_.Kp_lat, config_.Ki_lat, config_.Kd_lat); - ROS_INFO("Angular gains: (Kp: %f, Ki, %f, Kd, %f)", config_.Kp_ang, config_.Ki_ang, config_.Kd_ang); + ROS_INFO( + "Lateral gains: (Kp: %f, Ki, %f, Kd, %f)", config_.Kp_lat, config_.Ki_lat, config_.Kd_lat); + ROS_INFO( + "Angular gains: (Kp: %f, Ki, %f, Kd, %f)", config_.Kp_ang, config_.Ki_ang, config_.Kd_ang); ROS_INFO("Robot type (holonomic): (%i)", holonomic_robot_enable_); @@ -1035,7 +1011,7 @@ void Controller::printParameters() const ROS_INFO("-----------------------------------------"); } -void Controller::configure(path_tracking_pid::PidConfig& config) +void Controller::configure(path_tracking_pid::PidConfig & config) { // Erase all queues when config changes controller_state_.error_lat.reset(); @@ -1045,11 +1021,16 @@ void Controller::configure(path_tracking_pid::PidConfig& config) controller_state_.error_deriv_ang.reset(); config.l = copysign(config.l, config.target_x_vel); - if (controller_state_.end_phase_enabled) - { - ROS_WARN_COND(abs(config.target_end_x_vel - config_.target_end_x_vel) > 1e-3, "Won't change end velocity in end phase"); - ROS_WARN_COND(abs(config.target_x_acc - config_.target_x_acc) > 1e-3, "Won't change accelerations in end phase"); - ROS_WARN_COND(abs(config.target_x_decc - config_.target_x_decc) > 1e-3, "Won't change accelerations in end phase"); + if (controller_state_.end_phase_enabled) { + ROS_WARN_COND( + abs(config.target_end_x_vel - config_.target_end_x_vel) > 1e-3, + "Won't change end velocity in end phase"); + ROS_WARN_COND( + abs(config.target_x_acc - config_.target_x_acc) > 1e-3, + "Won't change accelerations in end phase"); + ROS_WARN_COND( + abs(config.target_x_decc - config_.target_x_decc) > 1e-3, + "Won't change accelerations in end phase"); config.target_end_x_vel = config_.target_end_x_vel; config.target_x_acc = config_.target_x_acc; config.target_x_decc = config_.target_x_decc; @@ -1062,15 +1043,13 @@ void Controller::configure(path_tracking_pid::PidConfig& config) config_ = config; - ROS_DEBUG("Track base_link? Then global path poses are needed! '%d'", (int)config_.track_base_link); + ROS_DEBUG( + "Track base_link? Then global path poses are needed! '%d'", (int)config_.track_base_link); // printParameters(); } -path_tracking_pid::PidConfig Controller::getConfig() -{ - return config_; -} +path_tracking_pid::PidConfig Controller::getConfig() { return config_; } void Controller::setEnabled(bool value) { @@ -1095,27 +1074,24 @@ void Controller::reset() void Controller::setVelMaxExternal(double value) { - if (value < 0.0) - { + if (value < 0.0) { ROS_ERROR_THROTTLE(1.0, "External velocity limit (%f) has to be positive", value); return; } - if (value < 0.1) - { - ROS_WARN_THROTTLE(1.0, "External velocity limit is very small (%f), this could result in standstill", value); + if (value < 0.1) { + ROS_WARN_THROTTLE( + 1.0, "External velocity limit is very small (%f), this could result in standstill", value); } vel_max_external_ = value; } void Controller::setVelMaxObstacle(double value) { - ROS_WARN_COND(vel_max_obstacle_ != 0.0 && value == 0.0, "Collision imminent, slamming the brakes"); + ROS_WARN_COND( + vel_max_obstacle_ != 0.0 && value == 0.0, "Collision imminent, slamming the brakes"); vel_max_obstacle_ = value; } -double Controller::getVelMaxObstacle() const -{ - return vel_max_obstacle_; -} +double Controller::getVelMaxObstacle() const { return vel_max_obstacle_; } } // namespace path_tracking_pid diff --git a/src/details/second_order_lowpass.cpp b/src/details/second_order_lowpass.cpp index 7806c5b7..8614c5f0 100644 --- a/src/details/second_order_lowpass.cpp +++ b/src/details/second_order_lowpass.cpp @@ -1,13 +1,10 @@ -#include - #include +#include namespace path_tracking_pid::details { - namespace { - // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at 1/4 of the sample rate. constexpr double cutoff = 1.; @@ -17,10 +14,11 @@ void SecondOrderLowpass::push(double value) { errors_.push(value); - filtered_errors_.push((1 / (1 + cutoff * cutoff + M_SQRT2 * cutoff)) * - (errors_.at<2>() + 2 * errors_.at<1>() + errors_.at<0>() - - (cutoff * cutoff - M_SQRT2 * cutoff + 1) * filtered_errors_.at<1>() - - (-2 * cutoff * cutoff + 2) * filtered_errors_.at<0>())); + filtered_errors_.push( + (1 / (1 + cutoff * cutoff + M_SQRT2 * cutoff)) * + (errors_.at<2>() + 2 * errors_.at<1>() + errors_.at<0>() - + (cutoff * cutoff - M_SQRT2 * cutoff + 1) * filtered_errors_.at<1>() - + (-2 * cutoff * cutoff + 2) * filtered_errors_.at<0>())); } void SecondOrderLowpass::reset() @@ -29,11 +27,8 @@ void SecondOrderLowpass::reset() filtered_errors_.reset(); } -const FifoArray& SecondOrderLowpass::errors() const -{ - return errors_; -} -const FifoArray& SecondOrderLowpass::filtered_errors() const +const FifoArray & SecondOrderLowpass::errors() const { return errors_; } +const FifoArray & SecondOrderLowpass::filtered_errors() const { return filtered_errors_; } diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index be134237..f0a35047 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -4,13 +4,12 @@ #include "path_tracking_pid/path_tracking_pid_local_planner.hpp" -#include "common.hpp" - #include #include #include #include +#include "common.hpp" #include "mbf_msgs/ExePathResult.h" #include "path_tracking_pid/PidDebug.h" #include "path_tracking_pid/PidFeedback.h" @@ -21,39 +20,41 @@ // register planner as move_base and move_base plugins PLUGINLIB_EXPORT_CLASS(path_tracking_pid::TrackingPidLocalPlanner, nav_core::BaseLocalPlanner) -PLUGINLIB_EXPORT_CLASS(path_tracking_pid::TrackingPidLocalPlanner, mbf_costmap_core::CostmapController) +PLUGINLIB_EXPORT_CLASS( + path_tracking_pid::TrackingPidLocalPlanner, mbf_costmap_core::CostmapController) namespace path_tracking_pid { - -namespace { - +namespace +{ constexpr double MAP_PARALLEL_THRESH = 0.2; constexpr double DT_MAX = 1.5; -} // namespace anonymous +} // namespace -void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig& config) +void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig & config) { pid_controller_.configure(config); controller_debug_enabled_ = config.controller_debug_enabled; - if (controller_debug_enabled_ && !global_plan_.empty()) - { + if (controller_debug_enabled_ && !global_plan_.empty()) { received_path_.header = global_plan_.at(0).header; received_path_.poses = global_plan_; path_pub_.publish(received_path_); } } -void TrackingPidLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap) +void TrackingPidLocalPlanner::initialize( + std::string name, tf2_ros::Buffer * tf, costmap_2d::Costmap2DROS * costmap) { ros::NodeHandle nh("~/" + name); ros::NodeHandle gn; ROS_DEBUG("TrackingPidLocalPlanner::initialize(%s, ..., ...)", name.c_str()); // setup dynamic reconfigure - pid_server_ = std::make_unique>(config_mutex_, nh); - pid_server_->setCallback([this](auto& config, auto /*unused*/) { this->reconfigure_pid(config); }); + pid_server_ = + std::make_unique>(config_mutex_, nh); + pid_server_->setCallback( + [this](auto & config, auto /*unused*/) { this->reconfigure_pid(config); }); pid_controller_.setEnabled(false); bool holonomic_robot; @@ -69,14 +70,14 @@ void TrackingPidLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, nh.param("use_tricycle_model", use_tricycle_model_, false); nh.param("steered_wheel_frame", steered_wheel_frame_, "steer"); - collision_marker_pub_ = nh.advertise("collision_markers", 3); visualization_ = std::make_unique(nh); debug_pub_ = nh.advertise("debug", 1); path_pub_ = nh.advertise("visualization_path", 1, true); sub_odom_ = gn.subscribe("odom", 1, &TrackingPidLocalPlanner::curOdomCallback, this); - sub_vel_max_external_ = nh.subscribe("vel_max", 1, &TrackingPidLocalPlanner::velMaxExternalCallback, this); + sub_vel_max_external_ = + nh.subscribe("vel_max", 1, &TrackingPidLocalPlanner::velMaxExternalCallback, this); feedback_pub_ = nh.advertise("feedback", 1); map_frame_ = costmap->getGlobalFrameID(); @@ -86,11 +87,12 @@ void TrackingPidLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, initialized_ = true; } -bool TrackingPidLocalPlanner::setPlan(const std::vector& global_plan) +bool TrackingPidLocalPlanner::setPlan(const std::vector & global_plan) { - if (!initialized_) - { - ROS_ERROR("path_tracking_pid has not been initialized, please call initialize() before using this planner"); + if (!initialized_) { + ROS_ERROR( + "path_tracking_pid has not been initialized, please call initialize() before using this " + "planner"); return false; } @@ -101,10 +103,10 @@ bool TrackingPidLocalPlanner::setPlan(const std::vectorlookupTransform(map_frame_, path_frame, ros::Time(0)); // Check alignment, when path-frame is severly mis-aligned show error @@ -112,13 +114,12 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector MAP_PARALLEL_THRESH || std::fabs(roll) > MAP_PARALLEL_THRESH) - { - ROS_ERROR("Path is given in %s frame which is severly mis-aligned with our map-frame: %s", path_frame.c_str(), - map_frame_.c_str()); + if (std::fabs(pitch) > MAP_PARALLEL_THRESH || std::fabs(roll) > MAP_PARALLEL_THRESH) { + ROS_ERROR( + "Path is given in %s frame which is severly mis-aligned with our map-frame: %s", + path_frame.c_str(), map_frame_.c_str()); } - for (auto& pose_stamped : global_plan_) - { + for (auto & pose_stamped : global_plan_) { tf2::doTransform(pose_stamped.pose, pose_stamped.pose, tf_transform); pose_stamped.header.frame_id = map_frame_; // 'Project' plan by removing z-component @@ -126,59 +127,59 @@ bool TrackingPidLocalPlanner::setPlan(const std::vectorlookupTransform(map_frame_, base_link_frame_, ros::Time(0)); - } - catch (const tf2::TransformException& ex) - { + } catch (const tf2::TransformException & ex) { ROS_ERROR("Received an exception trying to transform: %s", ex.what()); return false; } // Feasability check, but only when not resuming with odom-vel - if (pid_controller_.getConfig().init_vel_method != Pid_Odom && - pid_controller_.getConfig().init_vel_max_diff >= 0.0 && - std::abs(latest_odom_.twist.twist.linear.x - pid_controller_.getControllerState().current_x_vel) > - pid_controller_.getConfig().init_vel_max_diff) - { - ROS_ERROR("Significant diff between odom (%f) and controller_state (%f) detected. Aborting!", - latest_odom_.twist.twist.linear.x, pid_controller_.getControllerState().current_x_vel); + if ( + pid_controller_.getConfig().init_vel_method != Pid_Odom && + pid_controller_.getConfig().init_vel_max_diff >= 0.0 && + std::abs( + latest_odom_.twist.twist.linear.x - pid_controller_.getControllerState().current_x_vel) > + pid_controller_.getConfig().init_vel_max_diff) { + ROS_ERROR( + "Significant diff between odom (%f) and controller_state (%f) detected. Aborting!", + latest_odom_.twist.twist.linear.x, pid_controller_.getControllerState().current_x_vel); return false; } - if (use_tricycle_model_) - { - try - { - ROS_DEBUG("base_link_frame: %s, steered_wheel_frame: %s", base_link_frame_.c_str(), steered_wheel_frame_.c_str()); - tf_base_to_steered_wheel_stamped_ = tf_->lookupTransform(base_link_frame_, steered_wheel_frame_, ros::Time(0)); - } - catch (const tf2::TransformException& ex) - { + if (use_tricycle_model_) { + try { + ROS_DEBUG( + "base_link_frame: %s, steered_wheel_frame: %s", base_link_frame_.c_str(), + steered_wheel_frame_.c_str()); + tf_base_to_steered_wheel_stamped_ = + tf_->lookupTransform(base_link_frame_, steered_wheel_frame_, ros::Time(0)); + } catch (const tf2::TransformException & ex) { ROS_ERROR("Received an exception trying to transform: %s", ex.what()); - ROS_ERROR("Invalid transformation from base_link_frame to steered_wheel_frame. Tricycle model will be disabled"); + ROS_ERROR( + "Invalid transformation from base_link_frame to steered_wheel_frame. Tricycle model will " + "be disabled"); use_tricycle_model_ = false; } - pid_controller_.setTricycleModel(use_tricycle_model_, tf_base_to_steered_wheel_stamped_.transform); + pid_controller_.setTricycleModel( + use_tricycle_model_, tf_base_to_steered_wheel_stamped_.transform); // TODO(clopez): subscribe to steered wheel odom geometry_msgs::Twist steering_odom_twist; - pid_controller_.setPlan(tfCurPoseStamped_.transform, latest_odom_.twist.twist, - tf_base_to_steered_wheel_stamped_.transform, steering_odom_twist, global_plan_); - } - else - { + pid_controller_.setPlan( + tfCurPoseStamped_.transform, latest_odom_.twist.twist, + tf_base_to_steered_wheel_stamped_.transform, steering_odom_twist, global_plan_); + } else { pid_controller_.setPlan(tfCurPoseStamped_.transform, latest_odom_.twist.twist, global_plan_); } @@ -188,79 +189,66 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector ros::Duration(DT_MAX)) - { + if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) { ROS_ERROR("Invalid time increment: %f. Aborting", dt.toSec()); return false; } - try - { + try { ROS_DEBUG("map_frame: %s, base_link_frame: %s", map_frame_.c_str(), base_link_frame_.c_str()); tfCurPoseStamped_ = tf_->lookupTransform(map_frame_, base_link_frame_, ros::Time(0)); - } - catch (const tf2::TransformException& ex) - { + } catch (const tf2::TransformException & ex) { ROS_ERROR("Received an exception trying to transform: %s", ex.what()); active_goal_ = false; return false; } // Handle obstacles - if (pid_controller_.getConfig().anti_collision) - { + if (pid_controller_.getConfig().anti_collision) { auto cost = projectedCollisionCost(); - if (cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) - { + if (cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { pid_controller_.setVelMaxObstacle(0.0); - } - else if (pid_controller_.getConfig().obstacle_speed_reduction) - { + } else if (pid_controller_.getConfig().obstacle_speed_reduction) { double max_vel = pid_controller_.getConfig().max_x_vel; double reduction_factor = static_cast(cost) / costmap_2d::INSCRIBED_INFLATED_OBSTACLE; double limit = max_vel * (1 - reduction_factor); ROS_DEBUG("Cost: %d, factor: %f, limit: %f", cost, reduction_factor, limit); pid_controller_.setVelMaxObstacle(limit); - } - else - { + } else { pid_controller_.setVelMaxObstacle(INFINITY); // set back to inf } - } - else - { + } else { pid_controller_.setVelMaxObstacle(INFINITY); // Can be disabled live, so set back to inf } path_tracking_pid::PidDebug pid_debug; - double eda = 1 / FLT_EPSILON; // initial guess. Avoids errors in case function returns due to wrong delta_t; + double eda = + 1 / FLT_EPSILON; // initial guess. Avoids errors in case function returns due to wrong delta_t; double progress = 0.0; - cmd_vel = pid_controller_.update_with_limits(tfCurPoseStamped_.transform, latest_odom_.twist.twist, - dt, &eda, &progress, &pid_debug); + cmd_vel = pid_controller_.update_with_limits( + tfCurPoseStamped_.transform, latest_odom_.twist.twist, dt, &eda, &progress, &pid_debug); path_tracking_pid::PidFeedback feedback_msg; feedback_msg.eda = ros::Duration(eda); feedback_msg.progress = progress; feedback_pub_.publish(feedback_msg); - if (cancel_requested_) - { + if (cancel_requested_) { path_tracking_pid::PidConfig config = pid_controller_.getConfig(); // Copysign here, such that when cancelling while driving backwards, we decelerate to -0.0 and hence // the sign propagates correctly @@ -271,12 +259,10 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_ pid_controller_.configure(config); pid_server_->updateConfig(config); lock.unlock(); - cancel_requested_ = false; + cancel_requested_ = false; } - - if (controller_debug_enabled_) - { + if (controller_debug_enabled_) { debug_pub_.publish(pid_debug); // publish rviz visualization @@ -292,7 +278,8 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_ } prev_time_ = now; - prev_dt_ = dt; // Store last known valid dt for next cycles (https://github.com/magazino/move_base_flex/issues/195) + prev_dt_ = + dt; // Store last known valid dt for next cycles (https://github.com/magazino/move_base_flex/issues/195) return true; } @@ -366,9 +353,11 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Check how far we should check forward double x_vel = pid_controller_.getControllerState().current_x_vel; - double collision_look_ahead_distance = x_vel*x_vel / (2*pid_controller_.getConfig().target_x_decc) - + pid_controller_.getConfig().collision_look_ahead_length_offset; - uint n_steps = std::ceil(collision_look_ahead_distance / pid_controller_.getConfig().collision_look_ahead_resolution); + double collision_look_ahead_distance = + x_vel * x_vel / (2 * pid_controller_.getConfig().target_x_decc) + + pid_controller_.getConfig().collision_look_ahead_length_offset; + uint n_steps = std::ceil( + collision_look_ahead_distance / pid_controller_.getConfig().collision_look_ahead_resolution); double x_resolution = collision_look_ahead_distance / std::max(static_cast(n_steps), 1); // Define a x_step transform which will be used to step forward the position. @@ -388,11 +377,10 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link projected_step_tf = pid_controller_.findPositionOnPlan(current_tf, &projected_controller_state); projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose - for (uint step = 0; step < n_steps; step++) - { + for (uint step = 0; step < n_steps; step++) { tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf; - projected_step_tf = pid_controller_.findPositionOnPlan(tf2::toMsg(next_straight_step_tf), - &projected_controller_state); + projected_step_tf = pid_controller_.findPositionOnPlan( + tf2::toMsg(next_straight_step_tf), &projected_controller_state); projected_steps_tf.push_back(projected_step_tf); // Fill markers: @@ -406,8 +394,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() polygon_t previous_footprint_xy; polygon_t collision_polygon; - for (const auto& projection_tf : projected_steps_tf) - { + for (const auto & projection_tf : projected_steps_tf) { // Project footprint forward double x = projection_tf.getOrigin().x(); double y = projection_tf.getOrigin().y(); @@ -418,8 +405,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Append footprint to polygon polygon_t two_footprints = previous_footprint_xy; previous_footprint_xy.clear(); - for (const auto& point : footprint) - { + for (const auto & point : footprint) { boost::geometry::append(two_footprints, point); boost::geometry::append(previous_footprint_xy, point); } @@ -431,8 +417,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Add footprint to marker geometry_msgs::Point previous_point = footprint.back(); - for (const auto& point : footprint) - { + for (const auto & point : footprint) { mkCollisionFootprint.points.push_back(previous_point); mkCollisionFootprint.points.push_back(point); previous_point = point; @@ -440,14 +425,13 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() } // Create a convex hull so we can use costmap2d->convexFillCells - costmap_2d::Costmap2D* costmap2d = costmap_->getCostmap(); + costmap_2d::Costmap2D * costmap2d = costmap_->getCostmap(); polygon_t collision_polygon_hull; boost::geometry::convex_hull(collision_polygon, collision_polygon_hull); std::vector collision_polygon_hull_map; // Convert to map coordinates - for (const auto& point : collision_polygon_hull) - { + for (const auto & point : collision_polygon_hull) { int map_x; int map_y; costmap2d->worldToMapEnforceBounds(point.x, point.y, map_x, map_y); @@ -461,17 +445,14 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Get the max cost inside the concave polygon uint8_t max_cost = 0.0; - for (const auto& cell_in_polygon : cells_in_polygon) - { + for (const auto & cell_in_polygon : cells_in_polygon) { // Cost checker is cheaper than polygon checker, so lets do that first uint8_t cell_cost = costmap2d->getCost(cell_in_polygon.x, cell_in_polygon.y); - if (cell_cost > max_cost && cell_cost != costmap_2d::NO_INFORMATION) - { + if (cell_cost > max_cost && cell_cost != costmap_2d::NO_INFORMATION) { // Check if in concave polygon geometry_msgs::Point point; costmap2d->mapToWorld(cell_in_polygon.x, cell_in_polygon.y, point.x, point.y); - if (boost::geometry::within(point, collision_polygon)) - { + if (boost::geometry::within(point, collision_polygon)) { // Protip: uncomment below and 'if (cell_cost > max_cost)' to see evaluated cells // boost::unique_lock lock_controller(*(costmap2d->getMutex())); // costmap2d->setCost(cell_in_polygon.x, cell_in_polygon.y, 100); @@ -482,33 +463,27 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() mkCollisionIndicator.color.a = cell_cost / 255.0; point.z = mkCollisionIndicator.scale.z * 0.5; mkCollisionIndicator.pose.position = point; - if (max_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) - { + if (max_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { break; // Collision detected, no need to evaluate further } } } } - if (mkCollisionIndicator.scale.z > std::numeric_limits::epsilon()) - { + if (mkCollisionIndicator.scale.z > std::numeric_limits::epsilon()) { mkCollisionIndicator.action = visualization_msgs::Marker::ADD; - } - else - { + } else { mkCollisionIndicator.action = visualization_msgs::Marker::DELETE; } mkCollision.markers.push_back(mkCollisionIndicator); // Fiddle the polygon into a marker message - for (const geometry_msgs::Point point : collision_polygon) - { + for (const geometry_msgs::Point point : collision_polygon) { mkCollisionHull.points.push_back(point); } mkCollision.markers.push_back(mkCollisionFootprint); mkCollision.markers.push_back(mkCollisionHull); - if (n_steps > 0) - { + if (n_steps > 0) { mkCollision.markers.push_back(mkSteps); mkCollision.markers.push_back(mkPosesOnPath); } @@ -517,19 +492,19 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() return max_cost; } -uint32_t TrackingPidLocalPlanner::computeVelocityCommands(const geometry_msgs::PoseStamped& /* pose */, - const geometry_msgs::TwistStamped& /* velocity */, - geometry_msgs::TwistStamped& cmd_vel, std::string& /* message */) +uint32_t TrackingPidLocalPlanner::computeVelocityCommands( + const geometry_msgs::PoseStamped & /* pose */, const geometry_msgs::TwistStamped & /* velocity */, + geometry_msgs::TwistStamped & cmd_vel, std::string & /* message */) { - if (!initialized_) - { - ROS_ERROR("path_tracking_pid has not been initialized, please call initialize() before using this planner"); + if (!initialized_) { + ROS_ERROR( + "path_tracking_pid has not been initialized, please call initialize() before using this " + "planner"); active_goal_ = false; return mbf_msgs::ExePathResult::NOT_INITIALIZED; } // TODO(Cesar): Use provided pose and odom - if (!computeVelocityCommands(cmd_vel.twist)) - { + if (!computeVelocityCommands(cmd_vel.twist)) { active_goal_ = false; return mbf_msgs::ExePathResult::FAILURE; } @@ -537,27 +512,24 @@ uint32_t TrackingPidLocalPlanner::computeVelocityCommands(const geometry_msgs::P cmd_vel.header.frame_id = base_link_frame_; bool moving = std::abs(cmd_vel.twist.linear.x) > VELOCITY_EPS; - if (cancel_in_progress_) - { - if (!moving) - { - ROS_INFO("Cancel requested and we now (almost) reached velocity 0: %f", cmd_vel.twist.linear.x); - cancel_in_progress_ = false; - active_goal_ = false; - return mbf_msgs::ExePathResult::CANCELED; + if (cancel_in_progress_) { + if (!moving) { + ROS_INFO( + "Cancel requested and we now (almost) reached velocity 0: %f", cmd_vel.twist.linear.x); + cancel_in_progress_ = false; + active_goal_ = false; + return mbf_msgs::ExePathResult::CANCELED; } ROS_INFO_THROTTLE(1.0, "Cancel in progress... remaining x_vel: %f", cmd_vel.twist.linear.x); return to_underlying(ComputeVelocityCommandsResult::GRACEFULLY_CANCELLING); } - if (!moving && pid_controller_.getVelMaxObstacle() < VELOCITY_EPS) - { + if (!moving && pid_controller_.getVelMaxObstacle() < VELOCITY_EPS) { active_goal_ = false; return mbf_msgs::ExePathResult::BLOCKED_PATH; } - if (isGoalReached()) - { + if (isGoalReached()) { active_goal_ = false; } return mbf_msgs::ExePathResult::SUCCESS; @@ -569,7 +541,8 @@ bool TrackingPidLocalPlanner::isGoalReached() return pid_controller_.getControllerState().end_reached && !cancel_in_progress_; } -bool TrackingPidLocalPlanner::isGoalReached(double /* dist_tolerance */, double /* angle_tolerance */) +bool TrackingPidLocalPlanner::isGoalReached( + double /* dist_tolerance */, double /* angle_tolerance */) { return isGoalReached(); } @@ -581,20 +554,19 @@ bool TrackingPidLocalPlanner::cancel() cancel_in_progress_ = true; ros::Rate r(10); ROS_INFO("Cancel requested, waiting in loop for cancel to finish"); - while (active_goal_) - { - r.sleep(); + while (active_goal_) { + r.sleep(); } ROS_INFO("Finished waiting loop, done cancelling"); return true; } -void TrackingPidLocalPlanner::curOdomCallback(const nav_msgs::Odometry& odom_msg) +void TrackingPidLocalPlanner::curOdomCallback(const nav_msgs::Odometry & odom_msg) { latest_odom_ = odom_msg; } -void TrackingPidLocalPlanner::velMaxExternalCallback(const std_msgs::Float64& msg) +void TrackingPidLocalPlanner::velMaxExternalCallback(const std_msgs::Float64 & msg) { pid_controller_.setVelMaxExternal(msg.data); } diff --git a/src/visualization.cpp b/src/visualization.cpp index 41f61b42..e84240b4 100644 --- a/src/visualization.cpp +++ b/src/visualization.cpp @@ -49,7 +49,7 @@ void Visualization::publishPlanPoint(const std_msgs::Header & header, const tf2: } void Visualization::publishSphere( - const std_msgs::Header & header, const std::string& ns, int id, const tf2::Transform & pose, + const std_msgs::Header & header, const std::string & ns, int id, const tf2::Transform & pose, const std_msgs::ColorRGBA & color) { geometry_msgs::Pose msg; @@ -58,7 +58,7 @@ void Visualization::publishSphere( } void Visualization::publishSphere( - const std_msgs::Header & header, const std::string& ns, int id, geometry_msgs::Pose pose, + const std_msgs::Header & header, const std::string & ns, int id, geometry_msgs::Pose pose, const std_msgs::ColorRGBA & color) { visualization_msgs::Marker marker; From 6f154fe042b77548e917ff45c3ab50484f649c58 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 15:55:58 +0000 Subject: [PATCH 079/156] Cleaned up includes. --- include/path_tracking_pid/controller.hpp | 16 +++++----- .../path_tracking_pid_local_planner.hpp | 32 +++++++++---------- include/path_tracking_pid/visualization.hpp | 10 +++--- src/controller.cpp | 6 ++-- src/path_tracking_pid_local_planner.cpp | 16 +++++----- src/visualization.cpp | 9 +++--- 6 files changed, 44 insertions(+), 45 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 4ccfefe0..0dce48b7 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -1,18 +1,18 @@ #pragma once +#include +#include +#include +#include +#include +#include + #include +#include #include #include #include -#include "boost/noncopyable.hpp" -#include "geometry_msgs/PoseStamped.h" -#include "geometry_msgs/Transform.h" -#include "geometry_msgs/Twist.h" -#include "path_tracking_pid/PidConfig.h" -#include "path_tracking_pid/PidDebug.h" -#include "tf2/LinearMath/Transform.h" - namespace path_tracking_pid { struct TricycleSteeringCmdVel diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 6f8855f6..2406b3af 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -1,26 +1,26 @@ #pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include #include #include - -#include "./controller.hpp" -#include "boost/noncopyable.hpp" -#include "boost/thread/recursive_mutex.hpp" -#include "costmap_2d/costmap_2d_ros.h" -#include "dynamic_reconfigure/server.h" -#include "geometry_msgs/Point.h" -#include "mbf_costmap_core/costmap_controller.h" -#include "nav_core/base_local_planner.h" -#include "nav_msgs/Odometry.h" -#include "nav_msgs/Path.h" -#include "path_tracking_pid/PidConfig.h" -#include "path_tracking_pid/visualization.hpp" -#include "std_msgs/Bool.h" -#include "std_msgs/Float64.h" -#include "tf2_ros/buffer.h" +#include +#include +#include +#include BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x, y) diff --git a/include/path_tracking_pid/visualization.hpp b/include/path_tracking_pid/visualization.hpp index e563f9df..80e4d22c 100644 --- a/include/path_tracking_pid/visualization.hpp +++ b/include/path_tracking_pid/visualization.hpp @@ -1,9 +1,9 @@ -#include +#include +#include +#include +#include -#include "geometry_msgs/Pose.h" -#include "ros/publisher.h" -#include "std_msgs/ColorRGBA.h" -#include "tf2/LinearMath/Transform.h" +#include namespace path_tracking_pid { diff --git a/src/controller.cpp b/src/controller.cpp index 4013d3e0..3d6e26d1 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -2,14 +2,14 @@ // Created by nobleo on 11-9-18. // -#include "path_tracking_pid/controller.hpp" +#include +#include #include +#include #include -#include "angles/angles.h" #include "common.hpp" -#include "tf2/utils.h" namespace path_tracking_pid { diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index f0a35047..d2749bc1 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -2,21 +2,21 @@ // Created by nobleo on 12-9-18. // -#include "path_tracking_pid/path_tracking_pid_local_planner.hpp" +#include +#include +#include +#include +#include +#include +#include #include #include +#include #include #include #include "common.hpp" -#include "mbf_msgs/ExePathResult.h" -#include "path_tracking_pid/PidDebug.h" -#include "path_tracking_pid/PidFeedback.h" -#include "pluginlib/class_list_macros.h" -#include "tf2/utils.h" -#include "visualization_msgs/Marker.h" -#include "visualization_msgs/MarkerArray.h" // register planner as move_base and move_base plugins PLUGINLIB_EXPORT_CLASS(path_tracking_pid::TrackingPidLocalPlanner, nav_core::BaseLocalPlanner) diff --git a/src/visualization.cpp b/src/visualization.cpp index e84240b4..9366e861 100644 --- a/src/visualization.cpp +++ b/src/visualization.cpp @@ -1,11 +1,10 @@ -#include "path_tracking_pid/visualization.hpp" +#include +#include +#include +#include #include -#include "ros/node_handle.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "visualization_msgs/Marker.h" - namespace path_tracking_pid { Visualization::Visualization(ros::NodeHandle nh) From cf7e1b23ad0d435c40695ff9c8ce27911494f72d Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 21 Feb 2022 12:00:02 +0000 Subject: [PATCH 080/156] Extracted filter_plan() function. --- src/controller.cpp | 55 ++++++++++++++++++++++++++++------------------ 1 file changed, 34 insertions(+), 21 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 3d6e26d1..50d591b8 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -63,6 +63,34 @@ bool is_pose_angle_obtuse( return distSquared(prev, next) > (distSquared(prev, cur) + distSquared(cur, next)); } +// Filters (and converts) the given plan. The first and last poses are always accepted (if they +// exist). Intermediate poses are only accepted if the angle (with respect to the previous and next +// poses) is obtuse. All accepted poses are converted to transforms. +std::vector filter_plan(const std::vector & plan) +{ + const auto plan_size = plan.size(); + auto result = std::vector{}; + + if (plan_size > 0) { + result.push_back(to_transform(plan.front().pose)); + } + + for (int pose_idx = 1; pose_idx < static_cast(plan_size) - 1; ++pose_idx) { + const auto prev_pose = plan[pose_idx - 1].pose; + const auto pose = plan[pose_idx].pose; + const auto next_pose = plan[pose_idx + 1].pose; + if (is_pose_angle_obtuse(prev_pose, pose, next_pose)) { + result.push_back(to_transform(pose)); + } + } + + if (plan_size > 1) { + result.push_back(to_transform(plan.back().pose)); + } + + return result; +} + } // namespace void Controller::setHolonomic(bool holonomic) @@ -151,34 +179,19 @@ void Controller::setPlan( tf2::Transform current_tf2; tf2::convert(current_tf, current_tf2); - /* Minimal sanity check */ - global_plan_tf_.clear(); - global_plan_tf_.push_back(to_transform(global_plan[0].pose)); - // For now do not allow repeated points or in-place rotation - // To allow that the way the progress is checked and the interpolation is done needs to be changed - // Also check if points suddenly go in the opposite direction, this could lead to deadlocks - for (int pose_idx = 1; pose_idx < static_cast(global_plan.size()) - 1; ++pose_idx) { - const auto prev_pose = global_plan[pose_idx - 1].pose; - const auto pose = global_plan[pose_idx].pose; - const auto next_pose = global_plan[pose_idx + 1].pose; - if (is_pose_angle_obtuse(prev_pose, pose, next_pose)) { - global_plan_tf_.push_back(to_transform(pose)); - } else { - ROS_WARN( - "Pose %i of path is not used since it is not in the expected direction of the path!", - pose_idx); - } + global_plan_tf_ = filter_plan(global_plan); + if (global_plan_tf_.size() != global_plan.size()) { + ROS_WARN( + "Not all poses of path are used since not all poses were in the expected direction of the " + "path!"); } - // Add last pose as we didn't evaluate that one - const auto last_transform = to_transform(global_plan.back().pose); - global_plan_tf_.push_back(last_transform); if (!config_.track_base_link) { // Add carrot length to plan using goal pose (we assume the last pose contains correct angle) tf2::Transform carrotTF( tf2::Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0), tf2::Vector3(config_.l, 0.0, 0.0)); - global_plan_tf_.push_back(last_transform * carrotTF); + global_plan_tf_.push_back(global_plan_tf_.back() * carrotTF); } // Whenever a new path is recieved, computed the closest pose to From b491f68898324d1ab119ef4e3f1d2b58644c7cdf Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 21 Feb 2022 13:33:12 +0000 Subject: [PATCH 081/156] Added unittests for FifoArray. --- CMakeLists.txt | 2 + unittest/test_fifo_array.cpp | 117 +++++++++++++++++++++++++++++++++++ unittest/test_main.cpp | 7 +++ 3 files changed, 126 insertions(+) create mode 100644 unittest/test_fifo_array.cpp create mode 100644 unittest/test_main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index da83a8f0..25c96b48 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,4 +106,6 @@ install( if(CATKIN_ENABLE_TESTING) add_rostest(test/test_path_tracking_pid.test ARGS rviz:=false reconfigure:=false) + catkin_add_gtest(unittests unittest/test_main.cpp unittest/test_fifo_array.cpp) + target_link_libraries(unittests ${catkin_LIBRARIES}) endif() diff --git a/unittest/test_fifo_array.cpp b/unittest/test_fifo_array.cpp new file mode 100644 index 00000000..42a1562b --- /dev/null +++ b/unittest/test_fifo_array.cpp @@ -0,0 +1,117 @@ +#include + +#include + +namespace +{ + +using path_tracking_pid::details::FifoArray; + +TEST(PathTrackingPidDetailsFifoArray, Initialize) +{ + FifoArray fifo; + + EXPECT_EQ(fifo[0], 0); + EXPECT_EQ(fifo[1], 0); + EXPECT_EQ(fifo[2], 0); +} + +TEST(PathTrackingPidDetailsFifoArray, Push) +{ + FifoArray fifo; + + fifo.push(42); + + EXPECT_EQ(fifo[0], 42); + EXPECT_EQ(fifo[1], 0); + EXPECT_EQ(fifo[2], 0); +} + +TEST(PathTrackingPidDetailsFifoArray, Push_UntilFull) +{ + FifoArray fifo; + + fifo.push(42); + fifo.push(111); + fifo.push(-4); + + EXPECT_EQ(fifo[0], -4); + EXPECT_EQ(fifo[1], 111); + EXPECT_EQ(fifo[2], 42); +} + +TEST(PathTrackingPidDetailsFifoArray, Push_BeyondFull) +{ + FifoArray fifo; + + fifo.push(42); + fifo.push(111); + fifo.push(-4); + fifo.push(314); + + EXPECT_EQ(fifo[0], 314); + EXPECT_EQ(fifo[1], -4); + EXPECT_EQ(fifo[2], 111); +} + +TEST(PathTrackingPidDetailsFifoArray, Reset) +{ + FifoArray fifo; + + fifo.push(42); + fifo.push(111); + fifo.push(-4); + fifo.push(314); + + fifo.reset(); + + EXPECT_EQ(fifo[0], 0); + EXPECT_EQ(fifo[1], 0); + EXPECT_EQ(fifo[2], 0); +} + +TEST(PathTrackingPidDetailsFifoArray, At) +{ + FifoArray fifo; + + fifo.push(42); + fifo.push(111); + fifo.push(-4); + + EXPECT_EQ(fifo[0], fifo.at<0>()); + EXPECT_EQ(fifo[1], fifo.at<1>()); + EXPECT_EQ(fifo[2], fifo.at<2>()); +} + +TEST(PathTrackingPidDetailsFifoArray, OtherType) +{ + FifoArray fifo; + + fifo.push(24.0); + fifo.push(0.5); + fifo.push(0.25); + + EXPECT_EQ(fifo[0], 0.25); + EXPECT_EQ(fifo[1], 0.5); + EXPECT_EQ(fifo[2], 24.0); +} + +TEST(PathTrackingPidDetailsFifoArray, OtherSize) +{ + FifoArray fifo; + + fifo.push(1); + fifo.push(2); + fifo.push(3); + fifo.push(4); + fifo.push(5); + fifo.push(6); + + EXPECT_EQ(fifo[0], 6); + EXPECT_EQ(fifo[1], 5); + EXPECT_EQ(fifo[2], 4); + EXPECT_EQ(fifo[3], 3); + EXPECT_EQ(fifo[4], 2); +} + +} // namespace diff --git a/unittest/test_main.cpp b/unittest/test_main.cpp new file mode 100644 index 00000000..01836b2b --- /dev/null +++ b/unittest/test_main.cpp @@ -0,0 +1,7 @@ +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 3e46d5f8f58694f85ac2df4a0866f4685246fa34 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 21 Feb 2022 15:45:02 +0000 Subject: [PATCH 082/156] Reworked by moving test source files. --- CMakeLists.txt | 2 +- {unittest => test/unittests}/test_fifo_array.cpp | 0 {unittest => test/unittests}/test_main.cpp | 0 3 files changed, 1 insertion(+), 1 deletion(-) rename {unittest => test/unittests}/test_fifo_array.cpp (100%) rename {unittest => test/unittests}/test_main.cpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 25c96b48..731d37c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,6 +106,6 @@ install( if(CATKIN_ENABLE_TESTING) add_rostest(test/test_path_tracking_pid.test ARGS rviz:=false reconfigure:=false) - catkin_add_gtest(unittests unittest/test_main.cpp unittest/test_fifo_array.cpp) + catkin_add_gtest(unittests test/unittests/test_main.cpp test/unittests/test_fifo_array.cpp) target_link_libraries(unittests ${catkin_LIBRARIES}) endif() diff --git a/unittest/test_fifo_array.cpp b/test/unittests/test_fifo_array.cpp similarity index 100% rename from unittest/test_fifo_array.cpp rename to test/unittests/test_fifo_array.cpp diff --git a/unittest/test_main.cpp b/test/unittests/test_main.cpp similarity index 100% rename from unittest/test_main.cpp rename to test/unittests/test_main.cpp From 70a6b86d7f119f21a7e52ed8e86de80d0e916fd5 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Mon, 21 Feb 2022 17:59:54 +0100 Subject: [PATCH 083/156] Enable VERBOSE_TESTS --- .github/workflows/industrial_ci_action.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index 8bdb8931..f755af2e 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -14,4 +14,7 @@ jobs: steps: - uses: actions/checkout@v1 - uses: 'ros-industrial/industrial_ci@master' - env: ${{matrix.env}} + env: + ROS_DISTRO: ${{ matrix.env.ROS_DISTRO }} + CMAKE_ARGS: ${{ matrix.env.CMAKE_ARGS }} + VERBOSE_TESTS: true From c33c2eb514fc80107cd02663596c2773588944f8 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Tue, 22 Feb 2022 10:57:53 +0100 Subject: [PATCH 084/156] Enable roslint --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 731d37c7..9d38e6ef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,6 +78,7 @@ include_directories(include) # Roslint cpp set(ROSLINT_CPP_OPTS "--filter=-legal/copyright") roslint_cpp() +roslint_add_test() install( TARGETS From c7e973af7753d2ca49c227c13aa57948adf5f444 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Tue, 22 Feb 2022 11:22:21 +0100 Subject: [PATCH 085/156] Disable whitespace/braces check This check was in conflict with the ROS2 clang-format style so I've disabled it. --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d38e6ef..1acba745 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,7 +76,7 @@ include_directories(include) # Configure roslint for nodes # Roslint cpp -set(ROSLINT_CPP_OPTS "--filter=-legal/copyright") +set(ROSLINT_CPP_OPTS "--filter=-legal/copyright,-whitespace/braces") roslint_cpp() roslint_add_test() From 32f9d89c771718304660863e78fc77ee2c4dd716 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Tue, 22 Feb 2022 11:24:36 +0100 Subject: [PATCH 086/156] Fix roslint --- src/controller.cpp | 1 + src/path_tracking_pid_local_planner.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/controller.cpp b/src/controller.cpp index 50d591b8..1038d96c 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index d2749bc1..8e080b00 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -10,6 +10,7 @@ #include #include +#include #include #include #include From 9fc9c6495c144ad99b1c3a48420546d79682016d Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 23 Feb 2022 08:09:17 +0000 Subject: [PATCH 087/156] Refactored output parameters of update. --- include/path_tracking_pid/controller.hpp | 28 ++--- src/controller.cpp | 127 +++++++++++------------ src/path_tracking_pid_local_planner.cpp | 15 ++- 3 files changed, 84 insertions(+), 86 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 0dce48b7..6e05c0d6 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -109,35 +109,37 @@ class Controller : private boost::noncopyable return findPositionOnPlan(current_tf, controller_state_ptr, path_pose_idx); } + // Result of update() and update_with_limits(). + struct UpdateResult + { + geometry_msgs::Twist velocity_command; // Velocity command + double eda = 0; // Estimated duration of arrival + double progress = 0; // Progress along the path [0,1] + PidDebug pid_debug; // Information to debug the controller + }; + /** * Run one iteration of a PID controller * @param target_x_vel robot will try to reach target x velocity within (de)acceleration limits * @param current Where is the robot now? * @param odom_twist Robot odometry * @param dt Duration since last update - * @return Velocity command - * @return eda Estimated Duration of Arrival - * @return progress Progress along the path [0,1] - * @return pid_debug Variable with information to debug the controller + * @return Update result */ - geometry_msgs::Twist update( + UpdateResult update( double target_x_vel, double target_end_x_vel, const geometry_msgs::Transform & current_tf, - const geometry_msgs::Twist & odom_twist, ros::Duration dt, double * eda, double * progress, - path_tracking_pid::PidDebug * pid_debug); + const geometry_msgs::Twist & odom_twist, ros::Duration dt); /** * Run one iteration of a PID controller with velocity limits applied * @param current Where is the robot now? * @param odom_twist Robot odometry * @param dt Duration since last update - * @return Velocity command - * @return eda Estimated Duration of Arrival - * @return progress Progress along the path [0,1] - * @return pid_debug Variable with information to debug the controller + * @return Update result */ - geometry_msgs::Twist update_with_limits( + UpdateResult update_with_limits( const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, - ros::Duration dt, double * eda, double * progress, path_tracking_pid::PidDebug * pid_debug); + ros::Duration dt); /** * Perform prediction steps on the lateral error and return a reduced velocity that stays within bounds diff --git a/src/controller.cpp b/src/controller.cpp index 1038d96c..2b733d0c 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -433,11 +433,12 @@ tf2::Transform Controller::findPositionOnPlan( return current_goal_local; } -geometry_msgs::Twist Controller::update( +Controller::UpdateResult Controller::update( double target_x_vel, double target_end_x_vel, const geometry_msgs::Transform & current_tf, - const geometry_msgs::Twist & odom_twist, ros::Duration dt, double * eda, double * progress, - path_tracking_pid::PidDebug * pid_debug) + const geometry_msgs::Twist & odom_twist, ros::Duration dt) { + UpdateResult result; + const double current_x_vel = controller_state_.current_x_vel; const double current_yaw_vel = controller_state_.current_yaw_vel; @@ -473,7 +474,7 @@ geometry_msgs::Twist Controller::update( findPositionOnPlan(current_with_carrot_g, &controller_state_, path_pose_idx); } - *progress = 1.0 - distance_to_goal_ / distance_to_goal_vector_[0]; + result.progress = 1.0 - distance_to_goal_ / distance_to_goal_vector_[0]; // Compute errorPose between controlPose and currentGoalPose tf2::Transform error = current_with_carrot_.inverseTimes(current_goal_); @@ -657,8 +658,8 @@ geometry_msgs::Twist Controller::update( new_x_vel <= target_end_x_vel + VELOCITY_EPS)) { controller_state_.end_reached = true; controller_state_.end_phase_enabled = false; - *progress = 1.0; - *eda = 0.0; + result.progress = 1.0; + result.eda = 0.0; enabled_ = false; } else { controller_state_.end_reached = false; @@ -666,9 +667,9 @@ geometry_msgs::Twist Controller::update( if (fabs(target_x_vel) > VELOCITY_EPS) { const double t_const = (copysign(distance_to_goal_, target_x_vel) - d_end_phase) / target_x_vel; - *eda = fmin(fmax(t_end_phase_current, 0.0) + fmax(t_const, 0.0), LONG_DURATION); + result.eda = fmin(fmax(t_end_phase_current, 0.0) + fmax(t_const, 0.0), LONG_DURATION); } else { - *eda = LONG_DURATION; + result.eda = LONG_DURATION; } } /******* end calculation of forward velocity ********/ @@ -712,52 +713,51 @@ geometry_msgs::Twist Controller::update( // Populate debug output // Error topic containing the 'control' error on which the PID acts - pid_debug->control_error.linear.x = 0.0; - pid_debug->control_error.linear.y = controller_state_.error_lat.errors().at<0>(); - pid_debug->control_error.angular.z = controller_state_.error_ang.errors().at<0>(); + result.pid_debug.control_error.linear.x = 0.0; + result.pid_debug.control_error.linear.y = controller_state_.error_lat.errors().at<0>(); + result.pid_debug.control_error.angular.z = controller_state_.error_ang.errors().at<0>(); // Error topic containing the 'tracking' error, i.e. the real error between path and tracked link - pid_debug->tracking_error.linear.x = 0.0; - pid_debug->tracking_error.linear.y = controller_state_.tracking_error_lat; - pid_debug->tracking_error.angular.z = controller_state_.tracking_error_ang; + result.pid_debug.tracking_error.linear.x = 0.0; + result.pid_debug.tracking_error.linear.y = controller_state_.tracking_error_lat; + result.pid_debug.tracking_error.angular.z = controller_state_.tracking_error_ang; - pid_debug->proportional.linear.x = 0.0; - pid_debug->proportional.linear.y = proportional_lat; - pid_debug->proportional.angular.z = proportional_ang; + result.pid_debug.proportional.linear.x = 0.0; + result.pid_debug.proportional.linear.y = proportional_lat; + result.pid_debug.proportional.angular.z = proportional_ang; - pid_debug->integral.linear.x = 0.0; - pid_debug->integral.linear.y = integral_lat; - pid_debug->integral.angular.z = integral_ang; + result.pid_debug.integral.linear.x = 0.0; + result.pid_debug.integral.linear.y = integral_lat; + result.pid_debug.integral.angular.z = integral_ang; - pid_debug->derivative.linear.x = 0.0; - pid_debug->derivative.linear.y = derivative_lat; - pid_debug->derivative.angular.z = derivative_ang; + result.pid_debug.derivative.linear.x = 0.0; + result.pid_debug.derivative.linear.y = derivative_lat; + result.pid_debug.derivative.angular.z = derivative_ang; - pid_debug->feedforward.linear.x = new_x_vel; - pid_debug->feedforward.linear.y = feedforward_lat_; - pid_debug->feedforward.angular.z = feedforward_ang_; + result.pid_debug.feedforward.linear.x = new_x_vel; + result.pid_debug.feedforward.linear.y = feedforward_lat_; + result.pid_debug.feedforward.angular.z = feedforward_ang_; - geometry_msgs::Twist output_combined; // Generate twist message if (holonomic_robot_enable_) { - output_combined.linear.x = control_effort_long_; - output_combined.linear.y = control_effort_lat_; - output_combined.linear.z = 0; - output_combined.angular.x = 0; - output_combined.angular.y = 0; - output_combined.angular.z = control_effort_ang_; - output_combined.angular.z = - std::clamp(output_combined.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); + result.velocity_command.linear.x = control_effort_long_; + result.velocity_command.linear.y = control_effort_lat_; + result.velocity_command.linear.z = 0; + result.velocity_command.angular.x = 0; + result.velocity_command.angular.y = 0; + result.velocity_command.angular.z = control_effort_ang_; + result.velocity_command.angular.z = + std::clamp(result.velocity_command.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); } else { - output_combined.linear.x = control_effort_long_; - output_combined.linear.y = 0; - output_combined.linear.z = 0; - output_combined.angular.x = 0; - output_combined.angular.y = 0; - output_combined.angular.z = + result.velocity_command.linear.x = control_effort_long_; + result.velocity_command.linear.y = 0; + result.velocity_command.linear.z = 0; + result.velocity_command.angular.x = 0; + result.velocity_command.angular.y = 0; + result.velocity_command.angular.z = copysign(1.0, config_.l) * control_effort_lat_ + control_effort_ang_; // Take the sign of l for the lateral control effort - output_combined.angular.z = - std::clamp(output_combined.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); + result.velocity_command.angular.z = + std::clamp(result.velocity_command.angular.z, -config_.max_yaw_vel, config_.max_yaw_vel); // For non-holonomic robots apply saturation based on minimum turning radius double max_ang_twist_tr; if (config_.min_turning_radius < RADIUS_EPS) { @@ -766,23 +766,24 @@ geometry_msgs::Twist Controller::update( // do not restrict angular velocity. Thus use the biggets number possible max_ang_twist_tr = std::numeric_limits::infinity(); } else { - max_ang_twist_tr = fabs(output_combined.linear.x / config_.min_turning_radius); + max_ang_twist_tr = fabs(result.velocity_command.linear.x / config_.min_turning_radius); } - output_combined.angular.z = - std::clamp(output_combined.angular.z, -max_ang_twist_tr, max_ang_twist_tr); + result.velocity_command.angular.z = + std::clamp(result.velocity_command.angular.z, -max_ang_twist_tr, max_ang_twist_tr); } // Apply max acceleration limit to yaw const double yaw_acc = std::clamp( - (output_combined.angular.z - current_yaw_vel) / dt.toSec(), -config_.max_yaw_acc, + (result.velocity_command.angular.z - current_yaw_vel) / dt.toSec(), -config_.max_yaw_acc, config_.max_yaw_acc); const double new_yaw_vel = current_yaw_vel + (yaw_acc * dt.toSec()); - output_combined.angular.z = new_yaw_vel; + result.velocity_command.angular.z = new_yaw_vel; // Transform velocity commands at base_link to steer when using tricycle model if (use_tricycle_model_) { geometry_msgs::Twist output_steering; - TricycleSteeringCmdVel steering_cmd = computeTricycleModelInverseKinematics(output_combined); - if (output_combined.linear.x < 0.0 && steering_cmd.speed > 0.0) { + TricycleSteeringCmdVel steering_cmd = + computeTricycleModelInverseKinematics(result.velocity_command); + if (result.velocity_command.linear.x < 0.0 && steering_cmd.speed > 0.0) { steering_cmd.speed = -steering_cmd.speed; if (steering_cmd.steering_angle > 0) { steering_cmd.steering_angle = steering_cmd.steering_angle - M_PI; @@ -822,11 +823,11 @@ geometry_msgs::Twist Controller::update( controller_state_.current_x_vel = output_steering.linear.x; controller_state_.current_yaw_vel = output_steering.angular.z; - pid_debug->steering_angle = steering_cmd.steering_angle; - pid_debug->steering_yaw_vel = steering_cmd.steering_angle_velocity; - pid_debug->steering_x_vel = steering_cmd.speed; + result.pid_debug.steering_angle = steering_cmd.steering_angle; + result.pid_debug.steering_yaw_vel = steering_cmd.steering_angle_velocity; + result.pid_debug.steering_x_vel = steering_cmd.speed; - output_combined = output_steering; + result.velocity_command = output_steering; } // Publish control effort if controller enabled @@ -838,12 +839,13 @@ geometry_msgs::Twist Controller::update( controller_state_.current_x_vel = new_x_vel; controller_state_.current_yaw_vel = new_yaw_vel; - return output_combined; + + return result; } -geometry_msgs::Twist Controller::update_with_limits( +Controller::UpdateResult Controller::update_with_limits( const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, - ros::Duration dt, double * eda, double * progress, path_tracking_pid::PidDebug * pid_debug) + ros::Duration dt) { // All limits are absolute double max_x_vel = std::abs(config_.target_x_vel); @@ -884,7 +886,7 @@ geometry_msgs::Twist Controller::update_with_limits( // Update the controller with the new setting max_x_vel = std::copysign(max_x_vel, config_.target_x_vel); - return update(max_x_vel, max_end_x_vel, current_tf, odom_twist, dt, eda, progress, pid_debug); + return update(max_x_vel, max_end_x_vel, current_tf, odom_twist, dt); } // output updated velocity command: (Current position, current measured velocity, closest point index, estimated @@ -965,13 +967,10 @@ double Controller::mpc_based_max_vel( } else if (mpc_fwd_iter != config_.mpc_max_fwd_iterations) { // Run controller // Output: pred_twist.[linear.x, linear.y, linear.z, angular.x, angular.y, angular.z] - path_tracking_pid::PidDebug pid_debug_unused; - double eda_unused; - double progress_unused; pred_twist = Controller::update( - new_nominal_x_vel, config_.target_end_x_vel, predicted_tf, pred_twist, - ros::Duration(config_.mpc_simulation_sample_time), &eda_unused, &progress_unused, - &pid_debug_unused); + new_nominal_x_vel, config_.target_end_x_vel, predicted_tf, pred_twist, + ros::Duration(config_.mpc_simulation_sample_time)) + .velocity_command; // Run plant model const double theta = tf2::getYaw(predicted_tf.rotation); diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 8e080b00..0521567e 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -237,16 +237,13 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd pid_controller_.setVelMaxObstacle(INFINITY); // Can be disabled live, so set back to inf } - path_tracking_pid::PidDebug pid_debug; - double eda = - 1 / FLT_EPSILON; // initial guess. Avoids errors in case function returns due to wrong delta_t; - double progress = 0.0; - cmd_vel = pid_controller_.update_with_limits( - tfCurPoseStamped_.transform, latest_odom_.twist.twist, dt, &eda, &progress, &pid_debug); + const auto update_result = + pid_controller_.update_with_limits(tfCurPoseStamped_.transform, latest_odom_.twist.twist, dt); + cmd_vel = update_result.velocity_command; path_tracking_pid::PidFeedback feedback_msg; - feedback_msg.eda = ros::Duration(eda); - feedback_msg.progress = progress; + feedback_msg.eda = ros::Duration(update_result.eda); + feedback_msg.progress = update_result.progress; feedback_pub_.publish(feedback_msg); if (cancel_requested_) { @@ -264,7 +261,7 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd } if (controller_debug_enabled_) { - debug_pub_.publish(pid_debug); + debug_pub_.publish(update_result.pid_debug); // publish rviz visualization std_msgs::Header header; From fb18c4651dd34e655b148d27230274b4fda10026 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 23 Feb 2022 08:48:58 +0000 Subject: [PATCH 088/156] Refactored output parameters of distToSegmentSquared(). --- include/path_tracking_pid/controller.hpp | 22 +++----- src/controller.cpp | 72 +++++++++++------------- 2 files changed, 43 insertions(+), 51 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 6e05c0d6..085f0c1e 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -192,21 +192,17 @@ class Controller : private boost::noncopyable double getVelMaxObstacle() const; private: - void distToSegmentSquared( - const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, - tf2::Transform & pose_projection, double & distance_to_p, double & distance_to_w) const; + // Result of distToSegmentSquared(). + struct DistToSegmentSquaredResult + { + tf2::Transform pose_projection; + double distance2_to_p = 0; // Square distance in meter squared. + double distance_to_w = 0; // Distance in meter. + }; - // Overloaded function for callers that don't need the additional results - double distToSegmentSquared( + DistToSegmentSquaredResult distToSegmentSquared( const tf2::Transform & pose_p, const tf2::Transform & pose_v, - const tf2::Transform & pose_w) const - { - tf2::Transform dummy_tf; - double dummy_double; - double result; - distToSegmentSquared(pose_p, pose_v, pose_w, dummy_tf, result, dummy_double); - return result; - } + const tf2::Transform & pose_w) const; geometry_msgs::Twist computeTricycleModelForwardKinematics(double x_vel, double steering_angle); TricycleSteeringCmdVel computeTricycleModelInverseKinematics( diff --git a/src/controller.cpp b/src/controller.cpp index 2b733d0c..54406ff3 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -215,7 +215,8 @@ void Controller::setPlan( for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ dist_to_segment = - distToSegmentSquared(current_tf2, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]); + distToSegmentSquared(current_tf2, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]) + .distance2_to_p; // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! if (dist_to_segment < minimum_distance_to_path) { @@ -281,15 +282,16 @@ void Controller::setPlan( // TODO(clopez) use steering_odom_twist to check if setpoint is being followed } -void Controller::distToSegmentSquared( - const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, - tf2::Transform & pose_projection, double & distance_to_p, double & distance_to_w) const +Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( + const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w) const { + DistToSegmentSquaredResult result; + const double l2 = distSquared(pose_v, pose_w); if (l2 == 0) { - pose_projection = pose_w; - distance_to_w = 0.0; - distance_to_p = distSquared(pose_p, pose_w); + result.pose_projection = pose_w; + result.distance_to_w = 0.0; + result.distance2_to_p = distSquared(pose_p, pose_w); } else { double t = ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) * (pose_w.getOrigin().x() - pose_v.getOrigin().x()) + @@ -297,7 +299,7 @@ void Controller::distToSegmentSquared( (pose_w.getOrigin().y() - pose_v.getOrigin().y())) / l2; t = fmax(0.0, fmin(1.0, t)); - pose_projection.setOrigin(tf2::Vector3( + result.pose_projection.setOrigin(tf2::Vector3( pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()), pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); double yaw_projection = tf2::getYaw(pose_v.getRotation()); // get yaw of the first vector + t * @@ -314,10 +316,12 @@ void Controller::distToSegmentSquared( pose_quaternion.setRPY(0.0, 0.0, yaw_projection); } - pose_projection.setRotation(pose_quaternion); - distance_to_w = sqrt(distSquared(pose_projection, pose_w)); - distance_to_p = distSquared(pose_p, pose_projection); + result.pose_projection.setRotation(pose_quaternion); + result.distance_to_w = sqrt(distSquared(result.pose_projection, pose_w)); + result.distance2_to_p = distSquared(pose_p, result.pose_projection); } + + return result; } tf2::Transform Controller::findPositionOnPlan( @@ -380,51 +384,43 @@ tf2::Transform Controller::findPositionOnPlan( tf2::Transform current_goal_local; current_goal_local = global_plan_tf_[controller_state_ptr->current_global_plan_index]; - tf2::Transform pose_projection_ahead; - tf2::Transform pose_projection_behind; - double distance2_to_line_ahead; - double distance2_to_line_behind; - double distance_to_end_line_ahead; - double distance_to_end_line_behind; if (controller_state_ptr->current_global_plan_index == 0) { - distToSegmentSquared( - current_tf2, global_plan_tf_[0], global_plan_tf_[1], pose_projection_ahead, - distance2_to_line_ahead, distance_to_end_line_ahead); - current_goal_local = pose_projection_ahead; - distance_to_goal_ = distance_to_goal_vector_[1] + distance_to_end_line_ahead; + const auto dist_result = + distToSegmentSquared(current_tf2, global_plan_tf_[0], global_plan_tf_[1]); + + current_goal_local = dist_result.pose_projection; + distance_to_goal_ = distance_to_goal_vector_[1] + dist_result.distance_to_w; controller_state_ptr->last_visited_pose_index = 0; path_pose_idx = controller_state_ptr->current_global_plan_index; } else if (controller_state_ptr->current_global_plan_index == global_plan_tf_.size() - 1) { - distToSegmentSquared( + const auto dist_result = distToSegmentSquared( current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], - global_plan_tf_[controller_state_ptr->current_global_plan_index], pose_projection_behind, - distance2_to_line_behind, distance_to_end_line_behind); - current_goal_local = pose_projection_behind; - distance_to_goal_ = distance_to_end_line_behind; + global_plan_tf_[controller_state_ptr->current_global_plan_index]); + + current_goal_local = dist_result.pose_projection; + distance_to_goal_ = dist_result.distance_to_w; controller_state_ptr->last_visited_pose_index = global_plan_tf_.size() - 2; path_pose_idx = controller_state_ptr->current_global_plan_index - 1; } else { - distToSegmentSquared( + const auto dist_result_ahead = distToSegmentSquared( current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index], - global_plan_tf_[controller_state_ptr->current_global_plan_index + 1], pose_projection_ahead, - distance2_to_line_ahead, distance_to_end_line_ahead); - distToSegmentSquared( + global_plan_tf_[controller_state_ptr->current_global_plan_index + 1]); + const auto dist_result_behind = distToSegmentSquared( current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], - global_plan_tf_[controller_state_ptr->current_global_plan_index], pose_projection_behind, - distance2_to_line_behind, distance_to_end_line_behind); + global_plan_tf_[controller_state_ptr->current_global_plan_index]); - if (distance2_to_line_ahead < distance2_to_line_behind) { - current_goal_local = pose_projection_ahead; + if (dist_result_ahead.distance2_to_p < dist_result_behind.distance2_to_p) { + current_goal_local = dist_result_ahead.pose_projection; distance_to_goal_ = distance_to_goal_vector_[controller_state_ptr->current_global_plan_index + 1] + - distance_to_end_line_ahead; + dist_result_ahead.distance_to_w; controller_state_ptr->last_visited_pose_index = controller_state_ptr->current_global_plan_index; } else { - current_goal_local = pose_projection_behind; + current_goal_local = dist_result_behind.pose_projection; distance_to_goal_ = distance_to_goal_vector_[controller_state_ptr->current_global_plan_index] + - distance_to_end_line_behind; + dist_result_behind.distance_to_w; controller_state_ptr->last_visited_pose_index = controller_state_ptr->current_global_plan_index - 1; } From 2f383406bc69c54d60b5d6352ad094d32d0926c2 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 23 Feb 2022 09:35:46 +0000 Subject: [PATCH 089/156] Refactored distToSegmentSquared(). --- src/controller.cpp | 63 +++++++++++++++++++++++++--------------------- 1 file changed, 35 insertions(+), 28 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 54406ff3..ad36817b 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -36,6 +36,14 @@ bool have_same_sign(T val1, T val2) return std::signbit(val1) == std::signbit(val2); } +// Floating-point aware version of std::clamp(). fclamp() is to std::clamp() what std::fmin() and +// std::fmax() are to std::min() and std::max(). +template +T fclamp(T value, T low, T high) +{ + return std::fmax(low, std::fmin(value, high)); +} + // Converts a pose to the corresponding transform. tf2::Transform to_transform(const geometry_msgs::Pose & pose) { @@ -285,42 +293,41 @@ void Controller::setPlan( Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w) const { - DistToSegmentSquaredResult result; - const double l2 = distSquared(pose_v, pose_w); if (l2 == 0) { + DistToSegmentSquaredResult result; + result.pose_projection = pose_w; result.distance_to_w = 0.0; result.distance2_to_p = distSquared(pose_p, pose_w); - } else { - double t = ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) * - (pose_w.getOrigin().x() - pose_v.getOrigin().x()) + - (pose_p.getOrigin().y() - pose_v.getOrigin().y()) * - (pose_w.getOrigin().y() - pose_v.getOrigin().y())) / - l2; - t = fmax(0.0, fmin(1.0, t)); - result.pose_projection.setOrigin(tf2::Vector3( - pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()), - pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); - double yaw_projection = tf2::getYaw(pose_v.getRotation()); // get yaw of the first vector + t * - // (tf2::getYaw(pose_w.getRotation()) - - // tf2::getYaw(pose_v.getRotation())); - tf2::Quaternion pose_quaternion; - if (estimate_pose_angle_enabled_) { - pose_quaternion.setRPY( - 0.0, 0.0, - atan2( - pose_w.getOrigin().y() - pose_v.getOrigin().y(), - pose_w.getOrigin().x() - pose_v.getOrigin().x())); - } else { - pose_quaternion.setRPY(0.0, 0.0, yaw_projection); - } - result.pose_projection.setRotation(pose_quaternion); - result.distance_to_w = sqrt(distSquared(result.pose_projection, pose_w)); - result.distance2_to_p = distSquared(pose_p, result.pose_projection); + return result; } + DistToSegmentSquaredResult result; + + const double t = fclamp( + ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) * + (pose_w.getOrigin().x() - pose_v.getOrigin().x()) + + (pose_p.getOrigin().y() - pose_v.getOrigin().y()) * + (pose_w.getOrigin().y() - pose_v.getOrigin().y())) / + l2, + 0.0, 1.0); + result.pose_projection.setOrigin(tf2::Vector3( + pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()), + pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); + + const auto yaw = estimate_pose_angle_enabled_ ? atan2( + pose_w.getOrigin().y() - pose_v.getOrigin().y(), + pose_w.getOrigin().x() - pose_v.getOrigin().x()) + : tf2::getYaw(pose_v.getRotation()); + tf2::Quaternion pose_quaternion; + pose_quaternion.setRPY(0.0, 0.0, yaw); + result.pose_projection.setRotation(pose_quaternion); + + result.distance_to_w = sqrt(distSquared(result.pose_projection, pose_w)); + result.distance2_to_p = distSquared(pose_p, result.pose_projection); + return result; } From 5c81ea7348f6fce99d88c71dc2b90e297c53ab99 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 23 Feb 2022 10:05:25 +0000 Subject: [PATCH 090/156] Refactored findPositionOnPlan(). --- src/controller.cpp | 58 ++++++++++++++++++++++++---------------------- 1 file changed, 30 insertions(+), 28 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index ad36817b..2900ad95 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -388,52 +388,54 @@ tf2::Transform Controller::findPositionOnPlan( global_plan_tf_.size() - 1); // To finalize, compute the indexes of the start and end points of // the closest line segment to the current carrot - tf2::Transform current_goal_local; - current_goal_local = global_plan_tf_[controller_state_ptr->current_global_plan_index]; if (controller_state_ptr->current_global_plan_index == 0) { const auto dist_result = distToSegmentSquared(current_tf2, global_plan_tf_[0], global_plan_tf_[1]); - current_goal_local = dist_result.pose_projection; distance_to_goal_ = distance_to_goal_vector_[1] + dist_result.distance_to_w; controller_state_ptr->last_visited_pose_index = 0; path_pose_idx = controller_state_ptr->current_global_plan_index; - } else if (controller_state_ptr->current_global_plan_index == global_plan_tf_.size() - 1) { + + return dist_result.pose_projection; + } + + if (controller_state_ptr->current_global_plan_index == global_plan_tf_.size() - 1) { const auto dist_result = distToSegmentSquared( current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], global_plan_tf_[controller_state_ptr->current_global_plan_index]); - current_goal_local = dist_result.pose_projection; distance_to_goal_ = dist_result.distance_to_w; controller_state_ptr->last_visited_pose_index = global_plan_tf_.size() - 2; path_pose_idx = controller_state_ptr->current_global_plan_index - 1; - } else { - const auto dist_result_ahead = distToSegmentSquared( - current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index], - global_plan_tf_[controller_state_ptr->current_global_plan_index + 1]); - const auto dist_result_behind = distToSegmentSquared( - current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], - global_plan_tf_[controller_state_ptr->current_global_plan_index]); - if (dist_result_ahead.distance2_to_p < dist_result_behind.distance2_to_p) { - current_goal_local = dist_result_ahead.pose_projection; - distance_to_goal_ = - distance_to_goal_vector_[controller_state_ptr->current_global_plan_index + 1] + - dist_result_ahead.distance_to_w; - controller_state_ptr->last_visited_pose_index = - controller_state_ptr->current_global_plan_index; - } else { - current_goal_local = dist_result_behind.pose_projection; - distance_to_goal_ = - distance_to_goal_vector_[controller_state_ptr->current_global_plan_index] + - dist_result_behind.distance_to_w; - controller_state_ptr->last_visited_pose_index = - controller_state_ptr->current_global_plan_index - 1; - } + return dist_result.pose_projection; + } + + const auto dist_result_ahead = distToSegmentSquared( + current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index], + global_plan_tf_[controller_state_ptr->current_global_plan_index + 1]); + const auto dist_result_behind = distToSegmentSquared( + current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], + global_plan_tf_[controller_state_ptr->current_global_plan_index]); + + if (dist_result_ahead.distance2_to_p < dist_result_behind.distance2_to_p) { + distance_to_goal_ = + distance_to_goal_vector_[controller_state_ptr->current_global_plan_index + 1] + + dist_result_ahead.distance_to_w; + controller_state_ptr->last_visited_pose_index = controller_state_ptr->current_global_plan_index; path_pose_idx = controller_state_ptr->current_global_plan_index; + + return dist_result_ahead.pose_projection; } - return current_goal_local; + + distance_to_goal_ = distance_to_goal_vector_[controller_state_ptr->current_global_plan_index] + + dist_result_behind.distance_to_w; + controller_state_ptr->last_visited_pose_index = + controller_state_ptr->current_global_plan_index - 1; + path_pose_idx = controller_state_ptr->current_global_plan_index; + + return dist_result_behind.pose_projection; } Controller::UpdateResult Controller::update( From 27794c5225eb2e903394666e0dc66d451dcb9dbc Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 07:34:39 +0000 Subject: [PATCH 091/156] Reworked by adding documentation. --- include/path_tracking_pid/controller.hpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 085f0c1e..b520c0c0 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -200,6 +200,16 @@ class Controller : private boost::noncopyable double distance_to_w = 0; // Distance in meter. }; + /** + * @brief Closest point between a line segment and a point + * Calculate the closest point between the line segment bounded by PV and the point W. + * + * @param[in] pose_p Start of the line segment + * @param[in] pose_v End of the line segment + * @param[in] pose_w The point + * @return The pose projection of the closest point with the distance (squared) to pose_p and + * the distance to pose_w. + */ DistToSegmentSquaredResult distToSegmentSquared( const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w) const; From 029012c708ff84d5d036f8ca5cef185e258756d0 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 09:29:38 +0000 Subject: [PATCH 092/156] Reworked by switching to std::clamp(). --- src/controller.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index ad36817b..b91b44bc 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -36,14 +36,6 @@ bool have_same_sign(T val1, T val2) return std::signbit(val1) == std::signbit(val2); } -// Floating-point aware version of std::clamp(). fclamp() is to std::clamp() what std::fmin() and -// std::fmax() are to std::min() and std::max(). -template -T fclamp(T value, T low, T high) -{ - return std::fmax(low, std::fmin(value, high)); -} - // Converts a pose to the corresponding transform. tf2::Transform to_transform(const geometry_msgs::Pose & pose) { @@ -306,7 +298,7 @@ Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( DistToSegmentSquaredResult result; - const double t = fclamp( + const double t = std::clamp( ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) * (pose_w.getOrigin().x() - pose_v.getOrigin().x()) + (pose_p.getOrigin().y() - pose_v.getOrigin().y()) * From dad418d4c05d85079ef02dcc452e077153b41eb5 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 21 Feb 2022 15:55:36 +0000 Subject: [PATCH 093/156] Refactored mandatory ptr parameter to reference. --- include/path_tracking_pid/controller.hpp | 8 ++-- src/controller.cpp | 57 +++++++++++------------- src/path_tracking_pid_local_planner.cpp | 4 +- 3 files changed, 33 insertions(+), 36 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index b520c0c0..df6cb638 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -94,19 +94,19 @@ class Controller : private boost::noncopyable /** * Find position on plan by looking at the surroundings of last known pose. * @param current Where is the robot now? - * @param controller_state_ptr The current state of the controller that gets updated by this function + * @param controller_state The current state of the controller that gets updated by this function * @return tf of found position on plan * @return index of current path-pose if requested */ tf2::Transform findPositionOnPlan( - const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr, + const geometry_msgs::Transform & current_tf, ControllerState & controller_state, size_t & path_pose_idx); // Overloaded function definition for users that don't require the segment index tf2::Transform findPositionOnPlan( - const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr) + const geometry_msgs::Transform & current_tf, ControllerState & controller_state) { size_t path_pose_idx; - return findPositionOnPlan(current_tf, controller_state_ptr, path_pose_idx); + return findPositionOnPlan(current_tf, controller_state, path_pose_idx); } // Result of update() and update_with_limits(). diff --git a/src/controller.cpp b/src/controller.cpp index 97857799..38216296 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -324,7 +324,7 @@ Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( } tf2::Transform Controller::findPositionOnPlan( - const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr, + const geometry_msgs::Transform & current_tf, ControllerState & controller_state, size_t & path_pose_idx) { tf2::Transform current_tf2; @@ -346,7 +346,7 @@ tf2::Transform Controller::findPositionOnPlan( // Hence, when idx_path==i we are currently tracking the section connection pose i and pose i+1 // First look in current position and in front - for (auto idx_path = controller_state_ptr->current_global_plan_index; + for (auto idx_path = controller_state.current_global_plan_index; idx_path < global_plan_tf_.size(); idx_path++) { error = current_tf2.inverseTimes(global_plan_tf_[idx_path]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose @@ -355,14 +355,14 @@ tf2::Transform Controller::findPositionOnPlan( if (distance_to_path <= minimum_distance_to_path) { minimum_distance_to_path = distance_to_path; - controller_state_ptr->current_global_plan_index = idx_path; + controller_state.current_global_plan_index = idx_path; } else { break; } } // Then look backwards - for (auto idx_path = controller_state_ptr->current_global_plan_index; idx_path > 0; --idx_path) { + for (auto idx_path = controller_state.current_global_plan_index; idx_path > 0; --idx_path) { error = current_tf2.inverseTimes(global_plan_tf_[idx_path - 1]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! @@ -370,62 +370,59 @@ tf2::Transform Controller::findPositionOnPlan( if (distance_to_path < minimum_distance_to_path) { minimum_distance_to_path = distance_to_path; - controller_state_ptr->current_global_plan_index = idx_path - 1; + controller_state.current_global_plan_index = idx_path - 1; } else { break; } } ROS_DEBUG( - "progress: %lu of %lu", controller_state_ptr->current_global_plan_index, - global_plan_tf_.size() - 1); + "progress: %lu of %lu", controller_state.current_global_plan_index, global_plan_tf_.size() - 1); // To finalize, compute the indexes of the start and end points of // the closest line segment to the current carrot - if (controller_state_ptr->current_global_plan_index == 0) { + if (controller_state.current_global_plan_index == 0) { const auto dist_result = distToSegmentSquared(current_tf2, global_plan_tf_[0], global_plan_tf_[1]); distance_to_goal_ = distance_to_goal_vector_[1] + dist_result.distance_to_w; - controller_state_ptr->last_visited_pose_index = 0; - path_pose_idx = controller_state_ptr->current_global_plan_index; + controller_state.last_visited_pose_index = 0; + path_pose_idx = controller_state.current_global_plan_index; return dist_result.pose_projection; } - if (controller_state_ptr->current_global_plan_index == global_plan_tf_.size() - 1) { + if (controller_state.current_global_plan_index == global_plan_tf_.size() - 1) { const auto dist_result = distToSegmentSquared( - current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], - global_plan_tf_[controller_state_ptr->current_global_plan_index]); + current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], + global_plan_tf_[controller_state.current_global_plan_index]); distance_to_goal_ = dist_result.distance_to_w; - controller_state_ptr->last_visited_pose_index = global_plan_tf_.size() - 2; - path_pose_idx = controller_state_ptr->current_global_plan_index - 1; + controller_state.last_visited_pose_index = global_plan_tf_.size() - 2; + path_pose_idx = controller_state.current_global_plan_index - 1; return dist_result.pose_projection; } const auto dist_result_ahead = distToSegmentSquared( - current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index], - global_plan_tf_[controller_state_ptr->current_global_plan_index + 1]); + current_tf2, global_plan_tf_[controller_state.current_global_plan_index], + global_plan_tf_[controller_state.current_global_plan_index + 1]); const auto dist_result_behind = distToSegmentSquared( - current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1], - global_plan_tf_[controller_state_ptr->current_global_plan_index]); + current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], + global_plan_tf_[controller_state.current_global_plan_index]); if (dist_result_ahead.distance2_to_p < dist_result_behind.distance2_to_p) { - distance_to_goal_ = - distance_to_goal_vector_[controller_state_ptr->current_global_plan_index + 1] + - dist_result_ahead.distance_to_w; - controller_state_ptr->last_visited_pose_index = controller_state_ptr->current_global_plan_index; - path_pose_idx = controller_state_ptr->current_global_plan_index; + distance_to_goal_ = distance_to_goal_vector_[controller_state.current_global_plan_index + 1] + + dist_result_ahead.distance_to_w; + controller_state.last_visited_pose_index = controller_state.current_global_plan_index; + path_pose_idx = controller_state.current_global_plan_index; return dist_result_ahead.pose_projection; } - distance_to_goal_ = distance_to_goal_vector_[controller_state_ptr->current_global_plan_index] + + distance_to_goal_ = distance_to_goal_vector_[controller_state.current_global_plan_index] + dist_result_behind.distance_to_w; - controller_state_ptr->last_visited_pose_index = - controller_state_ptr->current_global_plan_index - 1; - path_pose_idx = controller_state_ptr->current_global_plan_index; + controller_state.last_visited_pose_index = controller_state.current_global_plan_index - 1; + path_pose_idx = controller_state.current_global_plan_index; return dist_result_behind.pose_projection; } @@ -455,7 +452,7 @@ Controller::UpdateResult Controller::update( if (config_.track_base_link) { // Find closes robot position to path and then project carrot on goal current_pos_on_plan_ = current_goal_ = - findPositionOnPlan(current_tf, &controller_state_, path_pose_idx); + findPositionOnPlan(current_tf, controller_state_, path_pose_idx); // To track the base link the goal is then transform to the control point goal double theda_rp = tf2::getYaw(current_goal_.getRotation()); tf2::Vector3 newControlOrigin; @@ -468,7 +465,7 @@ Controller::UpdateResult Controller::update( geometry_msgs::Transform current_with_carrot_g; tf2::convert(current_with_carrot_, current_with_carrot_g); current_pos_on_plan_ = current_goal_ = - findPositionOnPlan(current_with_carrot_g, &controller_state_, path_pose_idx); + findPositionOnPlan(current_with_carrot_g, controller_state_, path_pose_idx); } result.progress = 1.0 - distance_to_goal_ / distance_to_goal_vector_[0]; diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 0521567e..ad266653 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -373,12 +373,12 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() tf2::Transform projected_step_tf; tf2::fromMsg(current_tf, projected_step_tf); projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link - projected_step_tf = pid_controller_.findPositionOnPlan(current_tf, &projected_controller_state); + projected_step_tf = pid_controller_.findPositionOnPlan(current_tf, projected_controller_state); projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose for (uint step = 0; step < n_steps; step++) { tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf; projected_step_tf = pid_controller_.findPositionOnPlan( - tf2::toMsg(next_straight_step_tf), &projected_controller_state); + tf2::toMsg(next_straight_step_tf), projected_controller_state); projected_steps_tf.push_back(projected_step_tf); // Fill markers: From 05e3c2680651f9680662dfce55cbbdc15196a68d Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 23 Feb 2022 11:38:14 +0000 Subject: [PATCH 094/156] Split loop into 3. --- src/controller.cpp | 28 +++++++++++++++++----------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 38216296..dd7aa68d 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -201,20 +201,11 @@ void Controller::setPlan( // find closest current position to global plan double minimum_distance_to_path = 1e3; - double dist_to_segment; - double iterative_dist_to_goal = 0.0; - distance_to_goal_vector_.clear(); - distance_to_goal_vector_.resize(global_plan_tf_.size()); - distance_to_goal_vector_[global_plan_tf_.size() - 1] = 0.0; - turning_radius_inv_vector_.clear(); - turning_radius_inv_vector_.resize(global_plan_tf_.size()); - turning_radius_inv_vector_[global_plan_tf_.size() - 1] = 0.0; - tf2::Transform deltaPlan; // We define segment0 to be the segment connecting pose0 and pose1. // Hence, when picking the starting path's pose, we mean to start at the segment connecting that and the next pose. for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ - dist_to_segment = + const auto dist_to_segment = distToSegmentSquared(current_tf2, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]) .distance2_to_p; // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose @@ -223,15 +214,30 @@ void Controller::setPlan( minimum_distance_to_path = dist_to_segment; controller_state_.current_global_plan_index = idx_path; } + } + double iterative_dist_to_goal = 0.0; + distance_to_goal_vector_.clear(); + distance_to_goal_vector_.resize(global_plan_tf_.size()); + distance_to_goal_vector_[global_plan_tf_.size() - 1] = 0.0; + for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Create distance and turning radius vectors once for usage later */ - deltaPlan = global_plan_tf_[idx_path].inverseTimes(global_plan_tf_[idx_path + 1]); + const auto deltaPlan = global_plan_tf_[idx_path].inverseTimes(global_plan_tf_[idx_path + 1]); const double dpX = deltaPlan.getOrigin().x(); const double dpY = deltaPlan.getOrigin().y(); iterative_dist_to_goal += hypot(dpX, dpY); distance_to_goal_vector_[idx_path] = iterative_dist_to_goal; + } + + turning_radius_inv_vector_.clear(); + turning_radius_inv_vector_.resize(global_plan_tf_.size()); + turning_radius_inv_vector_[global_plan_tf_.size() - 1] = 0.0; + for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { // compute turning radius based on trigonometric analysis // radius such that next pose is connected from current pose with a semi-circle + const auto deltaPlan = global_plan_tf_[idx_path].inverseTimes(global_plan_tf_[idx_path + 1]); + const double dpX = deltaPlan.getOrigin().x(); + const double dpY = deltaPlan.getOrigin().y(); const double dpXY2 = dpY * dpY + dpX * dpX; if (dpXY2 < FLT_EPSILON) { turning_radius_inv_vector_[idx_path] = std::numeric_limits::infinity(); From a97a7f64adb054c8ed7f6451fb4e81eddc9de31d Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 23 Feb 2022 14:25:38 +0000 Subject: [PATCH 095/156] Replaced some loops with algorithms. --- src/controller.cpp | 69 +++++++++++++++++++++++++++------------------- 1 file changed, 40 insertions(+), 29 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index dd7aa68d..399d0258 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -216,38 +217,48 @@ void Controller::setPlan( } } - double iterative_dist_to_goal = 0.0; - distance_to_goal_vector_.clear(); - distance_to_goal_vector_.resize(global_plan_tf_.size()); - distance_to_goal_vector_[global_plan_tf_.size() - 1] = 0.0; - for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { - /* Create distance and turning radius vectors once for usage later */ - const auto deltaPlan = global_plan_tf_[idx_path].inverseTimes(global_plan_tf_[idx_path + 1]); - const double dpX = deltaPlan.getOrigin().x(); - const double dpY = deltaPlan.getOrigin().y(); - iterative_dist_to_goal += hypot(dpX, dpY); - distance_to_goal_vector_[idx_path] = iterative_dist_to_goal; + // Determine deltas between consecutive points on the global plan. + auto deltas = std::vector{}; + if (!global_plan_tf_.empty()) { + deltas.reserve(global_plan_tf_.size() - 1); + std::transform( + global_plan_tf_.cbegin(), global_plan_tf_.cend() - 1, global_plan_tf_.cbegin() + 1, + std::back_inserter(deltas), [](const auto & a, const auto & b) { return a.inverseTimes(b); }); } + // Repopulate distance vector. + distance_to_goal_vector_.clear(); + distance_to_goal_vector_.reserve(deltas.size() + 1); + std::transform( + deltas.cbegin(), deltas.cend(), std::back_inserter(distance_to_goal_vector_), + [](const auto & d) { + const auto & origin = d.getOrigin(); + return hypot(origin.x(), origin.y()); + }); + distance_to_goal_vector_.push_back(0.0); + std::partial_sum( + distance_to_goal_vector_.crbegin(), distance_to_goal_vector_.crend(), + distance_to_goal_vector_.rbegin()); + + // Repopulate turning radius vector. turning_radius_inv_vector_.clear(); - turning_radius_inv_vector_.resize(global_plan_tf_.size()); - turning_radius_inv_vector_[global_plan_tf_.size() - 1] = 0.0; - for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { - // compute turning radius based on trigonometric analysis - // radius such that next pose is connected from current pose with a semi-circle - const auto deltaPlan = global_plan_tf_[idx_path].inverseTimes(global_plan_tf_[idx_path + 1]); - const double dpX = deltaPlan.getOrigin().x(); - const double dpY = deltaPlan.getOrigin().y(); - const double dpXY2 = dpY * dpY + dpX * dpX; - if (dpXY2 < FLT_EPSILON) { - turning_radius_inv_vector_[idx_path] = std::numeric_limits::infinity(); - } else { - // 0.5*dpY*( 1 + dpX*dpX/(dpY*dPY) ); - // turning_radius_vector[idx_path] = 0.5*(1/dpY)*( dpY*dpY + dpX*dpX ); - turning_radius_inv_vector_[idx_path] = 2 * dpY / dpXY2; - } - ROS_DEBUG("turning_radius_inv_vector[%d] = %f", idx_path, turning_radius_inv_vector_[idx_path]); - } + turning_radius_inv_vector_.reserve(deltas.size() + 1); + std::transform( + deltas.cbegin(), deltas.cend(), std::back_inserter(turning_radius_inv_vector_), + [](const auto & d) { + const auto & origin = d.getOrigin(); + const auto dpX = origin.x(); + const auto dpY = origin.y(); + const auto dpXY2 = std::pow(dpX, 2) + std::pow(dpY, 2); + if (dpXY2 < FLT_EPSILON) { + return std::numeric_limits::infinity(); + } + return 2 * dpY / dpXY2; + }); + turning_radius_inv_vector_.push_back(0.0); + + assert(global_plan_tf_.size() == distance_to_goal_vector_.size()); + assert(global_plan_tf_.size() == turning_radius_inv_vector_.size()); // Set initial velocity switch (config_.init_vel_method) { From eadc7781faef1572b7fd1748a8db6eccd387f469 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 09:25:06 +0000 Subject: [PATCH 096/156] Extracted deltas_of_plan(). --- CMakeLists.txt | 8 +++-- src/calculations.cpp | 24 +++++++++++++++ src/calculations.hpp | 19 ++++++++++++ src/controller.cpp | 10 ++---- test/unittests/test_calculations.cpp | 46 ++++++++++++++++++++++++++++ 5 files changed, 97 insertions(+), 10 deletions(-) create mode 100644 src/calculations.cpp create mode 100644 src/calculations.hpp create mode 100644 test/unittests/test_calculations.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1acba745..a6bfc3c0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,6 +64,7 @@ catkin_package( add_library(${PROJECT_NAME} src/${PROJECT_NAME}_local_planner.cpp src/controller.cpp + src/calculations.cpp src/visualization.cpp src/details/second_order_lowpass.cpp ) @@ -107,6 +108,9 @@ install( if(CATKIN_ENABLE_TESTING) add_rostest(test/test_path_tracking_pid.test ARGS rviz:=false reconfigure:=false) - catkin_add_gtest(unittests test/unittests/test_main.cpp test/unittests/test_fifo_array.cpp) - target_link_libraries(unittests ${catkin_LIBRARIES}) + catkin_add_gtest(unittests + test/unittests/test_main.cpp + test/unittests/test_fifo_array.cpp + test/unittests/test_calculations.cpp) + target_link_libraries(unittests ${catkin_LIBRARIES} ${PROJECT_NAME}) endif() diff --git a/src/calculations.cpp b/src/calculations.cpp new file mode 100644 index 00000000..46bc50a2 --- /dev/null +++ b/src/calculations.cpp @@ -0,0 +1,24 @@ +#include "calculations.hpp" + +#include + +namespace path_tracking_pid +{ + +std::vector deltas_of_plan(const std::vector & input) +{ + auto result = std::vector{}; + + if (input.size() < 2) { + return result; + } + + result.reserve(input.size() - 1); + std::transform( + input.cbegin(), input.cend() - 1, input.cbegin() + 1, std::back_inserter(result), + [](const tf2::Transform & a, const tf2::Transform & b) { return a.inverseTimes(b); }); + + return result; +} + +} // namespace path_tracking_pid diff --git a/src/calculations.hpp b/src/calculations.hpp new file mode 100644 index 00000000..932832a0 --- /dev/null +++ b/src/calculations.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include + +#include + +namespace path_tracking_pid +{ + +/** + * Determine the deltas between consecutive poses in the given plan. Each delta is the inverse of a + * pose times the next pose. If the plan contains fewer than 2 poses, the output is empty. + * + * @param[in] plan Plan to process. + * @return Deltas between consecutive poses. + */ +std::vector deltas_of_plan(const std::vector & plan); + +} // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index 399d0258..9d45496d 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -11,6 +11,7 @@ #include #include +#include "calculations.hpp" #include "common.hpp" namespace path_tracking_pid @@ -217,14 +218,7 @@ void Controller::setPlan( } } - // Determine deltas between consecutive points on the global plan. - auto deltas = std::vector{}; - if (!global_plan_tf_.empty()) { - deltas.reserve(global_plan_tf_.size() - 1); - std::transform( - global_plan_tf_.cbegin(), global_plan_tf_.cend() - 1, global_plan_tf_.cbegin() + 1, - std::back_inserter(deltas), [](const auto & a, const auto & b) { return a.inverseTimes(b); }); - } + const auto deltas = deltas_of_plan(global_plan_tf_); // Repopulate distance vector. distance_to_goal_vector_.clear(); diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp new file mode 100644 index 00000000..ce6c322b --- /dev/null +++ b/test/unittests/test_calculations.cpp @@ -0,0 +1,46 @@ +#include + +#include "../../src/calculations.hpp" + +namespace +{ + +using path_tracking_pid::deltas_of_plan; + +// Create a transform (with an identity basis) based on the given coordinates. +tf2::Transform create_transform(double x, double y, double z) +{ + tf2::Transform result; + result.getBasis().setIdentity(); + result.setOrigin({x, y, z}); + return result; +} + +TEST(PathTrackingPidCalculations, DeltasOfPlan_Empty) +{ + const auto plan = std::vector{}; + const auto result = deltas_of_plan(plan); + + EXPECT_TRUE(result.empty()); +} + +TEST(PathTrackingPidCalculations, DeltasOfPlan_OnlyOne) +{ + const auto plan = std::vector{create_transform(1, 1, 1)}; + const auto result = deltas_of_plan(plan); + + EXPECT_TRUE(result.empty()); +} + +TEST(PathTrackingPidCalculations, DeltasOfPlan) +{ + const auto plan = std::vector{ + create_transform(0, 0, 0), create_transform(1, 1, 1), create_transform(0, 0, 4)}; + const auto ref = + std::vector{create_transform(1, 1, 1), create_transform(-1, -1, 3)}; + const auto result = deltas_of_plan(plan); + + EXPECT_EQ(ref, result); +} + +} // namespace From 61bd1c12fc1dccb950401347ba0c6f3feb7a02ce Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 11:11:12 +0000 Subject: [PATCH 097/156] Keep RosLint happy. --- src/calculations.cpp | 1 + test/unittests/test_calculations.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/calculations.cpp b/src/calculations.cpp index 46bc50a2..cefd4697 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -1,6 +1,7 @@ #include "calculations.hpp" #include +#include namespace path_tracking_pid { diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index ce6c322b..3680fc79 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -1,5 +1,7 @@ #include +#include + #include "../../src/calculations.hpp" namespace From 9553a440f39da860f2c719bcb6afddf2e71e68c1 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 11:24:58 +0000 Subject: [PATCH 098/156] Extracted distances_to_goal(). --- src/calculations.cpp | 18 +++++++++++++ src/calculations.hpp | 9 +++++++ src/controller.cpp | 15 +---------- test/unittests/test_calculations.cpp | 39 ++++++++++++++++++++++++++++ 4 files changed, 67 insertions(+), 14 deletions(-) diff --git a/src/calculations.cpp b/src/calculations.cpp index cefd4697..31e82698 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -1,6 +1,7 @@ #include "calculations.hpp" #include +#include #include namespace path_tracking_pid @@ -22,4 +23,21 @@ std::vector deltas_of_plan(const std::vector & i return result; } +std::vector distances_to_goal(const std::vector & deltas) +{ + auto result = std::vector{}; + + result.reserve(deltas.size() + 1); + std::transform( + deltas.cbegin(), deltas.cend(), std::back_inserter(result), [](const tf2::Transform & d) { + const auto & origin = d.getOrigin(); + return std::hypot(origin.x(), origin.y()); + }); + result.push_back(0.0); + + std::partial_sum(result.crbegin(), result.crend(), result.rbegin()); + + return result; +} + } // namespace path_tracking_pid diff --git a/src/calculations.hpp b/src/calculations.hpp index 932832a0..9b7b489f 100644 --- a/src/calculations.hpp +++ b/src/calculations.hpp @@ -16,4 +16,13 @@ namespace path_tracking_pid */ std::vector deltas_of_plan(const std::vector & plan); +/** + * Determine the distances to the goal for each pose of a plan, based on the given deltas of that + * plan. The 2D distance is calculated; the z component is ignored. + * + * @param[in] deltas Deltas between consecutive poses of a plan. (Result of deltas_of_plan().) + * @return Distances to the goal. + */ +std::vector distances_to_goal(const std::vector & deltas); + } // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index 9d45496d..a4019fd4 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include @@ -220,19 +219,7 @@ void Controller::setPlan( const auto deltas = deltas_of_plan(global_plan_tf_); - // Repopulate distance vector. - distance_to_goal_vector_.clear(); - distance_to_goal_vector_.reserve(deltas.size() + 1); - std::transform( - deltas.cbegin(), deltas.cend(), std::back_inserter(distance_to_goal_vector_), - [](const auto & d) { - const auto & origin = d.getOrigin(); - return hypot(origin.x(), origin.y()); - }); - distance_to_goal_vector_.push_back(0.0); - std::partial_sum( - distance_to_goal_vector_.crbegin(), distance_to_goal_vector_.crend(), - distance_to_goal_vector_.rbegin()); + distance_to_goal_vector_ = distances_to_goal(deltas); // Repopulate turning radius vector. turning_radius_inv_vector_.clear(); diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index 3680fc79..b20918e3 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -8,6 +8,7 @@ namespace { using path_tracking_pid::deltas_of_plan; +using path_tracking_pid::distances_to_goal; // Create a transform (with an identity basis) based on the given coordinates. tf2::Transform create_transform(double x, double y, double z) @@ -45,4 +46,42 @@ TEST(PathTrackingPidCalculations, DeltasOfPlan) EXPECT_EQ(ref, result); } +TEST(PathTrackingPidCalculations, DistancesToGoal_Empty) +{ + const auto deltas = std::vector{}; + const auto ref = std::vector{0.0}; + const auto result = distances_to_goal(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, DistancesToGoal_Single) +{ + const auto deltas = std::vector{create_transform(3, 4, 0)}; + const auto ref = std::vector{5.0, 0.0}; + const auto result = distances_to_goal(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, DistancesToGoal) +{ + const auto deltas = std::vector{ + create_transform(3, 4, 0), create_transform(-2, 0, 0), create_transform(0, 1, 0)}; + const auto ref = std::vector{8.0, 3.0, 1.0, 0.0}; + const auto result = distances_to_goal(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, DistancesToGoal_IgnoreZ) +{ + const auto deltas = std::vector{ + create_transform(3, 4, 1), create_transform(-2, 0, 2), create_transform(0, 1, 3)}; + const auto ref = std::vector{8.0, 3.0, 1.0, 0.0}; + const auto result = distances_to_goal(deltas); + + EXPECT_EQ(ref, result); +} + } // namespace From 727444b303420aefa1e3aae3d9a12656fb056d51 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 11:43:53 +0000 Subject: [PATCH 099/156] Extracted inverse_turning_radiuses(). --- src/calculations.cpp | 21 ++++++++++++ src/calculations.hpp | 9 ++++++ src/controller.cpp | 18 +---------- test/unittests/test_calculations.cpp | 48 ++++++++++++++++++++++++++++ 4 files changed, 79 insertions(+), 17 deletions(-) diff --git a/src/calculations.cpp b/src/calculations.cpp index 31e82698..33229fef 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -40,4 +40,25 @@ std::vector distances_to_goal(const std::vector & deltas return result; } +std::vector inverse_turning_radiuses(const std::vector & deltas) +{ + auto result = std::vector{}; + + result.reserve(deltas.size() + 1); + std::transform( + deltas.cbegin(), deltas.cend(), std::back_inserter(result), [](const tf2::Transform & d) { + const auto & origin = d.getOrigin(); + const auto dpX = origin.x(); + const auto dpY = origin.y(); + const auto dpXY2 = std::pow(dpX, 2) + std::pow(dpY, 2); + if (dpXY2 < FLT_EPSILON) { + return std::numeric_limits::infinity(); + } + return (2 * dpY) / dpXY2; + }); + result.push_back(0.0); + + return result; +} + } // namespace path_tracking_pid diff --git a/src/calculations.hpp b/src/calculations.hpp index 9b7b489f..ea04a297 100644 --- a/src/calculations.hpp +++ b/src/calculations.hpp @@ -25,4 +25,13 @@ std::vector deltas_of_plan(const std::vector & p */ std::vector distances_to_goal(const std::vector & deltas); +/** + * Determine the inverse turning radiuses for each pose of a plan, based on the given deltas of that + * plan. The 2D radius is calculated; the z component is ignored. + * + * @param[in] deltas Deltas between consecutive poses of a plan. (Result of deltas_of_plan().) + * @return Inverse turning radiuses. + */ +std::vector inverse_turning_radiuses(const std::vector & deltas); + } // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index a4019fd4..c5c8bf04 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -220,23 +220,7 @@ void Controller::setPlan( const auto deltas = deltas_of_plan(global_plan_tf_); distance_to_goal_vector_ = distances_to_goal(deltas); - - // Repopulate turning radius vector. - turning_radius_inv_vector_.clear(); - turning_radius_inv_vector_.reserve(deltas.size() + 1); - std::transform( - deltas.cbegin(), deltas.cend(), std::back_inserter(turning_radius_inv_vector_), - [](const auto & d) { - const auto & origin = d.getOrigin(); - const auto dpX = origin.x(); - const auto dpY = origin.y(); - const auto dpXY2 = std::pow(dpX, 2) + std::pow(dpY, 2); - if (dpXY2 < FLT_EPSILON) { - return std::numeric_limits::infinity(); - } - return 2 * dpY / dpXY2; - }); - turning_radius_inv_vector_.push_back(0.0); + turning_radius_inv_vector_ = inverse_turning_radiuses(deltas); assert(global_plan_tf_.size() == distance_to_goal_vector_.size()); assert(global_plan_tf_.size() == turning_radius_inv_vector_.size()); diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index b20918e3..28c01052 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -9,6 +9,7 @@ namespace using path_tracking_pid::deltas_of_plan; using path_tracking_pid::distances_to_goal; +using path_tracking_pid::inverse_turning_radiuses; // Create a transform (with an identity basis) based on the given coordinates. tf2::Transform create_transform(double x, double y, double z) @@ -84,4 +85,51 @@ TEST(PathTrackingPidCalculations, DistancesToGoal_IgnoreZ) EXPECT_EQ(ref, result); } +TEST(PathTrackingPidCalculations, InverseTurningRadiuses_Empty) +{ + const auto deltas = std::vector{}; + const auto ref = std::vector{0.0}; + const auto result = inverse_turning_radiuses(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, InverseTurningRadiuses_SingleTooSmall) +{ + const auto deltas = std::vector{create_transform(0, 0, 0)}; + const auto ref = std::vector{std::numeric_limits::infinity(), 0.0}; + const auto result = inverse_turning_radiuses(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, InverseTurningRadiuses_Single) +{ + const auto deltas = std::vector{create_transform(3, 4, 0)}; + const auto ref = std::vector{8.0 / 25.0, 0.0}; + const auto result = inverse_turning_radiuses(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, InverseTurningRadiuses) +{ + const auto deltas = std::vector{ + create_transform(3, 4, 0), create_transform(2, 0, 0), create_transform(0, 2, 0)}; + const auto ref = std::vector{8.0 / 25.0, 0.0, 1.0, 0.0}; + const auto result = inverse_turning_radiuses(deltas); + + EXPECT_EQ(ref, result); +} + +TEST(PathTrackingPidCalculations, InverseTurningRadiuses_IgnoreZ) +{ + const auto deltas = std::vector{ + create_transform(3, 4, 1), create_transform(2, 0, 2), create_transform(0, 2, 3)}; + const auto ref = std::vector{8.0 / 25.0, 0.0, 1.0, 0.0}; + const auto result = inverse_turning_radiuses(deltas); + + EXPECT_EQ(ref, result); +} + } // namespace From f3c81ec8252dd5c174a5f809ef1fb46e114395e3 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 13:06:39 +0000 Subject: [PATCH 100/156] Keep RosLint happy. --- src/calculations.cpp | 1 + test/unittests/test_calculations.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/calculations.cpp b/src/calculations.cpp index 33229fef..2fbeb57c 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -1,6 +1,7 @@ #include "calculations.hpp" #include +#include #include #include diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index 28c01052..d0f964cc 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -1,5 +1,6 @@ #include +#include #include #include "../../src/calculations.hpp" From 454f58676426d1653ee4d7eb6bb15070be74fd56 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 14:17:32 +0000 Subject: [PATCH 101/156] Refactored setTricycleModel(). --- include/path_tracking_pid/controller.hpp | 4 ++-- src/common.hpp | 19 +++++++++++++++++++ src/controller.cpp | 8 ++++---- src/path_tracking_pid_local_planner.cpp | 3 ++- 4 files changed, 27 insertions(+), 7 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index df6cb638..ef123f86 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -66,7 +66,7 @@ class Controller : private boost::noncopyable * @param estimate_pose_angle The transformation from base to steered wheel */ void setTricycleModel( - bool tricycle_model_enabled, const geometry_msgs::Transform & tf_base_to_steered_wheel); + bool tricycle_model_enabled, const tf2::Transform & tf_base_to_steered_wheel); /** * Set plan @@ -250,7 +250,7 @@ class Controller : private boost::noncopyable // tricycle model bool use_tricycle_model_ = false; - geometry_msgs::Transform tf_base_to_steered_wheel_; + tf2::Transform tf_base_to_steered_wheel_; std::array, 2> inverse_kinematics_matrix_{}; std::array, 2> forward_kinematics_matrix_{}; diff --git a/src/common.hpp b/src/common.hpp index 114cf44b..34601e30 100644 --- a/src/common.hpp +++ b/src/common.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include namespace path_tracking_pid @@ -13,4 +15,21 @@ constexpr std::underlying_type_t to_underlying(enum_type value) noexc return static_cast>(value); } +/** + * Converts input (of input_type) to result_type. Like tf2::fromMsg() but using a return value + * instead of an output parameter. + * + * @tparam result_type Resulting type of the conversion. Should be a tf2 type. + * @tparam input_type Input type for the conversion. Should be a message type. + * @param[in] input Input object to convert. + * @return Converted object. + */ +template +result_type tf2_convert(const input_type & input) +{ + result_type result; + tf2::fromMsg(input, result); + return result; +} + } // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index c5c8bf04..69e88a41 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -109,13 +109,13 @@ void Controller::setEstimatePoseAngle(bool estimate_pose_angle) } void Controller::setTricycleModel( - bool tricycle_model_enabled, const geometry_msgs::Transform & tf_base_to_steered_wheel) + bool tricycle_model_enabled, const tf2::Transform & tf_base_to_steered_wheel) { // Set tricycle model use_tricycle_model_ = tricycle_model_enabled; tf_base_to_steered_wheel_ = tf_base_to_steered_wheel; - const double wheel_x = tf_base_to_steered_wheel_.translation.x; - const double wheel_y = tf_base_to_steered_wheel_.translation.y; + const double wheel_x = tf_base_to_steered_wheel_.getOrigin().x(); + const double wheel_y = tf_base_to_steered_wheel_.getOrigin().y(); const double distance_base_to_steered_wheel = hypot(wheel_x, wheel_y); const double wheel_theta = atan2(wheel_y, wheel_x); @@ -138,7 +138,7 @@ void Controller::setTricycleModel( forward_kinematics_matrix_[1][0] = -inverse_kinematics_matrix_[1][0] / determinant; forward_kinematics_matrix_[1][1] = inverse_kinematics_matrix_[0][0] / determinant; - controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel_.rotation); + controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel_.getRotation()); } geometry_msgs::Twist Controller::computeTricycleModelForwardKinematics( diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index ad266653..91d2a2ac 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -173,7 +173,8 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector(tf_base_to_steered_wheel_stamped_.transform)); // TODO(clopez): subscribe to steered wheel odom geometry_msgs::Twist steering_odom_twist; From 322655100b42706e3912c49703c4f2b4afd84894 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 16:09:51 +0000 Subject: [PATCH 102/156] Refactored setPlan(). --- include/path_tracking_pid/controller.hpp | 10 ++-- src/controller.cpp | 58 ++++++++---------------- src/path_tracking_pid_local_planner.cpp | 27 +++++++++-- 3 files changed, 49 insertions(+), 46 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index ef123f86..4bf076fc 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -75,8 +75,8 @@ class Controller : private boost::noncopyable * @param global_plan Plan to follow */ void setPlan( - const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, - const std::vector & global_plan); + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const std::vector & global_plan); /** * Set plan @@ -87,10 +87,10 @@ class Controller : private boost::noncopyable * @param global_plan Plan to follow */ void setPlan( - const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, - const geometry_msgs::Transform & tf_base_to_steered_wheel, + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const tf2::Transform & tf_base_to_steered_wheel, const geometry_msgs::Twist & steering_odom_twist, - const std::vector & global_plan); + const std::vector & global_plan); /** * Find position on plan by looking at the surroundings of last known pose. * @param current Where is the robot now? diff --git a/src/controller.cpp b/src/controller.cpp index 69e88a41..77fd827c 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -37,57 +37,43 @@ bool have_same_sign(T val1, T val2) return std::signbit(val1) == std::signbit(val2); } -// Converts a pose to the corresponding transform. -tf2::Transform to_transform(const geometry_msgs::Pose & pose) -{ - tf2::Transform result; - tf2::fromMsg(pose, result); - return result; -} - // Returns the square distance between two points double distSquared(const tf2::Transform & a, const tf2::Transform & b) { return a.getOrigin().distance2(b.getOrigin()); } -// Return the square distance between two points. -double distSquared(const geometry_msgs::Pose & a, const geometry_msgs::Pose & b) -{ - return std::pow(a.position.x - b.position.x, 2) + std::pow(a.position.y - b.position.y, 2); -} - // Indicates if the angle of the cur pose is obtuse (with respect to the prev and next poses). bool is_pose_angle_obtuse( - const geometry_msgs::Pose & prev, const geometry_msgs::Pose & cur, - const geometry_msgs::Pose & next) + const tf2::Transform & prev, const tf2::Transform & cur, const tf2::Transform & next) { return distSquared(prev, next) > (distSquared(prev, cur) + distSquared(cur, next)); } -// Filters (and converts) the given plan. The first and last poses are always accepted (if they -// exist). Intermediate poses are only accepted if the angle (with respect to the previous and next -// poses) is obtuse. All accepted poses are converted to transforms. -std::vector filter_plan(const std::vector & plan) +// Filters the given plan. The first and last poses are always accepted (if they exist). +// Intermediate poses are only accepted if the angle (with respect to the previous and next poses) +// is obtuse. +std::vector filter_plan(const std::vector & plan) { const auto plan_size = plan.size(); auto result = std::vector{}; + result.reserve(plan.size()); if (plan_size > 0) { - result.push_back(to_transform(plan.front().pose)); + result.push_back(plan.front()); } for (int pose_idx = 1; pose_idx < static_cast(plan_size) - 1; ++pose_idx) { - const auto prev_pose = plan[pose_idx - 1].pose; - const auto pose = plan[pose_idx].pose; - const auto next_pose = plan[pose_idx + 1].pose; + const auto & prev_pose = plan[pose_idx - 1]; + const auto & pose = plan[pose_idx]; + const auto & next_pose = plan[pose_idx + 1]; if (is_pose_angle_obtuse(prev_pose, pose, next_pose)) { - result.push_back(to_transform(pose)); + result.push_back(pose); } } if (plan_size > 1) { - result.push_back(to_transform(plan.back().pose)); + result.push_back(plan.back()); } return result; @@ -172,14 +158,10 @@ TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics( } void Controller::setPlan( - const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, - const std::vector & global_plan) + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const std::vector & global_plan) { - ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%d)", (int)global_plan.size()); - ROS_DEBUG("Plan is defined in frame '%s'", global_plan.at(0).header.frame_id.c_str()); - - tf2::Transform current_tf2; - tf2::convert(current_tf, current_tf2); + ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%zu)", global_plan.size()); global_plan_tf_ = filter_plan(global_plan); if (global_plan_tf_.size() != global_plan.size()) { @@ -207,7 +189,7 @@ void Controller::setPlan( for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ const auto dist_to_segment = - distToSegmentSquared(current_tf2, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]) + distToSegmentSquared(current_tf, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]) .distance2_to_p; // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! @@ -254,13 +236,13 @@ void Controller::setPlan( } void Controller::setPlan( - const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, - const geometry_msgs::Transform & tf_base_to_steered_wheel, + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const tf2::Transform & tf_base_to_steered_wheel, const geometry_msgs::Twist & /* steering_odom_twist */, - const std::vector & global_plan) + const std::vector & global_plan) { setPlan(current_tf, odom_twist, global_plan); - controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel.rotation); + controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel.getRotation()); // TODO(clopez) use steering_odom_twist to check if setpoint is being followed } diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 91d2a2ac..56548617 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -31,6 +31,24 @@ namespace constexpr double MAP_PARALLEL_THRESH = 0.2; constexpr double DT_MAX = 1.5; +/** + * Convert the plan from geometry message format to tf2 format. + * + * @param[in] plan Plan to convert. + * @return Converted plan. + */ +std::vector convert_plan(const std::vector & plan) +{ + auto result = std::vector{}; + + result.reserve(plan.size()); + std::transform( + plan.cbegin(), plan.cend(), std::back_inserter(result), + [](const geometry_msgs::PoseStamped & msg) { return tf2_convert(msg.pose); }); + + return result; +} + } // namespace void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig & config) @@ -179,10 +197,13 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector(tfCurPoseStamped_.transform), latest_odom_.twist.twist, + tf2_convert(tf_base_to_steered_wheel_stamped_.transform), steering_odom_twist, + convert_plan(global_plan_)); } else { - pid_controller_.setPlan(tfCurPoseStamped_.transform, latest_odom_.twist.twist, global_plan_); + pid_controller_.setPlan( + tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, + convert_plan(global_plan_)); } pid_controller_.setEnabled(true); From c06019008cc63dcda733a2d21d8447777c939021 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 16:25:55 +0000 Subject: [PATCH 103/156] Refactored findPositionOnPlan(). --- include/path_tracking_pid/controller.hpp | 5 ++--- src/controller.cpp | 12 ++++-------- src/path_tracking_pid_local_planner.cpp | 10 +++++----- 3 files changed, 11 insertions(+), 16 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 4bf076fc..2c2b6c47 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -99,11 +99,10 @@ class Controller : private boost::noncopyable * @return index of current path-pose if requested */ tf2::Transform findPositionOnPlan( - const geometry_msgs::Transform & current_tf, ControllerState & controller_state, - size_t & path_pose_idx); + const tf2::Transform & current_tf, ControllerState & controller_state, size_t & path_pose_idx); // Overloaded function definition for users that don't require the segment index tf2::Transform findPositionOnPlan( - const geometry_msgs::Transform & current_tf, ControllerState & controller_state) + const tf2::Transform & current_tf, ControllerState & controller_state) { size_t path_pose_idx; return findPositionOnPlan(current_tf, controller_state, path_pose_idx); diff --git a/src/controller.cpp b/src/controller.cpp index 77fd827c..d44cecaf 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -288,11 +288,9 @@ Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( } tf2::Transform Controller::findPositionOnPlan( - const geometry_msgs::Transform & current_tf, ControllerState & controller_state, - size_t & path_pose_idx) + const tf2::Transform & current_tf, ControllerState & controller_state, size_t & path_pose_idx) { - tf2::Transform current_tf2; - tf2::convert(current_tf, current_tf2); + auto current_tf2 = current_tf; // 'Project' current_tf by removing z-component tf2::Vector3 originProj = current_tf2.getOrigin(); originProj.setZ(0.0); @@ -416,7 +414,7 @@ Controller::UpdateResult Controller::update( if (config_.track_base_link) { // Find closes robot position to path and then project carrot on goal current_pos_on_plan_ = current_goal_ = - findPositionOnPlan(current_tf, controller_state_, path_pose_idx); + findPositionOnPlan(tf2_convert(current_tf), controller_state_, path_pose_idx); // To track the base link the goal is then transform to the control point goal double theda_rp = tf2::getYaw(current_goal_.getRotation()); tf2::Vector3 newControlOrigin; @@ -426,10 +424,8 @@ Controller::UpdateResult Controller::update( current_goal_.setOrigin(newControlOrigin); } else { // find position of current position with projected carrot - geometry_msgs::Transform current_with_carrot_g; - tf2::convert(current_with_carrot_, current_with_carrot_g); current_pos_on_plan_ = current_goal_ = - findPositionOnPlan(current_with_carrot_g, controller_state_, path_pose_idx); + findPositionOnPlan(current_with_carrot_, controller_state_, path_pose_idx); } result.progress = 1.0 - distance_to_goal_ / distance_to_goal_vector_[0]; diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 56548617..68eae6f9 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -392,15 +392,15 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Step until lookahead is reached, for every step project the pose back to the path std::vector projected_steps_tf; - tf2::Transform projected_step_tf; - tf2::fromMsg(current_tf, projected_step_tf); + auto projected_step_tf = tf2_convert(current_tf); projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link - projected_step_tf = pid_controller_.findPositionOnPlan(current_tf, projected_controller_state); + projected_step_tf = + pid_controller_.findPositionOnPlan(projected_step_tf, projected_controller_state); projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose for (uint step = 0; step < n_steps; step++) { tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf; - projected_step_tf = pid_controller_.findPositionOnPlan( - tf2::toMsg(next_straight_step_tf), projected_controller_state); + projected_step_tf = + pid_controller_.findPositionOnPlan(next_straight_step_tf, projected_controller_state); projected_steps_tf.push_back(projected_step_tf); // Fill markers: From 02c0e8ba1392bfcf15971a0b3b565a4450d54545 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 17:42:14 +0000 Subject: [PATCH 104/156] Refactored last Controller functions(). --- include/path_tracking_pid/controller.hpp | 6 ++-- src/common.hpp | 4 ++- src/controller.cpp | 40 +++++++++++------------- src/path_tracking_pid_local_planner.cpp | 4 +-- 4 files changed, 26 insertions(+), 28 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 2c2b6c47..ea41e582 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -126,7 +126,7 @@ class Controller : private boost::noncopyable * @return Update result */ UpdateResult update( - double target_x_vel, double target_end_x_vel, const geometry_msgs::Transform & current_tf, + double target_x_vel, double target_end_x_vel, const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, ros::Duration dt); /** @@ -137,7 +137,7 @@ class Controller : private boost::noncopyable * @return Update result */ UpdateResult update_with_limits( - const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, ros::Duration dt); /** @@ -147,7 +147,7 @@ class Controller : private boost::noncopyable * @return Velocity command */ double mpc_based_max_vel( - double target_x_vel, const geometry_msgs::Transform & current_tf, + double target_x_vel, const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist); /** diff --git a/src/common.hpp b/src/common.hpp index 34601e30..97eeade6 100644 --- a/src/common.hpp +++ b/src/common.hpp @@ -24,7 +24,9 @@ constexpr std::underlying_type_t to_underlying(enum_type value) noexc * @param[in] input Input object to convert. * @return Converted object. */ -template +template < + typename result_type, typename input_type, + typename = std::enable_if_t>> result_type tf2_convert(const input_type & input) { result_type result; diff --git a/src/controller.cpp b/src/controller.cpp index d44cecaf..12930ef0 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -390,7 +390,7 @@ tf2::Transform Controller::findPositionOnPlan( } Controller::UpdateResult Controller::update( - double target_x_vel, double target_end_x_vel, const geometry_msgs::Transform & current_tf, + double target_x_vel, double target_end_x_vel, const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, ros::Duration dt) { UpdateResult result; @@ -399,22 +399,20 @@ Controller::UpdateResult Controller::update( const double current_yaw_vel = controller_state_.current_yaw_vel; // Compute location of the point to be controlled - const double theda_rp = tf2::getYaw(current_tf.rotation); + const double theda_rp = tf2::getYaw(current_tf.getRotation()); tf2::Vector3 current_with_carrot_origin; - current_with_carrot_origin.setX(current_tf.translation.x + config_.l * cos(theda_rp)); - current_with_carrot_origin.setY(current_tf.translation.y + config_.l * sin(theda_rp)); + current_with_carrot_origin.setX(current_tf.getOrigin().x() + config_.l * cos(theda_rp)); + current_with_carrot_origin.setY(current_tf.getOrigin().y() + config_.l * sin(theda_rp)); current_with_carrot_origin.setZ(0); current_with_carrot_.setOrigin(current_with_carrot_origin); - tf2::Quaternion cur_rot( - current_tf.rotation.x, current_tf.rotation.y, current_tf.rotation.z, current_tf.rotation.w); - current_with_carrot_.setRotation(cur_rot); + current_with_carrot_.setRotation(current_tf.getRotation()); size_t path_pose_idx; if (config_.track_base_link) { // Find closes robot position to path and then project carrot on goal current_pos_on_plan_ = current_goal_ = - findPositionOnPlan(tf2_convert(current_tf), controller_state_, path_pose_idx); + findPositionOnPlan(current_tf, controller_state_, path_pose_idx); // To track the base link the goal is then transform to the control point goal double theda_rp = tf2::getYaw(current_goal_.getRotation()); tf2::Vector3 newControlOrigin; @@ -466,9 +464,7 @@ Controller::UpdateResult Controller::update( global_plan_tf_[path_pose_idx].getOrigin().y(), global_plan_tf_[path_pose_idx].getOrigin().z())); - tf2::Vector3 current_tracking_err = - -(path_segmen_tf.inverse() * - tf2::Vector3(current_tf.translation.x, current_tf.translation.y, current_tf.translation.z)); + tf2::Vector3 current_tracking_err = -(path_segmen_tf.inverse() * current_tf.getOrigin()); // trackin_error here represents the error between tracked link and position on plan controller_state_.tracking_error_lat = current_tracking_err.y(); @@ -798,8 +794,7 @@ Controller::UpdateResult Controller::update( } Controller::UpdateResult Controller::update_with_limits( - const geometry_msgs::Transform & current_tf, const geometry_msgs::Twist & odom_twist, - ros::Duration dt) + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, ros::Duration dt) { // All limits are absolute double max_x_vel = std::abs(config_.target_x_vel); @@ -846,8 +841,7 @@ Controller::UpdateResult Controller::update_with_limits( // output updated velocity command: (Current position, current measured velocity, closest point index, estimated // duration of arrival, debug info) double Controller::mpc_based_max_vel( - double target_x_vel, const geometry_msgs::Transform & current_tf, - const geometry_msgs::Twist & odom_twist) + double target_x_vel, const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist) { // Temporary save global data ControllerState controller_state_saved; @@ -861,7 +855,7 @@ double Controller::mpc_based_max_vel( int mpc_fwd_iter = 0; // Reset MPC iterations // Create predicted position vector - geometry_msgs::Transform predicted_tf = current_tf; + auto predicted_tf = current_tf; geometry_msgs::Twist pred_twist = odom_twist; double new_nominal_x_vel = target_x_vel; // Start off from the current velocity @@ -927,14 +921,16 @@ double Controller::mpc_based_max_vel( .velocity_command; // Run plant model - const double theta = tf2::getYaw(predicted_tf.rotation); - predicted_tf.translation.x += - pred_twist.linear.x * cos(theta) * config_.mpc_simulation_sample_time; - predicted_tf.translation.y += - pred_twist.linear.x * sin(theta) * config_.mpc_simulation_sample_time; + const double theta = tf2::getYaw(predicted_tf.getRotation()); + predicted_tf.getOrigin().setX( + predicted_tf.getOrigin().getX() + + pred_twist.linear.x * cos(theta) * config_.mpc_simulation_sample_time); + predicted_tf.getOrigin().setY( + predicted_tf.getOrigin().getY() + + pred_twist.linear.x * sin(theta) * config_.mpc_simulation_sample_time); tf2::Quaternion q; q.setRPY(0, 0, theta + pred_twist.angular.z * config_.mpc_simulation_sample_time); - predicted_tf.rotation = tf2::toMsg(q); + predicted_tf.setRotation(q); } } // Apply limits to the velocity diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 68eae6f9..94e6fbce 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -259,8 +259,8 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd pid_controller_.setVelMaxObstacle(INFINITY); // Can be disabled live, so set back to inf } - const auto update_result = - pid_controller_.update_with_limits(tfCurPoseStamped_.transform, latest_odom_.twist.twist, dt); + const auto update_result = pid_controller_.update_with_limits( + tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, dt); cmd_vel = update_result.velocity_command; path_tracking_pid::PidFeedback feedback_msg; From 591d815568dbaa3e6d6897e9b76cd8cb63b5d7f4 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 17:48:29 +0000 Subject: [PATCH 105/156] Removed unnecessary conversions. --- src/controller.cpp | 4 +--- src/path_tracking_pid_local_planner.cpp | 6 ++---- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 12930ef0..588571bd 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -530,9 +530,7 @@ Controller::UpdateResult Controller::update( ROS_DEBUG("distance_to_goal: %f", distance_to_goal_); // Get 'angle' towards current_goal - tf2::Transform robot_pose; - tf2::convert(current_tf, robot_pose); - tf2::Transform base_to_goal = robot_pose.inverseTimes(current_goal_); + tf2::Transform base_to_goal = current_tf.inverseTimes(current_goal_); const double angle_to_goal = atan2(base_to_goal.getOrigin().x(), -base_to_goal.getOrigin().y()); // If we are as close to our goal or closer then we need to reach end velocity, enable end_phase. diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 94e6fbce..f8a3c995 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -289,8 +289,7 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd std_msgs::Header header; header.stamp = now; header.frame_id = map_frame_; - tf2::Transform tfCurPose; - tf2::fromMsg(tfCurPoseStamped_.transform, tfCurPose); + const auto tfCurPose = tf2_convert(tfCurPoseStamped_.transform); visualization_->publishAxlePoint(header, tfCurPose); visualization_->publishControlPoint(header, pid_controller_.getCurrentWithCarrot()); visualization_->publishGoalPoint(header, pid_controller_.getCurrentGoal()); @@ -388,11 +387,10 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Use a controller state to forward project the position on the path auto projected_controller_state = pid_controller_.getControllerState(); - geometry_msgs::Transform current_tf = tfCurPoseStamped_.transform; // Step until lookahead is reached, for every step project the pose back to the path std::vector projected_steps_tf; - auto projected_step_tf = tf2_convert(current_tf); + auto projected_step_tf = tf2_convert(tfCurPoseStamped_.transform); projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link projected_step_tf = pid_controller_.findPositionOnPlan(projected_step_tf, projected_controller_state); From 79a04e884bbd3916dcc1340449e3728bafd050f9 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 24 Feb 2022 17:56:42 +0000 Subject: [PATCH 106/156] Removed global_plan_ member. --- .../path_tracking_pid_local_planner.hpp | 1 - src/path_tracking_pid_local_planner.cpp | 24 +++++++------------ 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 2406b3af..175985c6 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -123,7 +123,6 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, ros::Duration prev_dt_; path_tracking_pid::Controller pid_controller_; - std::vector global_plan_; nav_msgs::Path received_path_; // Obstacle collision detection diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index f8a3c995..b2dd16ff 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -55,12 +55,6 @@ void TrackingPidLocalPlanner::reconfigure_pid(path_tracking_pid::PidConfig & con { pid_controller_.configure(config); controller_debug_enabled_ = config.controller_debug_enabled; - - if (controller_debug_enabled_ && !global_plan_.empty()) { - received_path_.header = global_plan_.at(0).header; - received_path_.poses = global_plan_; - path_pub_.publish(received_path_); - } } void TrackingPidLocalPlanner::initialize( @@ -115,11 +109,11 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector(tfCurPoseStamped_.transform), latest_odom_.twist.twist, tf2_convert(tf_base_to_steered_wheel_stamped_.transform), steering_odom_twist, - convert_plan(global_plan_)); + convert_plan(global_plan_copy)); } else { pid_controller_.setPlan( tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, - convert_plan(global_plan_)); + convert_plan(global_plan_copy)); } pid_controller_.setEnabled(true); From dd16487b1dce71253a4ce7e925ac69f742641dd9 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 28 Feb 2022 09:22:50 +0000 Subject: [PATCH 107/156] Moved visualization. --- .../path_tracking_pid_local_planner.hpp | 1 - include/path_tracking_pid/visualization.hpp | 19 +++- src/path_tracking_pid_local_planner.cpp | 107 +++--------------- src/visualization.cpp | 104 ++++++++++++++++- 4 files changed, 130 insertions(+), 101 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 175985c6..5da57f1f 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -145,7 +145,6 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, // Rviz visualization std::unique_ptr visualization_; ros::Publisher path_pub_; - ros::Publisher collision_marker_pub_; ros::Subscriber sub_odom_; ros::Publisher feedback_pub_; diff --git a/include/path_tracking_pid/visualization.hpp b/include/path_tracking_pid/visualization.hpp index 80e4d22c..d2c066b8 100644 --- a/include/path_tracking_pid/visualization.hpp +++ b/include/path_tracking_pid/visualization.hpp @@ -7,24 +7,33 @@ namespace path_tracking_pid { + class Visualization { public: - Visualization(ros::NodeHandle nh); + using point_t = geometry_msgs::Point; + using points_t = std::vector; + + explicit Visualization(ros::NodeHandle nh); void publishControlPoint(const std_msgs::Header & header, const tf2::Transform & pose); void publishAxlePoint(const std_msgs::Header & header, const tf2::Transform & pose); void publishGoalPoint(const std_msgs::Header & header, const tf2::Transform & pose); void publishPlanPoint(const std_msgs::Header & header, const tf2::Transform & pose); + void publishCollision( + const std::string & frame_id, uint8_t cost, const point_t & point, const points_t & footprint, + const points_t & hull, const points_t & steps, const points_t & path); private: + ros::Publisher marker_pub_; + ros::Publisher collision_marker_pub_; + void publishSphere( const std_msgs::Header & header, const std::string & ns, int id, const tf2::Transform & pose, const std_msgs::ColorRGBA & color); void publishSphere( - const std_msgs::Header & header, const std::string & ns, int id, geometry_msgs::Pose pose, - const std_msgs::ColorRGBA & color); - - ros::Publisher marker_pub_; + const std_msgs::Header & header, const std::string & ns, int id, + const geometry_msgs::Pose & pose, const std_msgs::ColorRGBA & color); }; + } // namespace path_tracking_pid diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index b2dd16ff..82e50dcb 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -83,7 +83,6 @@ void TrackingPidLocalPlanner::initialize( nh.param("use_tricycle_model", use_tricycle_model_, false); nh.param("steered_wheel_frame", steered_wheel_frame_, "steer"); - collision_marker_pub_ = nh.advertise("collision_markers", 3); visualization_ = std::make_unique(nh); debug_pub_ = nh.advertise("debug", 1); path_pub_ = nh.advertise("visualization_path", 1, true); @@ -298,72 +297,6 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd uint8_t TrackingPidLocalPlanner::projectedCollisionCost() { - // configure rviz visualization - visualization_msgs::Marker mkSteps; - mkSteps.header.frame_id = map_frame_; - mkSteps.header.stamp = ros::Time::now(); - mkSteps.ns = "extrapolated poses"; - mkSteps.action = visualization_msgs::Marker::ADD; - mkSteps.pose.orientation.w = 1.0; - mkSteps.id = __COUNTER__; - mkSteps.type = visualization_msgs::Marker::POINTS; - mkSteps.scale.x = 0.5; - mkSteps.scale.y = 0.5; - mkSteps.color.r = 1.0; - mkSteps.color.g = 0.5; - mkSteps.color.a = 1.0; - - visualization_msgs::Marker mkPosesOnPath; - mkPosesOnPath.header.frame_id = map_frame_; - mkPosesOnPath.header.stamp = ros::Time::now(); - mkPosesOnPath.ns = "goal poses on path"; - mkPosesOnPath.action = visualization_msgs::Marker::ADD; - mkPosesOnPath.pose.orientation.w = 1.0; - mkPosesOnPath.id = __COUNTER__; - mkPosesOnPath.type = visualization_msgs::Marker::POINTS; - mkPosesOnPath.scale.x = 0.5; - mkPosesOnPath.scale.y = 0.5; - mkPosesOnPath.color.r = 1.0; - mkPosesOnPath.color.g = 1.0; - mkPosesOnPath.color.a = 1.0; - - visualization_msgs::Marker mkCollisionFootprint; - mkCollisionFootprint.header.frame_id = map_frame_; - mkCollisionFootprint.header.stamp = ros::Time::now(); - mkCollisionFootprint.ns = "Collision footprint"; - mkCollisionFootprint.action = visualization_msgs::Marker::ADD; - mkCollisionFootprint.pose.orientation.w = 1.0; - mkCollisionFootprint.id = __COUNTER__; - mkCollisionFootprint.type = visualization_msgs::Marker::LINE_LIST; - mkCollisionFootprint.scale.x = 0.1; - mkCollisionFootprint.color.b = 1.0; - mkCollisionFootprint.color.a = 0.3; - - visualization_msgs::Marker mkCollisionHull; - mkCollisionHull.header.frame_id = map_frame_; - mkCollisionHull.header.stamp = ros::Time::now(); - mkCollisionHull.ns = "Collision polygon"; - mkCollisionHull.action = visualization_msgs::Marker::ADD; - mkCollisionHull.pose.orientation.w = 1.0; - mkCollisionHull.id = __COUNTER__; - mkCollisionHull.type = visualization_msgs::Marker::LINE_STRIP; - mkCollisionHull.scale.x = 0.2; - mkCollisionHull.color.r = 1.0; - mkCollisionHull.color.a = 0.3; - - visualization_msgs::Marker mkCollisionIndicator; - mkCollisionIndicator.header.frame_id = map_frame_; - mkCollisionIndicator.header.stamp = ros::Time::now(); - mkCollisionIndicator.ns = "Collision object"; - mkCollisionIndicator.pose.orientation.w = 1.0; - mkCollisionIndicator.id = __COUNTER__; - mkCollisionIndicator.type = visualization_msgs::Marker::CYLINDER; - mkCollisionIndicator.scale.x = 0.5; - mkCollisionIndicator.scale.y = 0.5; - mkCollisionIndicator.color.r = 1.0; - mkCollisionIndicator.color.a = 0.0; - visualization_msgs::MarkerArray mkCollision; - // Check how far we should check forward double x_vel = pid_controller_.getControllerState().current_x_vel; double collision_look_ahead_distance = @@ -383,6 +316,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() auto projected_controller_state = pid_controller_.getControllerState(); // Step until lookahead is reached, for every step project the pose back to the path + std::vector step_points; + std::vector poses_on_path_points; std::vector projected_steps_tf; auto projected_step_tf = tf2_convert(tfCurPoseStamped_.transform); projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link @@ -398,12 +333,13 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Fill markers: geometry_msgs::Point mkStep; tf2::toMsg(next_straight_step_tf.getOrigin(), mkStep); - mkSteps.points.push_back(mkStep); + step_points.push_back(mkStep); geometry_msgs::Point mkPointOnPath; tf2::toMsg(projected_step_tf.getOrigin(), mkPointOnPath); - mkPosesOnPath.points.push_back(mkPointOnPath); + poses_on_path_points.push_back(mkPointOnPath); } + std::vector collision_footprint_points; polygon_t previous_footprint_xy; polygon_t collision_polygon; for (const auto & projection_tf : projected_steps_tf) { @@ -430,8 +366,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Add footprint to marker geometry_msgs::Point previous_point = footprint.back(); for (const auto & point : footprint) { - mkCollisionFootprint.points.push_back(previous_point); - mkCollisionFootprint.points.push_back(point); + collision_footprint_points.push_back(previous_point); + collision_footprint_points.push_back(point); previous_point = point; } } @@ -456,6 +392,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() costmap2d->convexFillCells(collision_polygon_hull_map, cells_in_polygon); // Get the max cost inside the concave polygon + geometry_msgs::Point collision_point; uint8_t max_cost = 0.0; for (const auto & cell_in_polygon : cells_in_polygon) { // Cost checker is cheaper than polygon checker, so lets do that first @@ -465,41 +402,25 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() geometry_msgs::Point point; costmap2d->mapToWorld(cell_in_polygon.x, cell_in_polygon.y, point.x, point.y); if (boost::geometry::within(point, collision_polygon)) { - // Protip: uncomment below and 'if (cell_cost > max_cost)' to see evaluated cells - // boost::unique_lock lock_controller(*(costmap2d->getMutex())); - // costmap2d->setCost(cell_in_polygon.x, cell_in_polygon.y, 100); - max_cost = cell_cost; // Set collision indicator on suspected cell with current cost - mkCollisionIndicator.scale.z = cell_cost / 255.0; - mkCollisionIndicator.color.a = cell_cost / 255.0; - point.z = mkCollisionIndicator.scale.z * 0.5; - mkCollisionIndicator.pose.position = point; + collision_point = point; if (max_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { break; // Collision detected, no need to evaluate further } } } } - if (mkCollisionIndicator.scale.z > std::numeric_limits::epsilon()) { - mkCollisionIndicator.action = visualization_msgs::Marker::ADD; - } else { - mkCollisionIndicator.action = visualization_msgs::Marker::DELETE; - } - mkCollision.markers.push_back(mkCollisionIndicator); // Fiddle the polygon into a marker message + std::vector collision_hull_points; for (const geometry_msgs::Point point : collision_polygon) { - mkCollisionHull.points.push_back(point); + collision_hull_points.push_back(point); } - mkCollision.markers.push_back(mkCollisionFootprint); - mkCollision.markers.push_back(mkCollisionHull); - if (n_steps > 0) { - mkCollision.markers.push_back(mkSteps); - mkCollision.markers.push_back(mkPosesOnPath); - } - collision_marker_pub_.publish(mkCollision); + visualization_->publishCollision( + map_frame_, max_cost, collision_point, collision_footprint_points, collision_hull_points, + step_points, poses_on_path_points); return max_cost; } diff --git a/src/visualization.cpp b/src/visualization.cpp index 9366e861..fe24703b 100644 --- a/src/visualization.cpp +++ b/src/visualization.cpp @@ -1,15 +1,18 @@ #include #include #include +#include +#include #include #include namespace path_tracking_pid { Visualization::Visualization(ros::NodeHandle nh) +: marker_pub_{nh.advertise("visualization_marker", 4)}, + collision_marker_pub_{nh.advertise("collision_markers", 3)} { - marker_pub_ = nh.advertise("visualization_marker", 4); } void Visualization::publishControlPoint( @@ -47,6 +50,103 @@ void Visualization::publishPlanPoint(const std_msgs::Header & header, const tf2: publishSphere(header, "plan point", __COUNTER__, pose, color); } +void Visualization::publishCollision( + const std::string & frame_id, uint8_t cost, const point_t & point, const points_t & footprint, + const points_t & hull, const points_t & steps, const points_t & path) +{ + visualization_msgs::Marker mkSteps; + mkSteps.header.frame_id = frame_id; + mkSteps.header.stamp = ros::Time::now(); + mkSteps.ns = "extrapolated poses"; + mkSteps.action = visualization_msgs::Marker::ADD; + mkSteps.pose.orientation.w = 1.0; + mkSteps.id = __COUNTER__; + mkSteps.type = visualization_msgs::Marker::POINTS; + mkSteps.scale.x = 0.5; + mkSteps.scale.y = 0.5; + mkSteps.color.r = 1.0; + mkSteps.color.g = 0.5; + mkSteps.color.a = 1.0; + mkSteps.points = steps; + + visualization_msgs::Marker mkPosesOnPath; + mkPosesOnPath.header.frame_id = frame_id; + mkPosesOnPath.header.stamp = ros::Time::now(); + mkPosesOnPath.ns = "goal poses on path"; + mkPosesOnPath.action = visualization_msgs::Marker::ADD; + mkPosesOnPath.pose.orientation.w = 1.0; + mkPosesOnPath.id = __COUNTER__; + mkPosesOnPath.type = visualization_msgs::Marker::POINTS; + mkPosesOnPath.scale.x = 0.5; + mkPosesOnPath.scale.y = 0.5; + mkPosesOnPath.color.r = 1.0; + mkPosesOnPath.color.g = 1.0; + mkPosesOnPath.color.a = 1.0; + mkPosesOnPath.points = path; + + visualization_msgs::Marker mkCollisionFootprint; + mkCollisionFootprint.header.frame_id = frame_id; + mkCollisionFootprint.header.stamp = ros::Time::now(); + mkCollisionFootprint.ns = "Collision footprint"; + mkCollisionFootprint.action = visualization_msgs::Marker::ADD; + mkCollisionFootprint.pose.orientation.w = 1.0; + mkCollisionFootprint.id = __COUNTER__; + mkCollisionFootprint.type = visualization_msgs::Marker::LINE_LIST; + mkCollisionFootprint.scale.x = 0.1; + mkCollisionFootprint.color.b = 1.0; + mkCollisionFootprint.color.a = 0.3; + mkCollisionFootprint.points = footprint; + + visualization_msgs::Marker mkCollisionHull; + mkCollisionHull.header.frame_id = frame_id; + mkCollisionHull.header.stamp = ros::Time::now(); + mkCollisionHull.ns = "Collision polygon"; + mkCollisionHull.action = visualization_msgs::Marker::ADD; + mkCollisionHull.pose.orientation.w = 1.0; + mkCollisionHull.id = __COUNTER__; + mkCollisionHull.type = visualization_msgs::Marker::LINE_STRIP; + mkCollisionHull.scale.x = 0.2; + mkCollisionHull.color.r = 1.0; + mkCollisionHull.color.a = 0.3; + mkCollisionHull.points = hull; + + visualization_msgs::Marker mkCollisionIndicator; + mkCollisionIndicator.header.frame_id = frame_id; + mkCollisionIndicator.header.stamp = ros::Time::now(); + mkCollisionIndicator.ns = "Collision object"; + mkCollisionIndicator.pose.orientation.w = 1.0; + mkCollisionIndicator.id = __COUNTER__; + mkCollisionIndicator.type = visualization_msgs::Marker::CYLINDER; + mkCollisionIndicator.scale.x = 0.5; + mkCollisionIndicator.scale.y = 0.5; + mkCollisionIndicator.color.r = 1.0; + mkCollisionIndicator.color.a = 0.0; + + visualization_msgs::MarkerArray mkCollision; + mkCollisionIndicator.scale.z = cost / 255.0; + mkCollisionIndicator.color.a = cost / 255.0; + mkCollisionIndicator.pose.position = point; + mkCollisionIndicator.pose.position.z = mkCollisionIndicator.scale.z * 0.5; + if (mkCollisionIndicator.scale.z > std::numeric_limits::epsilon()) { + mkCollisionIndicator.action = visualization_msgs::Marker::ADD; + } else { + mkCollisionIndicator.action = visualization_msgs::Marker::DELETE; + } + + mkCollision.markers.push_back(mkCollisionIndicator); + + mkCollision.markers.push_back(mkCollisionFootprint); + mkCollision.markers.push_back(mkCollisionHull); + if (!mkSteps.points.empty()) { + mkCollision.markers.push_back(mkSteps); + } + if (!mkPosesOnPath.points.empty()) { + mkCollision.markers.push_back(mkPosesOnPath); + } + + collision_marker_pub_.publish(mkCollision); +} + void Visualization::publishSphere( const std_msgs::Header & header, const std::string & ns, int id, const tf2::Transform & pose, const std_msgs::ColorRGBA & color) @@ -57,7 +157,7 @@ void Visualization::publishSphere( } void Visualization::publishSphere( - const std_msgs::Header & header, const std::string & ns, int id, geometry_msgs::Pose pose, + const std_msgs::Header & header, const std::string & ns, int id, const geometry_msgs::Pose & pose, const std_msgs::ColorRGBA & color) { visualization_msgs::Marker marker; From 2a0762e90ade86a04fcdde0de52e95b3d2dc8b3f Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 28 Feb 2022 11:34:59 +0000 Subject: [PATCH 108/156] Reworked by renaming local variable. --- src/path_tracking_pid_local_planner.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index b2dd16ff..d36a6a76 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -109,10 +109,10 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector(tfCurPoseStamped_.transform), latest_odom_.twist.twist, tf2_convert(tf_base_to_steered_wheel_stamped_.transform), steering_odom_twist, - convert_plan(global_plan_copy)); + convert_plan(global_plan_map_frame)); } else { pid_controller_.setPlan( tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, - convert_plan(global_plan_copy)); + convert_plan(global_plan_map_frame)); } pid_controller_.setEnabled(true); From d2bcbbd1c036f409ba1c92c2188fea74ab13fb21 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 28 Feb 2022 14:39:55 +0000 Subject: [PATCH 109/156] Keep clang-tidy happy. --- src/calculations.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/calculations.cpp b/src/calculations.cpp index 2fbeb57c..fffb7a39 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -8,17 +8,17 @@ namespace path_tracking_pid { -std::vector deltas_of_plan(const std::vector & input) +std::vector deltas_of_plan(const std::vector & plan) { auto result = std::vector{}; - if (input.size() < 2) { + if (plan.size() < 2) { return result; } - result.reserve(input.size() - 1); + result.reserve(plan.size() - 1); std::transform( - input.cbegin(), input.cend() - 1, input.cbegin() + 1, std::back_inserter(result), + plan.cbegin(), plan.cend() - 1, plan.cbegin() + 1, std::back_inserter(result), [](const tf2::Transform & a, const tf2::Transform & b) { return a.inverseTimes(b); }); return result; From 836635450397c4132744b1d0aea9f1b602aa71b4 Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Tue, 1 Mar 2022 10:02:44 +0100 Subject: [PATCH 110/156] 2.18.0 ros-dynamic_reconfigure 1.7.2 9cd4f8d ros-path_tracking_pid 2.18.0 5f3c36e ros-roslint 0.12.0 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 583c5e18..6527d328 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ path_tracking_pid - 2.16.0 + 2.18.0 Follows a trajectory with open-loop speed and closed loop (pid) lateral control Cesar Lopez From 2e9447ed4d8a2d1ddb0a370e9520cff4ccc0fed7 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Wed, 2 Mar 2022 12:57:02 +0100 Subject: [PATCH 111/156] Refactor collision visualization functions --- include/path_tracking_pid/visualization.hpp | 19 +- src/path_tracking_pid_local_planner.cpp | 40 ++-- src/visualization.cpp | 213 +++++++++++--------- 3 files changed, 153 insertions(+), 119 deletions(-) diff --git a/include/path_tracking_pid/visualization.hpp b/include/path_tracking_pid/visualization.hpp index d2c066b8..351f2565 100644 --- a/include/path_tracking_pid/visualization.hpp +++ b/include/path_tracking_pid/visualization.hpp @@ -4,29 +4,32 @@ #include #include +#include namespace path_tracking_pid { - class Visualization { public: - using point_t = geometry_msgs::Point; - using points_t = std::vector; - explicit Visualization(ros::NodeHandle nh); void publishControlPoint(const std_msgs::Header & header, const tf2::Transform & pose); void publishAxlePoint(const std_msgs::Header & header, const tf2::Transform & pose); void publishGoalPoint(const std_msgs::Header & header, const tf2::Transform & pose); void publishPlanPoint(const std_msgs::Header & header, const tf2::Transform & pose); - void publishCollision( - const std::string & frame_id, uint8_t cost, const point_t & point, const points_t & footprint, - const points_t & hull, const points_t & steps, const points_t & path); + void publishCollisionObject( + const std_msgs::Header & header, uint8_t cost, const tf2::Vector3 & point); + void publishExtrapolatedPoses( + const std_msgs::Header & header, const std::vector & steps); + void publishgGoalPosesOnPath( + const std_msgs::Header & header, const std::vector & path); + void publishCollisionFootprint( + const std_msgs::Header & header, const std::vector & footprint); + void publishCollisionPolygon( + const std_msgs::Header & header, const std::vector & hull); private: ros::Publisher marker_pub_; - ros::Publisher collision_marker_pub_; void publishSphere( const std_msgs::Header & header, const std::string & ns, int id, const tf2::Transform & pose, diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 82e50dcb..94503a9f 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -316,8 +316,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() auto projected_controller_state = pid_controller_.getControllerState(); // Step until lookahead is reached, for every step project the pose back to the path - std::vector step_points; - std::vector poses_on_path_points; + std::vector step_points; + std::vector poses_on_path_points; std::vector projected_steps_tf; auto projected_step_tf = tf2_convert(tfCurPoseStamped_.transform); projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link @@ -331,15 +331,11 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() projected_steps_tf.push_back(projected_step_tf); // Fill markers: - geometry_msgs::Point mkStep; - tf2::toMsg(next_straight_step_tf.getOrigin(), mkStep); - step_points.push_back(mkStep); - geometry_msgs::Point mkPointOnPath; - tf2::toMsg(projected_step_tf.getOrigin(), mkPointOnPath); - poses_on_path_points.push_back(mkPointOnPath); + step_points.push_back(next_straight_step_tf.getOrigin()); + poses_on_path_points.push_back(projected_step_tf.getOrigin()); } - std::vector collision_footprint_points; + std::vector collision_footprint_points; polygon_t previous_footprint_xy; polygon_t collision_polygon; for (const auto & projection_tf : projected_steps_tf) { @@ -366,8 +362,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() // Add footprint to marker geometry_msgs::Point previous_point = footprint.back(); for (const auto & point : footprint) { - collision_footprint_points.push_back(previous_point); - collision_footprint_points.push_back(point); + collision_footprint_points.push_back(tf2_convert(previous_point)); + collision_footprint_points.push_back(tf2_convert(point)); previous_point = point; } } @@ -392,7 +388,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() costmap2d->convexFillCells(collision_polygon_hull_map, cells_in_polygon); // Get the max cost inside the concave polygon - geometry_msgs::Point collision_point; + tf2::Vector3 collision_point; uint8_t max_cost = 0.0; for (const auto & cell_in_polygon : cells_in_polygon) { // Cost checker is cheaper than polygon checker, so lets do that first @@ -404,7 +400,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() if (boost::geometry::within(point, collision_polygon)) { max_cost = cell_cost; // Set collision indicator on suspected cell with current cost - collision_point = point; + collision_point = tf2_convert(point); if (max_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { break; // Collision detected, no need to evaluate further } @@ -413,14 +409,20 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() } // Fiddle the polygon into a marker message - std::vector collision_hull_points; + std::vector collision_hull_points; for (const geometry_msgs::Point point : collision_polygon) { - collision_hull_points.push_back(point); + tf2::Vector3 point_tf2; + tf2::fromMsg(point, point_tf2); + collision_hull_points.push_back(point_tf2); } - - visualization_->publishCollision( - map_frame_, max_cost, collision_point, collision_footprint_points, collision_hull_points, - step_points, poses_on_path_points); + std_msgs::Header header; + header.stamp = ros::Time::now(); + header.frame_id = map_frame_; + visualization_->publishCollisionObject(header, max_cost, collision_point); + visualization_->publishExtrapolatedPoses(header, step_points); + visualization_->publishgGoalPosesOnPath(header, poses_on_path_points); + visualization_->publishCollisionFootprint(header, collision_footprint_points); + visualization_->publishCollisionPolygon(header, collision_hull_points); return max_cost; } diff --git a/src/visualization.cpp b/src/visualization.cpp index fe24703b..cd9fb912 100644 --- a/src/visualization.cpp +++ b/src/visualization.cpp @@ -1,17 +1,17 @@ #include #include #include -#include +#include #include #include #include +#include namespace path_tracking_pid { Visualization::Visualization(ros::NodeHandle nh) -: marker_pub_{nh.advertise("visualization_marker", 4)}, - collision_marker_pub_{nh.advertise("collision_markers", 3)} +: marker_pub_{nh.advertise("visualization_marker", 10)} { } @@ -50,101 +50,130 @@ void Visualization::publishPlanPoint(const std_msgs::Header & header, const tf2: publishSphere(header, "plan point", __COUNTER__, pose, color); } -void Visualization::publishCollision( - const std::string & frame_id, uint8_t cost, const point_t & point, const points_t & footprint, - const points_t & hull, const points_t & steps, const points_t & path) +void Visualization::publishCollisionObject( + const std_msgs::Header & header, uint8_t cost, const tf2::Vector3 & point) { - visualization_msgs::Marker mkSteps; - mkSteps.header.frame_id = frame_id; - mkSteps.header.stamp = ros::Time::now(); - mkSteps.ns = "extrapolated poses"; - mkSteps.action = visualization_msgs::Marker::ADD; - mkSteps.pose.orientation.w = 1.0; - mkSteps.id = __COUNTER__; - mkSteps.type = visualization_msgs::Marker::POINTS; - mkSteps.scale.x = 0.5; - mkSteps.scale.y = 0.5; - mkSteps.color.r = 1.0; - mkSteps.color.g = 0.5; - mkSteps.color.a = 1.0; - mkSteps.points = steps; - - visualization_msgs::Marker mkPosesOnPath; - mkPosesOnPath.header.frame_id = frame_id; - mkPosesOnPath.header.stamp = ros::Time::now(); - mkPosesOnPath.ns = "goal poses on path"; - mkPosesOnPath.action = visualization_msgs::Marker::ADD; - mkPosesOnPath.pose.orientation.w = 1.0; - mkPosesOnPath.id = __COUNTER__; - mkPosesOnPath.type = visualization_msgs::Marker::POINTS; - mkPosesOnPath.scale.x = 0.5; - mkPosesOnPath.scale.y = 0.5; - mkPosesOnPath.color.r = 1.0; - mkPosesOnPath.color.g = 1.0; - mkPosesOnPath.color.a = 1.0; - mkPosesOnPath.points = path; - - visualization_msgs::Marker mkCollisionFootprint; - mkCollisionFootprint.header.frame_id = frame_id; - mkCollisionFootprint.header.stamp = ros::Time::now(); - mkCollisionFootprint.ns = "Collision footprint"; - mkCollisionFootprint.action = visualization_msgs::Marker::ADD; - mkCollisionFootprint.pose.orientation.w = 1.0; - mkCollisionFootprint.id = __COUNTER__; - mkCollisionFootprint.type = visualization_msgs::Marker::LINE_LIST; - mkCollisionFootprint.scale.x = 0.1; - mkCollisionFootprint.color.b = 1.0; - mkCollisionFootprint.color.a = 0.3; - mkCollisionFootprint.points = footprint; - - visualization_msgs::Marker mkCollisionHull; - mkCollisionHull.header.frame_id = frame_id; - mkCollisionHull.header.stamp = ros::Time::now(); - mkCollisionHull.ns = "Collision polygon"; - mkCollisionHull.action = visualization_msgs::Marker::ADD; - mkCollisionHull.pose.orientation.w = 1.0; - mkCollisionHull.id = __COUNTER__; - mkCollisionHull.type = visualization_msgs::Marker::LINE_STRIP; - mkCollisionHull.scale.x = 0.2; - mkCollisionHull.color.r = 1.0; - mkCollisionHull.color.a = 0.3; - mkCollisionHull.points = hull; - - visualization_msgs::Marker mkCollisionIndicator; - mkCollisionIndicator.header.frame_id = frame_id; - mkCollisionIndicator.header.stamp = ros::Time::now(); - mkCollisionIndicator.ns = "Collision object"; - mkCollisionIndicator.pose.orientation.w = 1.0; - mkCollisionIndicator.id = __COUNTER__; - mkCollisionIndicator.type = visualization_msgs::Marker::CYLINDER; - mkCollisionIndicator.scale.x = 0.5; - mkCollisionIndicator.scale.y = 0.5; - mkCollisionIndicator.color.r = 1.0; - mkCollisionIndicator.color.a = 0.0; - - visualization_msgs::MarkerArray mkCollision; - mkCollisionIndicator.scale.z = cost / 255.0; - mkCollisionIndicator.color.a = cost / 255.0; - mkCollisionIndicator.pose.position = point; - mkCollisionIndicator.pose.position.z = mkCollisionIndicator.scale.z * 0.5; - if (mkCollisionIndicator.scale.z > std::numeric_limits::epsilon()) { - mkCollisionIndicator.action = visualization_msgs::Marker::ADD; + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = "Collision object"; + marker.pose.orientation.w = 1.0; + marker.id = __COUNTER__; + marker.type = visualization_msgs::Marker::CYLINDER; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.color.r = 1.0; + marker.color.a = 0.0; + marker.scale.z = cost / 255.0; + marker.color.a = cost / 255.0; + tf2::toMsg(point, marker.pose.position); + marker.pose.position.z = marker.scale.z * 0.5; + if (marker.scale.z > std::numeric_limits::epsilon()) { + marker.action = visualization_msgs::Marker::ADD; } else { - mkCollisionIndicator.action = visualization_msgs::Marker::DELETE; + marker.action = visualization_msgs::Marker::DELETE; } - mkCollision.markers.push_back(mkCollisionIndicator); + marker_pub_.publish(marker); +} - mkCollision.markers.push_back(mkCollisionFootprint); - mkCollision.markers.push_back(mkCollisionHull); - if (!mkSteps.points.empty()) { - mkCollision.markers.push_back(mkSteps); - } - if (!mkPosesOnPath.points.empty()) { - mkCollision.markers.push_back(mkPosesOnPath); - } +void Visualization::publishExtrapolatedPoses( + const std_msgs::Header & header, const std::vector & steps) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = "extrapolated poses"; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.id = __COUNTER__; + marker.type = visualization_msgs::Marker::POINTS; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.color.r = 1.0; + marker.color.g = 0.5; + marker.color.a = 1.0; + + std::transform( + steps.begin(), steps.end(), std::back_inserter(marker.points), [](const auto & step) { + geometry_msgs::Point p; + tf2::toMsg(step, p); + return p; + }); - collision_marker_pub_.publish(mkCollision); + marker_pub_.publish(marker); +} + +void Visualization::publishgGoalPosesOnPath( + const std_msgs::Header & header, const std::vector & path) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = "goal poses on path"; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.id = __COUNTER__; + marker.type = visualization_msgs::Marker::POINTS; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.a = 1.0; + + std::transform( + path.begin(), path.end(), std::back_inserter(marker.points), [](const auto & step) { + geometry_msgs::Point p; + tf2::toMsg(step, p); + return p; + }); + + marker_pub_.publish(marker); +} + +void Visualization::publishCollisionFootprint( + const std_msgs::Header & header, const std::vector & footprint) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = "Collision footprint"; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.id = __COUNTER__; + marker.type = visualization_msgs::Marker::LINE_LIST; + marker.scale.x = 0.1; + marker.color.b = 1.0; + marker.color.a = 0.3; + + std::transform( + footprint.begin(), footprint.end(), std::back_inserter(marker.points), [](const auto & step) { + geometry_msgs::Point p; + tf2::toMsg(step, p); + return p; + }); + + marker_pub_.publish(marker); +} + +void Visualization::publishCollisionPolygon( + const std_msgs::Header & header, const std::vector & hull) +{ + visualization_msgs::Marker marker; + marker.header = header; + marker.ns = "Collision polygon"; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.id = __COUNTER__; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.scale.x = 0.2; + marker.color.r = 1.0; + marker.color.a = 0.3; + + std::transform( + hull.begin(), hull.end(), std::back_inserter(marker.points), [](const auto & step) { + geometry_msgs::Point p; + tf2::toMsg(step, p); + return p; + }); + + marker_pub_.publish(marker); } void Visualization::publishSphere( From 6bd6587480fae48bc4e8eb95a13be1559ea2e4f1 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Thu, 3 Mar 2022 11:42:17 +0100 Subject: [PATCH 112/156] Remove unnecessary marker ids The markers are already unique because of the ns. --- include/path_tracking_pid/visualization.hpp | 6 +++--- src/visualization.cpp | 21 +++++++-------------- 2 files changed, 10 insertions(+), 17 deletions(-) diff --git a/include/path_tracking_pid/visualization.hpp b/include/path_tracking_pid/visualization.hpp index 351f2565..bc06d764 100644 --- a/include/path_tracking_pid/visualization.hpp +++ b/include/path_tracking_pid/visualization.hpp @@ -32,11 +32,11 @@ class Visualization ros::Publisher marker_pub_; void publishSphere( - const std_msgs::Header & header, const std::string & ns, int id, const tf2::Transform & pose, + const std_msgs::Header & header, const std::string & ns, const tf2::Transform & pose, const std_msgs::ColorRGBA & color); void publishSphere( - const std_msgs::Header & header, const std::string & ns, int id, - const geometry_msgs::Pose & pose, const std_msgs::ColorRGBA & color); + const std_msgs::Header & header, const std::string & ns, const geometry_msgs::Pose & pose, + const std_msgs::ColorRGBA & color); }; } // namespace path_tracking_pid diff --git a/src/visualization.cpp b/src/visualization.cpp index cd9fb912..9ec207a9 100644 --- a/src/visualization.cpp +++ b/src/visualization.cpp @@ -21,8 +21,7 @@ void Visualization::publishControlPoint( std_msgs::ColorRGBA color; color.a = 1.0; color.g = 1.0; - // id has to be unique, so using a compile-time counter :) - publishSphere(header, "control point", __COUNTER__, pose, color); + publishSphere(header, "control point", pose, color); } void Visualization::publishAxlePoint(const std_msgs::Header & header, const tf2::Transform & pose) @@ -30,7 +29,7 @@ void Visualization::publishAxlePoint(const std_msgs::Header & header, const tf2: std_msgs::ColorRGBA color; color.a = 1.0; color.b = 1.0; - publishSphere(header, "axle point", __COUNTER__, pose, color); + publishSphere(header, "axle point", pose, color); } void Visualization::publishGoalPoint(const std_msgs::Header & header, const tf2::Transform & pose) @@ -38,7 +37,7 @@ void Visualization::publishGoalPoint(const std_msgs::Header & header, const tf2: std_msgs::ColorRGBA color; color.a = 1.0; color.r = 1.0; - publishSphere(header, "goal point", __COUNTER__, pose, color); + publishSphere(header, "goal point", pose, color); } void Visualization::publishPlanPoint(const std_msgs::Header & header, const tf2::Transform & pose) @@ -47,7 +46,7 @@ void Visualization::publishPlanPoint(const std_msgs::Header & header, const tf2: color.a = 1.0; color.g = 0.5; color.r = 1.0; - publishSphere(header, "plan point", __COUNTER__, pose, color); + publishSphere(header, "plan point", pose, color); } void Visualization::publishCollisionObject( @@ -57,7 +56,6 @@ void Visualization::publishCollisionObject( marker.header = header; marker.ns = "Collision object"; marker.pose.orientation.w = 1.0; - marker.id = __COUNTER__; marker.type = visualization_msgs::Marker::CYLINDER; marker.scale.x = 0.5; marker.scale.y = 0.5; @@ -84,7 +82,6 @@ void Visualization::publishExtrapolatedPoses( marker.ns = "extrapolated poses"; marker.action = visualization_msgs::Marker::ADD; marker.pose.orientation.w = 1.0; - marker.id = __COUNTER__; marker.type = visualization_msgs::Marker::POINTS; marker.scale.x = 0.5; marker.scale.y = 0.5; @@ -110,7 +107,6 @@ void Visualization::publishgGoalPosesOnPath( marker.ns = "goal poses on path"; marker.action = visualization_msgs::Marker::ADD; marker.pose.orientation.w = 1.0; - marker.id = __COUNTER__; marker.type = visualization_msgs::Marker::POINTS; marker.scale.x = 0.5; marker.scale.y = 0.5; @@ -136,7 +132,6 @@ void Visualization::publishCollisionFootprint( marker.ns = "Collision footprint"; marker.action = visualization_msgs::Marker::ADD; marker.pose.orientation.w = 1.0; - marker.id = __COUNTER__; marker.type = visualization_msgs::Marker::LINE_LIST; marker.scale.x = 0.1; marker.color.b = 1.0; @@ -160,7 +155,6 @@ void Visualization::publishCollisionPolygon( marker.ns = "Collision polygon"; marker.action = visualization_msgs::Marker::ADD; marker.pose.orientation.w = 1.0; - marker.id = __COUNTER__; marker.type = visualization_msgs::Marker::LINE_STRIP; marker.scale.x = 0.2; marker.color.r = 1.0; @@ -177,22 +171,21 @@ void Visualization::publishCollisionPolygon( } void Visualization::publishSphere( - const std_msgs::Header & header, const std::string & ns, int id, const tf2::Transform & pose, + const std_msgs::Header & header, const std::string & ns, const tf2::Transform & pose, const std_msgs::ColorRGBA & color) { geometry_msgs::Pose msg; tf2::toMsg(pose, msg); - publishSphere(header, ns, id, msg, color); + publishSphere(header, ns, msg, color); } void Visualization::publishSphere( - const std_msgs::Header & header, const std::string & ns, int id, const geometry_msgs::Pose & pose, + const std_msgs::Header & header, const std::string & ns, const geometry_msgs::Pose & pose, const std_msgs::ColorRGBA & color) { visualization_msgs::Marker marker; marker.header = header; marker.ns = ns; - marker.id = id; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; marker.pose = pose; From 7c24fc53528329a0295d90950e6aec965c05a843 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Thu, 3 Mar 2022 11:42:50 +0100 Subject: [PATCH 113/156] Refactor std::transform calls into a separate function --- src/visualization.cpp | 47 ++++++++++++++----------------------------- 1 file changed, 15 insertions(+), 32 deletions(-) diff --git a/src/visualization.cpp b/src/visualization.cpp index 9ec207a9..12bafc0a 100644 --- a/src/visualization.cpp +++ b/src/visualization.cpp @@ -10,6 +10,17 @@ namespace path_tracking_pid { +std::vector to_msg(std::vector points) +{ + std::vector msgs; + std::transform(points.begin(), points.end(), std::back_inserter(msgs), [](const auto & msg) { + geometry_msgs::Point p; + tf2::toMsg(msg, p); + return p; + }); + return msgs; +} + Visualization::Visualization(ros::NodeHandle nh) : marker_pub_{nh.advertise("visualization_marker", 10)} { @@ -88,14 +99,7 @@ void Visualization::publishExtrapolatedPoses( marker.color.r = 1.0; marker.color.g = 0.5; marker.color.a = 1.0; - - std::transform( - steps.begin(), steps.end(), std::back_inserter(marker.points), [](const auto & step) { - geometry_msgs::Point p; - tf2::toMsg(step, p); - return p; - }); - + marker.points = to_msg(steps); marker_pub_.publish(marker); } @@ -113,14 +117,7 @@ void Visualization::publishgGoalPosesOnPath( marker.color.r = 1.0; marker.color.g = 1.0; marker.color.a = 1.0; - - std::transform( - path.begin(), path.end(), std::back_inserter(marker.points), [](const auto & step) { - geometry_msgs::Point p; - tf2::toMsg(step, p); - return p; - }); - + marker.points = to_msg(path); marker_pub_.publish(marker); } @@ -136,14 +133,7 @@ void Visualization::publishCollisionFootprint( marker.scale.x = 0.1; marker.color.b = 1.0; marker.color.a = 0.3; - - std::transform( - footprint.begin(), footprint.end(), std::back_inserter(marker.points), [](const auto & step) { - geometry_msgs::Point p; - tf2::toMsg(step, p); - return p; - }); - + marker.points = to_msg(footprint); marker_pub_.publish(marker); } @@ -159,14 +149,7 @@ void Visualization::publishCollisionPolygon( marker.scale.x = 0.2; marker.color.r = 1.0; marker.color.a = 0.3; - - std::transform( - hull.begin(), hull.end(), std::back_inserter(marker.points), [](const auto & step) { - geometry_msgs::Point p; - tf2::toMsg(step, p); - return p; - }); - + marker.points = to_msg(hull); marker_pub_.publish(marker); } From 33b026255f58319ffb4144a3d1ebd28aa7014bff Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 28 Feb 2022 13:21:46 +0000 Subject: [PATCH 114/156] Simplified the calculation of goal direction. --- src/controller.cpp | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 588571bd..84897632 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -30,13 +30,6 @@ constexpr double ang_lower_limit = -100.0; // Anti-windup term. Limits the absolute value of the integral term. constexpr double windup_limit = 1000.0; -// Indicates if both values have the same sign. -template -bool have_same_sign(T val1, T val2) -{ - return std::signbit(val1) == std::signbit(val2); -} - // Returns the square distance between two points double distSquared(const tf2::Transform & a, const tf2::Transform & b) { @@ -529,14 +522,13 @@ Controller::UpdateResult Controller::update( ROS_DEBUG("d_end_phase: %f", d_end_phase); ROS_DEBUG("distance_to_goal: %f", distance_to_goal_); - // Get 'angle' towards current_goal - tf2::Transform base_to_goal = current_tf.inverseTimes(current_goal_); - const double angle_to_goal = atan2(base_to_goal.getOrigin().x(), -base_to_goal.getOrigin().y()); + const auto in_direction_of_goal = + !std::signbit(current_tf.getOrigin().dot(current_goal_.getOrigin())); // If we are as close to our goal or closer then we need to reach end velocity, enable end_phase. // However, if robot is not facing to the same direction as the local velocity target vector, don't enable end_phase. // This is to avoid skipping paths that start with opposite velocity. - if ((distance_to_goal_ <= fabs(d_end_phase)) && have_same_sign(target_x_vel, angle_to_goal)) { + if ((distance_to_goal_ <= fabs(d_end_phase)) && in_direction_of_goal) { // This state will be remebered to avoid jittering on target_x_vel controller_state_.end_phase_enabled = true; } From c70762c0f0d503d838c347becbdea32e07ea75a5 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 3 Mar 2022 13:36:24 +0000 Subject: [PATCH 115/156] Reworked to separate function. --- src/calculations.cpp | 9 +++++ src/calculations.hpp | 12 ++++++ src/controller.cpp | 2 +- test/unittests/test_calculations.cpp | 56 ++++++++++++++++++++++++++++ 4 files changed, 78 insertions(+), 1 deletion(-) diff --git a/src/calculations.cpp b/src/calculations.cpp index fffb7a39..fa7da4f6 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -62,4 +62,13 @@ std::vector inverse_turning_radiuses(const std::vector & return result; } +bool is_in_direction_of_target( + const tf2::Transform & current, const tf2::Vector3 & target, double velocity) +{ + const auto delta = target - current.getOrigin(); + const auto projection = current.getBasis().tdotx(delta); + + return !std::signbit(projection * velocity); +} + } // namespace path_tracking_pid diff --git a/src/calculations.hpp b/src/calculations.hpp index ea04a297..ddb31176 100644 --- a/src/calculations.hpp +++ b/src/calculations.hpp @@ -34,4 +34,16 @@ std::vector distances_to_goal(const std::vector & deltas */ std::vector inverse_turning_radiuses(const std::vector & deltas); +/** + * Determine if the given current pose is in the direction of the given target position taking the + * given velocity into account. + * + * @param[in] current Current pose. + * @param[in] target Target position. + * @param[in] velocity Forward velocity in m/s. + * @return True if it is in the direction of the target, false otherwise. + */ +bool is_in_direction_of_target( + const tf2::Transform & current, const tf2::Vector3 & target, double velocity); + } // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index 84897632..873d24b6 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -523,7 +523,7 @@ Controller::UpdateResult Controller::update( ROS_DEBUG("distance_to_goal: %f", distance_to_goal_); const auto in_direction_of_goal = - !std::signbit(current_tf.getOrigin().dot(current_goal_.getOrigin())); + is_in_direction_of_target(current_tf, current_goal_.getOrigin(), target_x_vel); // If we are as close to our goal or closer then we need to reach end velocity, enable end_phase. // However, if robot is not facing to the same direction as the local velocity target vector, don't enable end_phase. diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index d0f964cc..417525d1 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -11,6 +11,7 @@ namespace using path_tracking_pid::deltas_of_plan; using path_tracking_pid::distances_to_goal; using path_tracking_pid::inverse_turning_radiuses; +using path_tracking_pid::is_in_direction_of_target; // Create a transform (with an identity basis) based on the given coordinates. tf2::Transform create_transform(double x, double y, double z) @@ -133,4 +134,59 @@ TEST(PathTrackingPidCalculations, InverseTurningRadiuses_IgnoreZ) EXPECT_EQ(ref, result); } +// Parameters for parameterized tests of is_in_direction_of_target(). +struct IsInDirectionOfTargetTestParams +{ + bool result = false; + tf2::Transform current; + tf2::Vector3 target; + double velocity = 0.0; +}; + +// Fixture for tests of is_in_direction_of_target(). +class IsInDirectionOfTargetTestFixture +: public ::testing::TestWithParam +{ +}; + +TEST_P(IsInDirectionOfTargetTestFixture, IsInDirectionOfTarget) +{ + const auto & [result, current, target, velocity] = GetParam(); + + EXPECT_EQ(result, is_in_direction_of_target(current, target, velocity)); + EXPECT_EQ(!result, is_in_direction_of_target(current, target, -velocity)); +} + +// Input for parameterized tests of is_in_direction_of_target(). +static const auto is_in_direction_of_target_params = std::vector({ + // base tests: x delta + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {2, 3, 4}}, {3, 3, 4}, 1.0}, + // 0 velocity + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {2, 3, 4}}, {3, 3, 4}, 0.0}, + // already at target + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {3, 3, 4}}, {3, 3, 4}, 1.0}, + // past target + {false, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {4, 3, 4}}, {3, 3, 4}, 1.0}, + // y delta + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {3, 2, 4}}, {3, 3, 4}, 1.0}, + // z delta + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {3, 3, 3}}, {3, 3, 4}, 1.0}, + // xyz delta + {true, tf2::Transform{{1, 0, 0, 0, 1, 0, 0, 0, 1}, {2, 2, 3}}, {3, 3, 4}, 1.0}, + // 0 basis + {true, tf2::Transform{{0, 0, 0, 0, 0, 0, 0, 0, 0}, {2, 3, 4}}, {3, 4, 5}, 1.0}, + // inspired by actual values from integration tests + {false, tf2::Transform{{-1, -0.5, 0, 0.5, -1, 0, 0, 0, 1}, {-1, 2, 0}}, {-0.5, 2, 0}, 0.5}, + {false, tf2::Transform{{-1, 0.5, 0, -0.5, -1, 0, 0, 0, 1}, {-0.6, 2, 0}}, {-0.5, 2, 0}, 0.5}, + {true, tf2::Transform{{-0.5, -1, 0, 1, -0.5, 0, 0, 0, 1}, {12, 1.5, 0}}, {11, 1.8, 0}, 0.5}, + {true, tf2::Transform{{-1, -0.5, 0, 0.5, -1, 0, 0, 0, 1}, {11, 2.3, 0}}, {10, 2.1, 0}, 0.7}, + {true, tf2::Transform{{-1, 0.5, 0, -0.5, -1, 0, 0, 0, 1}, {9, 2.2, 0}}, {8, 2, 0}, 0.5}, + {true, tf2::Transform{{0.1, -1, 0, 1, 0.1, 0, 0, 0, 1}, {12, 1, 0}}, {11, 2, 0}, 0.5}, + {true, tf2::Transform{{1, -0.5, 0, 0.5, 1, 0, 0, 0, 1}, {11.2, 0.1, 0}}, {11.1, 0.7, 0}, 0.5}, +}); + +INSTANTIATE_TEST_SUITE_P( + PathTrackingPidCalculations, IsInDirectionOfTargetTestFixture, + ::testing::ValuesIn(is_in_direction_of_target_params)); + } // namespace From c8da2a21ece52b5e7dfb05d40b8e9d291678d8ac Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 3 Mar 2022 13:56:46 +0000 Subject: [PATCH 116/156] Switched to old GTest syntax for melodic. --- test/unittests/test_calculations.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index 417525d1..ec3ea8e7 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -185,7 +185,7 @@ static const auto is_in_direction_of_target_params = std::vector Date: Thu, 3 Mar 2022 15:19:25 +0000 Subject: [PATCH 117/156] Fix for melodic that is compatible with noetic. --- test/unittests/test_calculations.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index ec3ea8e7..ab483b5e 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -187,6 +187,7 @@ static const auto is_in_direction_of_target_params = std::vector Date: Mon, 28 Feb 2022 15:18:30 +0000 Subject: [PATCH 118/156] Disable nav_core::BaseLocalPlanner support. --- .../path_tracking_pid_local_planner.hpp | 31 +++++++++---------- src/path_tracking_pid_local_planner.cpp | 3 +- 2 files changed, 15 insertions(+), 19 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 5da57f1f..88c03aae 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include #include @@ -26,8 +25,7 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x, namespace path_tracking_pid { -class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, - public mbf_costmap_core::CostmapController, +class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, private boost::noncopyable { private: @@ -57,14 +55,6 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, */ bool setPlan(const std::vector & global_plan) override; - /** - * @brief Calculates the velocity command based on the current robot pose given by pose. See the interface in move - * base. - * @param cmd_vel Output the velocity command - * @return true if succeded. - */ - bool computeVelocityCommands(geometry_msgs::Twist & cmd_vel) override; // NOLINT - /** * @brief Calculates the velocity command based on the current robot pose given by pose. The velocity * and message are not set. See the interface in move base flex. @@ -78,12 +68,6 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, const geometry_msgs::PoseStamped & pose, const geometry_msgs::TwistStamped & velocity, geometry_msgs::TwistStamped & cmd_vel, std::string & message) override; // NOLINT - /** - * @brief Returns true, if the goal is reached. Currently does not respect the parameters give - * @return true, if the goal is reached - */ - bool isGoalReached() override; - /** * @brief Returns true, if the goal is reached. Currently does not respect the parameters given. * @param dist_tolerance Tolerance in distance to the goal @@ -104,6 +88,19 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner, enum class ComputeVelocityCommandsResult { SUCCESS = 0, GRACEFULLY_CANCELLING = 1 }; private: + /** + * @brief Calculates the velocity command based on the current robot pose given by pose. + * @param cmd_vel Output the velocity command + * @return true if succeded. + */ + bool computeVelocityCommands(geometry_msgs::Twist & cmd_vel); // NOLINT + + /** + * @brief Returns true, if the goal is reached. + * @return true, if the goal is reached + */ + bool isGoalReached() const; + /** * Accept a new configuration for the PID controller * @param config the new configuration diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 73fa7475..c625e791 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -20,7 +20,6 @@ #include "common.hpp" // register planner as move_base and move_base plugins -PLUGINLIB_EXPORT_CLASS(path_tracking_pid::TrackingPidLocalPlanner, nav_core::BaseLocalPlanner) PLUGINLIB_EXPORT_CLASS( path_tracking_pid::TrackingPidLocalPlanner, mbf_costmap_core::CostmapController) @@ -468,7 +467,7 @@ uint32_t TrackingPidLocalPlanner::computeVelocityCommands( return mbf_msgs::ExePathResult::SUCCESS; } -bool TrackingPidLocalPlanner::isGoalReached() +bool TrackingPidLocalPlanner::isGoalReached() const { // Return reached boolean, but never succeed when we're preempting return pid_controller_.getControllerState().end_reached && !cancel_in_progress_; From 5b0f1b896217b6337a29d60f70cf0f67f6545d74 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 28 Feb 2022 15:48:02 +0000 Subject: [PATCH 119/156] Removed unnecessary nav_core references. --- CMakeLists.txt | 1 - package.xml | 2 -- path_tracking_pid_plugin.xml | 5 ----- 3 files changed, 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a6bfc3c0..27153211 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ find_package(catkin REQUIRED mbf_costmap_core mbf_msgs message_generation - nav_core nav_msgs pluginlib roscpp diff --git a/package.xml b/package.xml index 6527d328..99e2b721 100644 --- a/package.xml +++ b/package.xml @@ -22,7 +22,6 @@ geometry_msgs mbf_costmap_core mbf_msgs - nav_core nav_msgs pluginlib roscpp @@ -37,7 +36,6 @@ rospy - diff --git a/path_tracking_pid_plugin.xml b/path_tracking_pid_plugin.xml index ae82430c..4c6e7031 100644 --- a/path_tracking_pid_plugin.xml +++ b/path_tracking_pid_plugin.xml @@ -1,9 +1,4 @@ - - - Local planner that tries to stay on the global path as well as it can, using multiple SISO PID controllers for steering calculation and an open-loop strategy for forward velocity - - Local planner that tries to stay on the global path as well as it can, using multiple SISO PID controllers for steering calculation and an open-loop strategy for forward velocity From 25305c381af66695104f0f27e94dda987438c253 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 28 Feb 2022 15:41:08 +0000 Subject: [PATCH 120/156] Controller no longer changes plan. --- include/path_tracking_pid/controller.hpp | 10 +++-- src/controller.cpp | 54 ++++++++++++------------ src/path_tracking_pid_local_planner.cpp | 18 +++++--- 3 files changed, 44 insertions(+), 38 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index ea41e582..f02c2a69 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -73,8 +73,9 @@ class Controller : private boost::noncopyable * @param current Where is the robot now? * @param odom_twist Robot odometry * @param global_plan Plan to follow + * @return whether the plan was successfully updated or not */ - void setPlan( + bool setPlan( const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, const std::vector & global_plan); @@ -85,12 +86,14 @@ class Controller : private boost::noncopyable * @param tf_base_to_steered_wheel Where is the steered wheel now? * @param steering_odom_twist Steered wheel odometry * @param global_plan Plan to follow + * @return whether the plan was successfully updated or not */ - void setPlan( + bool setPlan( const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, const tf2::Transform & tf_base_to_steered_wheel, const geometry_msgs::Twist & steering_odom_twist, const std::vector & global_plan); + /** * Find position on plan by looking at the surroundings of last known pose. * @param current Where is the robot now? @@ -137,8 +140,7 @@ class Controller : private boost::noncopyable * @return Update result */ UpdateResult update_with_limits( - const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, - ros::Duration dt); + const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, ros::Duration dt); /** * Perform prediction steps on the lateral error and return a reduced velocity that stays within bounds diff --git a/src/controller.cpp b/src/controller.cpp index 873d24b6..dfd12953 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -43,33 +43,27 @@ bool is_pose_angle_obtuse( return distSquared(prev, next) > (distSquared(prev, cur) + distSquared(cur, next)); } -// Filters the given plan. The first and last poses are always accepted (if they exist). -// Intermediate poses are only accepted if the angle (with respect to the previous and next poses) -// is obtuse. -std::vector filter_plan(const std::vector & plan) +/** + * Checks the given plan. The first and last poses are always accepted (if they exist). Intermediate + * poses are only accepted if the angle (with respect to the previous and next poses) is obtuse. + * + * @param[in] plan Plan to check. + * @return True if all poses in the plan are accepted. False otherwise. + */ +bool check_plan(const std::vector & plan) { const auto plan_size = plan.size(); - auto result = std::vector{}; - result.reserve(plan.size()); - - if (plan_size > 0) { - result.push_back(plan.front()); - } for (int pose_idx = 1; pose_idx < static_cast(plan_size) - 1; ++pose_idx) { const auto & prev_pose = plan[pose_idx - 1]; const auto & pose = plan[pose_idx]; const auto & next_pose = plan[pose_idx + 1]; - if (is_pose_angle_obtuse(prev_pose, pose, next_pose)) { - result.push_back(pose); + if (!is_pose_angle_obtuse(prev_pose, pose, next_pose)) { + return false; } } - if (plan_size > 1) { - result.push_back(plan.back()); - } - - return result; + return true; } } // namespace @@ -150,19 +144,19 @@ TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics( return steering_cmd_vel; } -void Controller::setPlan( +bool Controller::setPlan( const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, const std::vector & global_plan) { ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%zu)", global_plan.size()); - global_plan_tf_ = filter_plan(global_plan); - if (global_plan_tf_.size() != global_plan.size()) { - ROS_WARN( - "Not all poses of path are used since not all poses were in the expected direction of the " - "path!"); + if (!check_plan(global_plan)) { + ROS_WARN("Rejected plan because not all poses were in the expected direction of the path!"); + return false; } + global_plan_tf_ = global_plan; + if (!config_.track_base_link) { // Add carrot length to plan using goal pose (we assume the last pose contains correct angle) tf2::Transform carrotTF( @@ -226,17 +220,23 @@ void Controller::setPlan( } controller_state_.end_phase_enabled = false; controller_state_.end_reached = false; + + return true; } -void Controller::setPlan( +bool Controller::setPlan( const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, const tf2::Transform & tf_base_to_steered_wheel, const geometry_msgs::Twist & /* steering_odom_twist */, const std::vector & global_plan) { - setPlan(current_tf, odom_twist, global_plan); - controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel.getRotation()); - // TODO(clopez) use steering_odom_twist to check if setpoint is being followed + const auto result = setPlan(current_tf, odom_twist, global_plan); + + if (result) { + controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel.getRotation()); + } + + return result; } Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index c625e791..657daa80 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -188,14 +188,18 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector(tfCurPoseStamped_.transform), latest_odom_.twist.twist, - tf2_convert(tf_base_to_steered_wheel_stamped_.transform), steering_odom_twist, - convert_plan(global_plan_map_frame)); + if (!pid_controller_.setPlan( + tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, + tf2_convert(tf_base_to_steered_wheel_stamped_.transform), + steering_odom_twist, convert_plan(global_plan_map_frame))) { + return false; + } } else { - pid_controller_.setPlan( - tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, - convert_plan(global_plan_map_frame)); + if (!pid_controller_.setPlan( + tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, + convert_plan(global_plan_map_frame))) { + return false; + } } pid_controller_.setEnabled(true); From b6495345b922e1e078e9b94c1c3a382a9dd972da Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 2 Mar 2022 08:51:41 +0000 Subject: [PATCH 121/156] Reworked by changing logging to error. --- src/controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller.cpp b/src/controller.cpp index dfd12953..d5e91a42 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -151,7 +151,7 @@ bool Controller::setPlan( ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%zu)", global_plan.size()); if (!check_plan(global_plan)) { - ROS_WARN("Rejected plan because not all poses were in the expected direction of the path!"); + ROS_ERROR("Rejected plan because not all poses were in the expected direction of the path!"); return false; } From 5aad0ec8ec7d4e478bbe10f11bfd462e92e68464 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 1 Mar 2022 09:06:40 +0000 Subject: [PATCH 122/156] Replaced data member with local. --- .../path_tracking_pid/path_tracking_pid_local_planner.hpp | 2 -- src/path_tracking_pid_local_planner.cpp | 7 ++++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 88c03aae..bd8bf9ec 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -120,8 +120,6 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, ros::Duration prev_dt_; path_tracking_pid::Controller pid_controller_; - nav_msgs::Path received_path_; - // Obstacle collision detection costmap_2d::Costmap2DROS * costmap_ = nullptr; diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 657daa80..84a28ec4 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -139,9 +139,10 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector Date: Tue, 1 Mar 2022 09:07:10 +0000 Subject: [PATCH 123/156] Removed NOLINT. --- include/path_tracking_pid/path_tracking_pid_local_planner.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index bd8bf9ec..627b2143 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -66,7 +66,7 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, */ uint32_t computeVelocityCommands( const geometry_msgs::PoseStamped & pose, const geometry_msgs::TwistStamped & velocity, - geometry_msgs::TwistStamped & cmd_vel, std::string & message) override; // NOLINT + geometry_msgs::TwistStamped & cmd_vel, std::string & message) override; /** * @brief Returns true, if the goal is reached. Currently does not respect the parameters given. @@ -93,7 +93,7 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, * @param cmd_vel Output the velocity command * @return true if succeded. */ - bool computeVelocityCommands(geometry_msgs::Twist & cmd_vel); // NOLINT + bool computeVelocityCommands(geometry_msgs::Twist & cmd_vel); /** * @brief Returns true, if the goal is reached. From 16acb5ef433f63c17bd599f192da0317eaa4c575 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 1 Mar 2022 09:33:16 +0000 Subject: [PATCH 124/156] Refactored output parameter. --- .../path_tracking_pid_local_planner.hpp | 5 ++--- src/path_tracking_pid_local_planner.cpp | 17 +++++++++-------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 627b2143..fbc0a63e 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -90,10 +90,9 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, private: /** * @brief Calculates the velocity command based on the current robot pose given by pose. - * @param cmd_vel Output the velocity command - * @return true if succeded. + * @return Velocity command on success, empty optional otherwise. */ - bool computeVelocityCommands(geometry_msgs::Twist & cmd_vel); + std::optional computeVelocityCommands(); /** * @brief Returns true, if the goal is reached. diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 84a28ec4..d79b1c04 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -209,7 +209,7 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector TrackingPidLocalPlanner::computeVelocityCommands() { ros::Time now = ros::Time::now(); if (prev_time_.isZero()) { @@ -219,14 +219,14 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd if (dt.isZero()) { ROS_ERROR_THROTTLE( 5, "dt=0 detected, skipping loop(s). Possible overloaded cpu or simulating too fast"); - cmd_vel = geometry_msgs::Twist(); + auto cmd_vel = geometry_msgs::Twist(); cmd_vel.linear.x = pid_controller_.getControllerState().current_x_vel; cmd_vel.angular.z = pid_controller_.getControllerState().current_yaw_vel; - return true; // False is no use: https://github.com/magazino/move_base_flex/issues/195 + return cmd_vel; } if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) { ROS_ERROR("Invalid time increment: %f. Aborting", dt.toSec()); - return false; + return std::nullopt; } try { ROS_DEBUG("map_frame: %s, base_link_frame: %s", map_frame_.c_str(), base_link_frame_.c_str()); @@ -234,7 +234,7 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd } catch (const tf2::TransformException & ex) { ROS_ERROR("Received an exception trying to transform: %s", ex.what()); active_goal_ = false; - return false; + return std::nullopt; } // Handle obstacles @@ -258,7 +258,6 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd const auto update_result = pid_controller_.update_with_limits( tf2_convert(tfCurPoseStamped_.transform), latest_odom_.twist.twist, dt); - cmd_vel = update_result.velocity_command; path_tracking_pid::PidFeedback feedback_msg; feedback_msg.eda = ros::Duration(update_result.eda); @@ -296,7 +295,7 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd prev_time_ = now; prev_dt_ = dt; // Store last known valid dt for next cycles (https://github.com/magazino/move_base_flex/issues/195) - return true; + return update_result.velocity_command; } uint8_t TrackingPidLocalPlanner::projectedCollisionCost() @@ -441,10 +440,12 @@ uint32_t TrackingPidLocalPlanner::computeVelocityCommands( return mbf_msgs::ExePathResult::NOT_INITIALIZED; } // TODO(Cesar): Use provided pose and odom - if (!computeVelocityCommands(cmd_vel.twist)) { + const auto opt_cmd_vel = computeVelocityCommands(); + if (!opt_cmd_vel) { active_goal_ = false; return mbf_msgs::ExePathResult::FAILURE; } + cmd_vel.twist = *opt_cmd_vel; cmd_vel.header.stamp = ros::Time::now(); cmd_vel.header.frame_id = base_link_frame_; From c615c89eaf31f8c41618a0e18c25da006fdad9ad Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 3 Mar 2022 13:43:18 +0000 Subject: [PATCH 125/156] Reworked by adding comment. --- src/path_tracking_pid_local_planner.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index d79b1c04..556912ec 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -222,6 +222,9 @@ std::optional TrackingPidLocalPlanner::computeVelocityComm auto cmd_vel = geometry_msgs::Twist(); cmd_vel.linear.x = pid_controller_.getControllerState().current_x_vel; cmd_vel.angular.z = pid_controller_.getControllerState().current_yaw_vel; + // At the first call of computeVelocityCommands() we can't calculate a cmd_vel. We can't return + // false because of https://github.com/magazino/move_base_flex/issues/195 so the current + // velocity is send instead. return cmd_vel; } if (dt < ros::Duration(0) || dt > ros::Duration(DT_MAX)) { From ed9caa39d53431ba4b141c694b25804c44efb9d4 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Fri, 4 Mar 2022 13:58:46 +0100 Subject: [PATCH 126/156] Refactor to use color names --- src/visualization.cpp | 54 ++++++++++++++++++++++--------------------- 1 file changed, 28 insertions(+), 26 deletions(-) diff --git a/src/visualization.cpp b/src/visualization.cpp index 12bafc0a..3708007c 100644 --- a/src/visualization.cpp +++ b/src/visualization.cpp @@ -10,6 +10,24 @@ namespace path_tracking_pid { +// Factory function for a visualization color from the given arguments. (Because the corresponding +// type has no appropriate constructor.) +std_msgs::ColorRGBA create_color(float r, float g, float b, float a = 1) +{ + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +const auto red = create_color(1, 0, 0); +const auto green = create_color(0, 1, 0); +const auto blue = create_color(1, 0, 1); +const auto yellow = create_color(1, 1, 0); +const auto orange = create_color(1, 0.5, 0); + std::vector to_msg(std::vector points) { std::vector msgs; @@ -29,35 +47,24 @@ Visualization::Visualization(ros::NodeHandle nh) void Visualization::publishControlPoint( const std_msgs::Header & header, const tf2::Transform & pose) { - std_msgs::ColorRGBA color; - color.a = 1.0; - color.g = 1.0; - publishSphere(header, "control point", pose, color); + publishSphere(header, "control point", pose, green); } void Visualization::publishAxlePoint(const std_msgs::Header & header, const tf2::Transform & pose) { std_msgs::ColorRGBA color; - color.a = 1.0; - color.b = 1.0; - publishSphere(header, "axle point", pose, color); + publishSphere(header, "axle point", pose, blue); } void Visualization::publishGoalPoint(const std_msgs::Header & header, const tf2::Transform & pose) { std_msgs::ColorRGBA color; - color.a = 1.0; - color.r = 1.0; - publishSphere(header, "goal point", pose, color); + publishSphere(header, "goal point", pose, red); } void Visualization::publishPlanPoint(const std_msgs::Header & header, const tf2::Transform & pose) { - std_msgs::ColorRGBA color; - color.a = 1.0; - color.g = 0.5; - color.r = 1.0; - publishSphere(header, "plan point", pose, color); + publishSphere(header, "plan point", pose, orange); } void Visualization::publishCollisionObject( @@ -70,10 +77,9 @@ void Visualization::publishCollisionObject( marker.type = visualization_msgs::Marker::CYLINDER; marker.scale.x = 0.5; marker.scale.y = 0.5; - marker.color.r = 1.0; - marker.color.a = 0.0; - marker.scale.z = cost / 255.0; + marker.color = red; marker.color.a = cost / 255.0; + marker.scale.z = cost / 255.0; tf2::toMsg(point, marker.pose.position); marker.pose.position.z = marker.scale.z * 0.5; if (marker.scale.z > std::numeric_limits::epsilon()) { @@ -96,9 +102,7 @@ void Visualization::publishExtrapolatedPoses( marker.type = visualization_msgs::Marker::POINTS; marker.scale.x = 0.5; marker.scale.y = 0.5; - marker.color.r = 1.0; - marker.color.g = 0.5; - marker.color.a = 1.0; + marker.color = orange; marker.points = to_msg(steps); marker_pub_.publish(marker); } @@ -114,9 +118,7 @@ void Visualization::publishgGoalPosesOnPath( marker.type = visualization_msgs::Marker::POINTS; marker.scale.x = 0.5; marker.scale.y = 0.5; - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.a = 1.0; + marker.color = yellow; marker.points = to_msg(path); marker_pub_.publish(marker); } @@ -131,7 +133,7 @@ void Visualization::publishCollisionFootprint( marker.pose.orientation.w = 1.0; marker.type = visualization_msgs::Marker::LINE_LIST; marker.scale.x = 0.1; - marker.color.b = 1.0; + marker.color = blue; marker.color.a = 0.3; marker.points = to_msg(footprint); marker_pub_.publish(marker); @@ -147,7 +149,7 @@ void Visualization::publishCollisionPolygon( marker.pose.orientation.w = 1.0; marker.type = visualization_msgs::Marker::LINE_STRIP; marker.scale.x = 0.2; - marker.color.r = 1.0; + marker.color = red; marker.color.a = 0.3; marker.points = to_msg(hull); marker_pub_.publish(marker); From 6ceb9532b238fc7d016877d4f2d51ce53e752d6a Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 8 Mar 2022 08:53:16 +0000 Subject: [PATCH 127/156] Moved distSquared and added tests. --- src/calculations.cpp | 5 +++++ src/calculations.hpp | 9 +++++++++ src/controller.cpp | 6 ------ test/unittests/test_calculations.cpp | 8 ++++++++ 4 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/calculations.cpp b/src/calculations.cpp index fa7da4f6..b622bc07 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -71,4 +71,9 @@ bool is_in_direction_of_target( return !std::signbit(projection * velocity); } +double distSquared(const tf2::Transform & a, const tf2::Transform & b) +{ + return a.getOrigin().distance2(b.getOrigin()); +} + } // namespace path_tracking_pid diff --git a/src/calculations.hpp b/src/calculations.hpp index ddb31176..0a579c58 100644 --- a/src/calculations.hpp +++ b/src/calculations.hpp @@ -46,4 +46,13 @@ std::vector inverse_turning_radiuses(const std::vector & bool is_in_direction_of_target( const tf2::Transform & current, const tf2::Vector3 & target, double velocity); +/** + * Returns the square distance between two poses. + * + * @param[in] a Pose A. + * @param[in] b Pose B. + * @return Distance between A and B. + */ +double distSquared(const tf2::Transform & a, const tf2::Transform & b); + } // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index d5e91a42..ffe96369 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -30,12 +30,6 @@ constexpr double ang_lower_limit = -100.0; // Anti-windup term. Limits the absolute value of the integral term. constexpr double windup_limit = 1000.0; -// Returns the square distance between two points -double distSquared(const tf2::Transform & a, const tf2::Transform & b) -{ - return a.getOrigin().distance2(b.getOrigin()); -} - // Indicates if the angle of the cur pose is obtuse (with respect to the prev and next poses). bool is_pose_angle_obtuse( const tf2::Transform & prev, const tf2::Transform & cur, const tf2::Transform & next) diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index ab483b5e..441f849b 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -10,6 +10,7 @@ namespace using path_tracking_pid::deltas_of_plan; using path_tracking_pid::distances_to_goal; +using path_tracking_pid::distSquared; using path_tracking_pid::inverse_turning_radiuses; using path_tracking_pid::is_in_direction_of_target; @@ -190,4 +191,11 @@ INSTANTIATE_TEST_CASE_P( ::testing::ValuesIn(is_in_direction_of_target_params), [](const auto & info) { return std::to_string(info.index); }); +TEST(PathTrackingPidCalculations, DistSquared) +{ + EXPECT_EQ(14, distSquared(create_transform(1, 2, 3), create_transform(2, 4, 6))); + EXPECT_EQ(45, distSquared(create_transform(6, 2, 0), create_transform(2, 4, -5))); + EXPECT_EQ(0, distSquared(create_transform(1, 2, 3), create_transform(1, 2, 3))); +} + } // namespace From b9067ef2af89ae5c200db22328e95c7636e3a830 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 8 Mar 2022 09:30:50 +0000 Subject: [PATCH 128/156] Made distToSegmentSquared static. --- include/path_tracking_pid/controller.hpp | 8 ++++--- src/controller.cpp | 28 +++++++++++++----------- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index f02c2a69..ac990693 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -208,12 +208,14 @@ class Controller : private boost::noncopyable * @param[in] pose_p Start of the line segment * @param[in] pose_v End of the line segment * @param[in] pose_w The point + * @param[in] estimate_pose_angle Indicates if the pose angle should be estimated from the line + * segment (true) or if the pose angle from pose_v should be used. * @return The pose projection of the closest point with the distance (squared) to pose_p and * the distance to pose_w. */ - DistToSegmentSquaredResult distToSegmentSquared( - const tf2::Transform & pose_p, const tf2::Transform & pose_v, - const tf2::Transform & pose_w) const; + static DistToSegmentSquaredResult distToSegmentSquared( + const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, + bool estimate_pose_angle); geometry_msgs::Twist computeTricycleModelForwardKinematics(double x_vel, double steering_angle); TricycleSteeringCmdVel computeTricycleModelInverseKinematics( diff --git a/src/controller.cpp b/src/controller.cpp index ffe96369..14aee1dc 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -169,9 +169,10 @@ bool Controller::setPlan( // Hence, when picking the starting path's pose, we mean to start at the segment connecting that and the next pose. for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ - const auto dist_to_segment = - distToSegmentSquared(current_tf, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1]) - .distance2_to_p; + const auto dist_to_segment = distToSegmentSquared( + current_tf, global_plan_tf_[idx_path], + global_plan_tf_[idx_path + 1], estimate_pose_angle_enabled_) + .distance2_to_p; // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! if (dist_to_segment < minimum_distance_to_path) { @@ -234,7 +235,8 @@ bool Controller::setPlan( } Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( - const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w) const + const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, + bool estimate_pose_angle) { const double l2 = distSquared(pose_v, pose_w); if (l2 == 0) { @@ -260,10 +262,10 @@ Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()), pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); - const auto yaw = estimate_pose_angle_enabled_ ? atan2( - pose_w.getOrigin().y() - pose_v.getOrigin().y(), - pose_w.getOrigin().x() - pose_v.getOrigin().x()) - : tf2::getYaw(pose_v.getRotation()); + const auto yaw = estimate_pose_angle ? atan2( + pose_w.getOrigin().y() - pose_v.getOrigin().y(), + pose_w.getOrigin().x() - pose_v.getOrigin().x()) + : tf2::getYaw(pose_v.getRotation()); tf2::Quaternion pose_quaternion; pose_quaternion.setRPY(0.0, 0.0, yaw); result.pose_projection.setRotation(pose_quaternion); @@ -330,8 +332,8 @@ tf2::Transform Controller::findPositionOnPlan( // the closest line segment to the current carrot if (controller_state.current_global_plan_index == 0) { - const auto dist_result = - distToSegmentSquared(current_tf2, global_plan_tf_[0], global_plan_tf_[1]); + const auto dist_result = distToSegmentSquared( + current_tf2, global_plan_tf_[0], global_plan_tf_[1], estimate_pose_angle_enabled_); distance_to_goal_ = distance_to_goal_vector_[1] + dist_result.distance_to_w; controller_state.last_visited_pose_index = 0; @@ -343,7 +345,7 @@ tf2::Transform Controller::findPositionOnPlan( if (controller_state.current_global_plan_index == global_plan_tf_.size() - 1) { const auto dist_result = distToSegmentSquared( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], - global_plan_tf_[controller_state.current_global_plan_index]); + global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_enabled_); distance_to_goal_ = dist_result.distance_to_w; controller_state.last_visited_pose_index = global_plan_tf_.size() - 2; @@ -354,10 +356,10 @@ tf2::Transform Controller::findPositionOnPlan( const auto dist_result_ahead = distToSegmentSquared( current_tf2, global_plan_tf_[controller_state.current_global_plan_index], - global_plan_tf_[controller_state.current_global_plan_index + 1]); + global_plan_tf_[controller_state.current_global_plan_index + 1], estimate_pose_angle_enabled_); const auto dist_result_behind = distToSegmentSquared( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], - global_plan_tf_[controller_state.current_global_plan_index]); + global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_enabled_); if (dist_result_ahead.distance2_to_p < dist_result_behind.distance2_to_p) { distance_to_goal_ = distance_to_goal_vector_[controller_state.current_global_plan_index + 1] + From 8dde3a8cf312914783851d642fb8ac351cdb310f Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Tue, 15 Feb 2022 13:52:44 +0100 Subject: [PATCH 129/156] Clarify init_vel_max_diff check --- README.md | 2 +- cfg/Pid.cfg | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 6bf07a7a..44e618f2 100644 --- a/README.md +++ b/README.md @@ -102,7 +102,7 @@ Tracking_pid parameters are all available through (rqt_)dynamic_reconfigure. The * **`l`** (double, default: `0.5`) Following distance from robot's rotational point to trajectory. * **`track_base_link`** (bool, default: `false`) Whether to track the path using the base_link instead of the control point ahead. A smooth path is needed. * **`init_vel_method`** (enum, default: `1`) Method to choose initial velocity on new paths. -* **`init_vel_max_diff`** (double, default: `0.5`) How much velocity difference is acceptable upon starting a new path. If internal state and new path's velocity differ more than this, the path will be rejected. Set to -1 to ignore this check. +* **`init_vel_max_diff`** (double, default: `0.5`) How much velocity difference is acceptable upon starting a new path. If internal state and new path's velocity differ more than this, the path will be rejected. Set to -1 to ignore this check. This check is only active if 'init_vel_method'==InternalSetpoint. Proportional, Integral and Derivative actions for the two closed loops: Lateral and angular loops. diff --git a/cfg/Pid.cfg b/cfg/Pid.cfg index cc0135e6..69c031cb 100755 --- a/cfg/Pid.cfg +++ b/cfg/Pid.cfg @@ -19,11 +19,11 @@ gen.add("min_turning_radius", double_t, 0, "Minimum turning radius", 0.0, 0.0, 1 gen.add("track_base_link", bool_t, 0, "Should the controller track the path using the base_link frame?", False) gen.add("init_vel_method", int_t, 0, "Initial velocity method", 1, 0, 3, edit_method=gen.enum([ - gen.const("Zero", int_t, 0, "Always start from zero"), - gen.const("InternalState", int_t, 1, "Last internal state is new initial state"), - gen.const("Odom", int_t, 2, "Start from current odometry value") + gen.const("Zero", int_t, 0, "Always start from zero"), + gen.const("InternalSetpoint", int_t, 1, "Last internal setpoint is new initial setpoint"), + gen.const("Odom", int_t, 2, "Start from current odometry value") ], "Initial velocity method enum")) -gen.add("init_vel_max_diff", double_t, 0, "Maximum vel-diff allowed when starting a path (-1 to ignore)", 0.5, -1, 10) +gen.add("init_vel_max_diff", double_t, 0, "Maximum vel-diff allowed when starting a path (-1 to ignore, only active upon 'init_vel_method'==InternalSetpoint)", 0.5, -1, 10) gen.add("Kp_lat", double_t, 0, "Kp Lateral", 1, 0, 10) gen.add("Ki_lat", double_t, 0, "Ki Lateral", 0, 0, 2) From dac1ff06b192ca0903c9458b91d3c08d95aa3b31 Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Wed, 9 Mar 2022 13:55:01 +0100 Subject: [PATCH 130/156] Rokus comments Co-authored-by: Rokus Ottervanger --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 44e618f2..91c7e1ae 100644 --- a/README.md +++ b/README.md @@ -102,7 +102,7 @@ Tracking_pid parameters are all available through (rqt_)dynamic_reconfigure. The * **`l`** (double, default: `0.5`) Following distance from robot's rotational point to trajectory. * **`track_base_link`** (bool, default: `false`) Whether to track the path using the base_link instead of the control point ahead. A smooth path is needed. * **`init_vel_method`** (enum, default: `1`) Method to choose initial velocity on new paths. -* **`init_vel_max_diff`** (double, default: `0.5`) How much velocity difference is acceptable upon starting a new path. If internal state and new path's velocity differ more than this, the path will be rejected. Set to -1 to ignore this check. This check is only active if 'init_vel_method'==InternalSetpoint. +* **`init_vel_max_diff`** (double, default: `0.5`) How much velocity difference is acceptable upon starting a new path. If internal state and current odometry's velocity differ more than this, the path will be aborted. Set to -1 to ignore this check. This check is only active if 'init_vel_method'==InternalSetpoint. Proportional, Integral and Derivative actions for the two closed loops: Lateral and angular loops. From c13602fda8a5fbd4993c54bf58e557410212f680 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 14 Mar 2022 08:05:33 +0000 Subject: [PATCH 131/156] Reworked by renaming data member. --- include/path_tracking_pid/controller.hpp | 2 +- src/controller.cpp | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index ac990693..f13407e0 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -245,7 +245,7 @@ class Controller : private boost::noncopyable bool enabled_ = true; bool holonomic_robot_enable_ = false; - bool estimate_pose_angle_enabled_ = false; + bool estimate_pose_angle_ = false; // feedforward controller double feedforward_lat_ = 0.0; diff --git a/src/controller.cpp b/src/controller.cpp index 14aee1dc..549ed272 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -72,7 +72,7 @@ void Controller::setHolonomic(bool holonomic) void Controller::setEstimatePoseAngle(bool estimate_pose_angle) { // Set configuration parameters - estimate_pose_angle_enabled_ = estimate_pose_angle; + estimate_pose_angle_ = estimate_pose_angle; } void Controller::setTricycleModel( @@ -169,10 +169,10 @@ bool Controller::setPlan( // Hence, when picking the starting path's pose, we mean to start at the segment connecting that and the next pose. for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ - const auto dist_to_segment = distToSegmentSquared( - current_tf, global_plan_tf_[idx_path], - global_plan_tf_[idx_path + 1], estimate_pose_angle_enabled_) - .distance2_to_p; + const auto dist_to_segment = + distToSegmentSquared( + current_tf, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1], estimate_pose_angle_) + .distance2_to_p; // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! if (dist_to_segment < minimum_distance_to_path) { @@ -333,7 +333,7 @@ tf2::Transform Controller::findPositionOnPlan( if (controller_state.current_global_plan_index == 0) { const auto dist_result = distToSegmentSquared( - current_tf2, global_plan_tf_[0], global_plan_tf_[1], estimate_pose_angle_enabled_); + current_tf2, global_plan_tf_[0], global_plan_tf_[1], estimate_pose_angle_); distance_to_goal_ = distance_to_goal_vector_[1] + dist_result.distance_to_w; controller_state.last_visited_pose_index = 0; @@ -345,7 +345,7 @@ tf2::Transform Controller::findPositionOnPlan( if (controller_state.current_global_plan_index == global_plan_tf_.size() - 1) { const auto dist_result = distToSegmentSquared( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], - global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_enabled_); + global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); distance_to_goal_ = dist_result.distance_to_w; controller_state.last_visited_pose_index = global_plan_tf_.size() - 2; @@ -356,10 +356,10 @@ tf2::Transform Controller::findPositionOnPlan( const auto dist_result_ahead = distToSegmentSquared( current_tf2, global_plan_tf_[controller_state.current_global_plan_index], - global_plan_tf_[controller_state.current_global_plan_index + 1], estimate_pose_angle_enabled_); + global_plan_tf_[controller_state.current_global_plan_index + 1], estimate_pose_angle_); const auto dist_result_behind = distToSegmentSquared( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], - global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_enabled_); + global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); if (dist_result_ahead.distance2_to_p < dist_result_behind.distance2_to_p) { distance_to_goal_ = distance_to_goal_vector_[controller_state.current_global_plan_index + 1] + From 9d12eda1869de42ee6dc2007a0f2b733fdba38a0 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 8 Mar 2022 10:24:30 +0000 Subject: [PATCH 132/156] Refactor name and output of distToSegmentSquared. --- include/path_tracking_pid/controller.hpp | 13 +---- src/controller.cpp | 65 ++++++++++++------------ 2 files changed, 34 insertions(+), 44 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index f13407e0..df74fa0c 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -193,14 +193,6 @@ class Controller : private boost::noncopyable double getVelMaxObstacle() const; private: - // Result of distToSegmentSquared(). - struct DistToSegmentSquaredResult - { - tf2::Transform pose_projection; - double distance2_to_p = 0; // Square distance in meter squared. - double distance_to_w = 0; // Distance in meter. - }; - /** * @brief Closest point between a line segment and a point * Calculate the closest point between the line segment bounded by PV and the point W. @@ -210,10 +202,9 @@ class Controller : private boost::noncopyable * @param[in] pose_w The point * @param[in] estimate_pose_angle Indicates if the pose angle should be estimated from the line * segment (true) or if the pose angle from pose_v should be used. - * @return The pose projection of the closest point with the distance (squared) to pose_p and - * the distance to pose_w. + * @return The pose projection of the closest point. */ - static DistToSegmentSquaredResult distToSegmentSquared( + static tf2::Transform closestPointOnSegment( const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, bool estimate_pose_angle); diff --git a/src/controller.cpp b/src/controller.cpp index 549ed272..738872dc 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -169,10 +169,10 @@ bool Controller::setPlan( // Hence, when picking the starting path's pose, we mean to start at the segment connecting that and the next pose. for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ - const auto dist_to_segment = - distToSegmentSquared( - current_tf, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1], estimate_pose_angle_) - .distance2_to_p; + const auto dist_to_segment = distSquared( + current_tf, closestPointOnSegment( + current_tf, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1], + estimate_pose_angle_)); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! if (dist_to_segment < minimum_distance_to_path) { @@ -234,22 +234,16 @@ bool Controller::setPlan( return result; } -Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( +tf2::Transform Controller::closestPointOnSegment( const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, bool estimate_pose_angle) { const double l2 = distSquared(pose_v, pose_w); if (l2 == 0) { - DistToSegmentSquaredResult result; - - result.pose_projection = pose_w; - result.distance_to_w = 0.0; - result.distance2_to_p = distSquared(pose_p, pose_w); - - return result; + return pose_w; } - DistToSegmentSquaredResult result; + tf2::Transform result; const double t = std::clamp( ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) * @@ -258,7 +252,7 @@ Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( (pose_w.getOrigin().y() - pose_v.getOrigin().y())) / l2, 0.0, 1.0); - result.pose_projection.setOrigin(tf2::Vector3( + result.setOrigin(tf2::Vector3( pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()), pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); @@ -268,10 +262,7 @@ Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared( : tf2::getYaw(pose_v.getRotation()); tf2::Quaternion pose_quaternion; pose_quaternion.setRPY(0.0, 0.0, yaw); - result.pose_projection.setRotation(pose_quaternion); - - result.distance_to_w = sqrt(distSquared(result.pose_projection, pose_w)); - result.distance2_to_p = distSquared(pose_p, result.pose_projection); + result.setRotation(pose_quaternion); return result; } @@ -332,50 +323,58 @@ tf2::Transform Controller::findPositionOnPlan( // the closest line segment to the current carrot if (controller_state.current_global_plan_index == 0) { - const auto dist_result = distToSegmentSquared( + const auto closest_point = closestPointOnSegment( current_tf2, global_plan_tf_[0], global_plan_tf_[1], estimate_pose_angle_); - distance_to_goal_ = distance_to_goal_vector_[1] + dist_result.distance_to_w; + distance_to_goal_ = + distance_to_goal_vector_[1] + sqrt(distSquared(global_plan_tf_[1], closest_point)); controller_state.last_visited_pose_index = 0; path_pose_idx = controller_state.current_global_plan_index; - return dist_result.pose_projection; + return closest_point; } if (controller_state.current_global_plan_index == global_plan_tf_.size() - 1) { - const auto dist_result = distToSegmentSquared( + const auto closest_point = closestPointOnSegment( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); - distance_to_goal_ = dist_result.distance_to_w; + distance_to_goal_ = + sqrt(distSquared(global_plan_tf_[controller_state.current_global_plan_index], closest_point)); controller_state.last_visited_pose_index = global_plan_tf_.size() - 2; path_pose_idx = controller_state.current_global_plan_index - 1; - return dist_result.pose_projection; + return closest_point; } - const auto dist_result_ahead = distToSegmentSquared( + const auto closest_point_ahead = closestPointOnSegment( current_tf2, global_plan_tf_[controller_state.current_global_plan_index], global_plan_tf_[controller_state.current_global_plan_index + 1], estimate_pose_angle_); - const auto dist_result_behind = distToSegmentSquared( + const auto closest_point_behind = closestPointOnSegment( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); - if (dist_result_ahead.distance2_to_p < dist_result_behind.distance2_to_p) { - distance_to_goal_ = distance_to_goal_vector_[controller_state.current_global_plan_index + 1] + - dist_result_ahead.distance_to_w; + if ( + distSquared(current_tf2, closest_point_ahead) < + distSquared(current_tf2, closest_point_behind)) { + distance_to_goal_ = + distance_to_goal_vector_[controller_state.current_global_plan_index + 1] + + sqrt(distSquared( + global_plan_tf_[controller_state.current_global_plan_index + 1], closest_point_ahead)); controller_state.last_visited_pose_index = controller_state.current_global_plan_index; path_pose_idx = controller_state.current_global_plan_index; - return dist_result_ahead.pose_projection; + return closest_point_ahead; } - distance_to_goal_ = distance_to_goal_vector_[controller_state.current_global_plan_index] + - dist_result_behind.distance_to_w; + distance_to_goal_ = + distance_to_goal_vector_[controller_state.current_global_plan_index] + + sqrt(distSquared( + global_plan_tf_[controller_state.current_global_plan_index], closest_point_behind)); controller_state.last_visited_pose_index = controller_state.current_global_plan_index - 1; path_pose_idx = controller_state.current_global_plan_index; - return dist_result_behind.pose_projection; + return closest_point_behind; } Controller::UpdateResult Controller::update( From d196881507103479cd99b4205367b7db236f368e Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 10 Mar 2022 15:47:43 +0000 Subject: [PATCH 133/156] Move, refactor and test closestPointOnSegment(). --- include/path_tracking_pid/controller.hpp | 15 ---- src/calculations.cpp | 39 +++++++++ src/calculations.hpp | 18 ++++ src/controller.cpp | 33 ------- test/unittests/test_calculations.cpp | 104 +++++++++++++++++++++++ 5 files changed, 161 insertions(+), 48 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index df74fa0c..b36312cd 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -193,21 +193,6 @@ class Controller : private boost::noncopyable double getVelMaxObstacle() const; private: - /** - * @brief Closest point between a line segment and a point - * Calculate the closest point between the line segment bounded by PV and the point W. - * - * @param[in] pose_p Start of the line segment - * @param[in] pose_v End of the line segment - * @param[in] pose_w The point - * @param[in] estimate_pose_angle Indicates if the pose angle should be estimated from the line - * segment (true) or if the pose angle from pose_v should be used. - * @return The pose projection of the closest point. - */ - static tf2::Transform closestPointOnSegment( - const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, - bool estimate_pose_angle); - geometry_msgs::Twist computeTricycleModelForwardKinematics(double x_vel, double steering_angle); TricycleSteeringCmdVel computeTricycleModelInverseKinematics( const geometry_msgs::Twist & cmd_vel); diff --git a/src/calculations.cpp b/src/calculations.cpp index b622bc07..037ac0be 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -1,5 +1,7 @@ #include "calculations.hpp" +#include + #include #include #include @@ -76,4 +78,41 @@ double distSquared(const tf2::Transform & a, const tf2::Transform & b) return a.getOrigin().distance2(b.getOrigin()); } +tf2::Transform closestPointOnSegment( + const tf2::Transform & point, const tf2::Transform & segment_start, + const tf2::Transform & segment_end, bool estimate_pose_angle) +{ + const double l2 = distSquared(segment_start, segment_end); + if (l2 == 0) { + return segment_end; + } + + tf2::Transform result; + + const double t = std::clamp( + ((point.getOrigin().x() - segment_start.getOrigin().x()) * + (segment_end.getOrigin().x() - segment_start.getOrigin().x()) + + (point.getOrigin().y() - segment_start.getOrigin().y()) * + (segment_end.getOrigin().y() - segment_start.getOrigin().y())) / + l2, + 0.0, 1.0); + result.setOrigin(tf2::Vector3( + segment_start.getOrigin().x() + + t * (segment_end.getOrigin().x() - segment_start.getOrigin().x()), + segment_start.getOrigin().y() + + t * (segment_end.getOrigin().y() - segment_start.getOrigin().y()), + 0.0)); + + const auto yaw = estimate_pose_angle + ? atan2( + segment_end.getOrigin().y() - segment_start.getOrigin().y(), + segment_end.getOrigin().x() - segment_start.getOrigin().x()) + : tf2::getYaw(segment_start.getRotation()); + tf2::Quaternion pose_quaternion; + pose_quaternion.setRPY(0.0, 0.0, yaw); + result.setRotation(pose_quaternion); + + return result; +} + } // namespace path_tracking_pid diff --git a/src/calculations.hpp b/src/calculations.hpp index 0a579c58..8c4c3d55 100644 --- a/src/calculations.hpp +++ b/src/calculations.hpp @@ -55,4 +55,22 @@ bool is_in_direction_of_target( */ double distSquared(const tf2::Transform & a, const tf2::Transform & b); +/** + * @brief Closest point between a line segment and a point + * + * Calculate the closest point between the line segment bounded by `segment_start` and `segment_end` + * and point `point`. + * + * @param[in] point The point + * @param[in] segment_start Start of the line segment + * @param[in] segment_end End of the line segment + * @param[in] estimate_pose_angle Indicates if the pose angle should be estimated from the line + * segment (true) or if the pose angle from segment_start should be + * used. + * @return The pose projection of the closest point. + */ +tf2::Transform closestPointOnSegment( + const tf2::Transform & point, const tf2::Transform & segment_start, + const tf2::Transform & segment_end, bool estimate_pose_angle); + } // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index 738872dc..c0efcd1f 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -234,39 +234,6 @@ bool Controller::setPlan( return result; } -tf2::Transform Controller::closestPointOnSegment( - const tf2::Transform & pose_p, const tf2::Transform & pose_v, const tf2::Transform & pose_w, - bool estimate_pose_angle) -{ - const double l2 = distSquared(pose_v, pose_w); - if (l2 == 0) { - return pose_w; - } - - tf2::Transform result; - - const double t = std::clamp( - ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) * - (pose_w.getOrigin().x() - pose_v.getOrigin().x()) + - (pose_p.getOrigin().y() - pose_v.getOrigin().y()) * - (pose_w.getOrigin().y() - pose_v.getOrigin().y())) / - l2, - 0.0, 1.0); - result.setOrigin(tf2::Vector3( - pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()), - pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); - - const auto yaw = estimate_pose_angle ? atan2( - pose_w.getOrigin().y() - pose_v.getOrigin().y(), - pose_w.getOrigin().x() - pose_v.getOrigin().x()) - : tf2::getYaw(pose_v.getRotation()); - tf2::Quaternion pose_quaternion; - pose_quaternion.setRPY(0.0, 0.0, yaw); - result.setRotation(pose_quaternion); - - return result; -} - tf2::Transform Controller::findPositionOnPlan( const tf2::Transform & current_tf, ControllerState & controller_state, size_t & path_pose_idx) { diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index 441f849b..bf9cd2a3 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -8,6 +9,7 @@ namespace { +using path_tracking_pid::closestPointOnSegment; using path_tracking_pid::deltas_of_plan; using path_tracking_pid::distances_to_goal; using path_tracking_pid::distSquared; @@ -23,6 +25,14 @@ tf2::Transform create_transform(double x, double y, double z) return result; } +// Create a quaternion based on the given roll, pitch and yaw. +tf2::Quaternion create_quaternion(double roll, double pitch, double yaw) +{ + tf2::Quaternion result; + result.setRPY(roll, pitch, yaw); + return result; +} + TEST(PathTrackingPidCalculations, DeltasOfPlan_Empty) { const auto plan = std::vector{}; @@ -198,4 +208,98 @@ TEST(PathTrackingPidCalculations, DistSquared) EXPECT_EQ(0, distSquared(create_transform(1, 2, 3), create_transform(1, 2, 3))); } +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_AtEnd) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = end; + const auto ref = end; + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_AtStart) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = start; + const auto ref = start; + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_CloseToEnd) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(7, 5, 0); + const auto ref = end; + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_CloseToStart) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(-7, -5, 0); + const auto ref = start; + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_Halfway) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(2, 4, 0); + const auto ref = create_transform(3, 3, 0); + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_TwoThirds) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(8, 5, 0); + const auto point = create_transform(2, 12, 0); + const auto ref = create_transform(6, 4, 0); + + EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_OtherYaw) +{ + const auto start = tf2::Transform(create_quaternion(1, 1, 1), {2, 2, 0}); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(2, 4, 0); + const auto ref = tf2::Transform(create_quaternion(0, 0, 1), {3, 3, 0}); + const auto result = closestPointOnSegment(point, start, end, false); + + EXPECT_EQ(ref.getOrigin(), result.getOrigin()); + // allow for small differences in the basis because of rounding errors in the calculations + for (auto r = 0; r < 3; ++r) { + for (auto c = 0; c < 3; ++c) { + EXPECT_NEAR(ref.getBasis()[r][c], result.getBasis()[r][c], 1e-6); + } + } +} + +TEST(PathTrackingPidCalculations, ClosestPointOnSegment_EstimateAngle) +{ + const auto start = create_transform(2, 2, 0); + const auto end = create_transform(4, 4, 0); + const auto point = create_transform(2, 4, 0); + const auto ref = tf2::Transform(create_quaternion(0, 0, M_PI / 4.0), {3, 3, 0}); + const auto result = closestPointOnSegment(point, start, end, true); + + EXPECT_EQ(ref.getOrigin(), result.getOrigin()); + // allow for small differences in the basis because of rounding errors in the calculations + for (auto r = 0; r < 3; ++r) { + for (auto c = 0; c < 3; ++c) { + EXPECT_NEAR(ref.getBasis()[r][c], result.getBasis()[r][c], 1e-6); + } + } +} + } // namespace From 8ab5a256581edc4ef00fd5103a57abbca5be0c37 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Mon, 14 Mar 2022 08:17:11 +0000 Subject: [PATCH 134/156] Reworked by renaming function. --- src/calculations.cpp | 2 +- src/calculations.hpp | 6 ++--- src/controller.cpp | 29 ++++++++++++------------ test/unittests/test_calculations.cpp | 34 ++++++++++++++-------------- 4 files changed, 35 insertions(+), 36 deletions(-) diff --git a/src/calculations.cpp b/src/calculations.cpp index 037ac0be..9aa369b1 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -78,7 +78,7 @@ double distSquared(const tf2::Transform & a, const tf2::Transform & b) return a.getOrigin().distance2(b.getOrigin()); } -tf2::Transform closestPointOnSegment( +tf2::Transform closestPoseOnSegment( const tf2::Transform & point, const tf2::Transform & segment_start, const tf2::Transform & segment_end, bool estimate_pose_angle) { diff --git a/src/calculations.hpp b/src/calculations.hpp index 8c4c3d55..4d1ced22 100644 --- a/src/calculations.hpp +++ b/src/calculations.hpp @@ -56,9 +56,9 @@ bool is_in_direction_of_target( double distSquared(const tf2::Transform & a, const tf2::Transform & b); /** - * @brief Closest point between a line segment and a point + * @brief Closest pose between a line segment and a point * - * Calculate the closest point between the line segment bounded by `segment_start` and `segment_end` + * Calculate the closest pose between the line segment bounded by `segment_start` and `segment_end` * and point `point`. * * @param[in] point The point @@ -69,7 +69,7 @@ double distSquared(const tf2::Transform & a, const tf2::Transform & b); * used. * @return The pose projection of the closest point. */ -tf2::Transform closestPointOnSegment( +tf2::Transform closestPoseOnSegment( const tf2::Transform & point, const tf2::Transform & segment_start, const tf2::Transform & segment_end, bool estimate_pose_angle); diff --git a/src/controller.cpp b/src/controller.cpp index c0efcd1f..f94396a5 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -170,7 +170,7 @@ bool Controller::setPlan( for (int idx_path = static_cast(global_plan_tf_.size() - 2); idx_path >= 0; --idx_path) { /* Get distance to segment to determine if this is the segment to start at */ const auto dist_to_segment = distSquared( - current_tf, closestPointOnSegment( + current_tf, closestPoseOnSegment( current_tf, global_plan_tf_[idx_path], global_plan_tf_[idx_path + 1], estimate_pose_angle_)); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose @@ -290,58 +290,57 @@ tf2::Transform Controller::findPositionOnPlan( // the closest line segment to the current carrot if (controller_state.current_global_plan_index == 0) { - const auto closest_point = closestPointOnSegment( + const auto closest_pose = closestPoseOnSegment( current_tf2, global_plan_tf_[0], global_plan_tf_[1], estimate_pose_angle_); distance_to_goal_ = - distance_to_goal_vector_[1] + sqrt(distSquared(global_plan_tf_[1], closest_point)); + distance_to_goal_vector_[1] + sqrt(distSquared(global_plan_tf_[1], closest_pose)); controller_state.last_visited_pose_index = 0; path_pose_idx = controller_state.current_global_plan_index; - return closest_point; + return closest_pose; } if (controller_state.current_global_plan_index == global_plan_tf_.size() - 1) { - const auto closest_point = closestPointOnSegment( + const auto closest_pose = closestPoseOnSegment( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); distance_to_goal_ = - sqrt(distSquared(global_plan_tf_[controller_state.current_global_plan_index], closest_point)); + sqrt(distSquared(global_plan_tf_[controller_state.current_global_plan_index], closest_pose)); controller_state.last_visited_pose_index = global_plan_tf_.size() - 2; path_pose_idx = controller_state.current_global_plan_index - 1; - return closest_point; + return closest_pose; } - const auto closest_point_ahead = closestPointOnSegment( + const auto closest_pose_ahead = closestPoseOnSegment( current_tf2, global_plan_tf_[controller_state.current_global_plan_index], global_plan_tf_[controller_state.current_global_plan_index + 1], estimate_pose_angle_); - const auto closest_point_behind = closestPointOnSegment( + const auto closest_pose_behind = closestPoseOnSegment( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); if ( - distSquared(current_tf2, closest_point_ahead) < - distSquared(current_tf2, closest_point_behind)) { + distSquared(current_tf2, closest_pose_ahead) < distSquared(current_tf2, closest_pose_behind)) { distance_to_goal_ = distance_to_goal_vector_[controller_state.current_global_plan_index + 1] + sqrt(distSquared( - global_plan_tf_[controller_state.current_global_plan_index + 1], closest_point_ahead)); + global_plan_tf_[controller_state.current_global_plan_index + 1], closest_pose_ahead)); controller_state.last_visited_pose_index = controller_state.current_global_plan_index; path_pose_idx = controller_state.current_global_plan_index; - return closest_point_ahead; + return closest_pose_ahead; } distance_to_goal_ = distance_to_goal_vector_[controller_state.current_global_plan_index] + sqrt(distSquared( - global_plan_tf_[controller_state.current_global_plan_index], closest_point_behind)); + global_plan_tf_[controller_state.current_global_plan_index], closest_pose_behind)); controller_state.last_visited_pose_index = controller_state.current_global_plan_index - 1; path_pose_idx = controller_state.current_global_plan_index; - return closest_point_behind; + return closest_pose_behind; } Controller::UpdateResult Controller::update( diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index bf9cd2a3..3850d4c3 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -9,7 +9,7 @@ namespace { -using path_tracking_pid::closestPointOnSegment; +using path_tracking_pid::closestPoseOnSegment; using path_tracking_pid::deltas_of_plan; using path_tracking_pid::distances_to_goal; using path_tracking_pid::distSquared; @@ -208,73 +208,73 @@ TEST(PathTrackingPidCalculations, DistSquared) EXPECT_EQ(0, distSquared(create_transform(1, 2, 3), create_transform(1, 2, 3))); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_AtEnd) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_AtEnd) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = end; const auto ref = end; - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_AtStart) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_AtStart) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = start; const auto ref = start; - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_CloseToEnd) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_CloseToEnd) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = create_transform(7, 5, 0); const auto ref = end; - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_CloseToStart) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_CloseToStart) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = create_transform(-7, -5, 0); const auto ref = start; - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_Halfway) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_Halfway) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = create_transform(2, 4, 0); const auto ref = create_transform(3, 3, 0); - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_TwoThirds) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_TwoThirds) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(8, 5, 0); const auto point = create_transform(2, 12, 0); const auto ref = create_transform(6, 4, 0); - EXPECT_EQ(ref, closestPointOnSegment(point, start, end, false)); + EXPECT_EQ(ref, closestPoseOnSegment(point, start, end, false)); } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_OtherYaw) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_OtherYaw) { const auto start = tf2::Transform(create_quaternion(1, 1, 1), {2, 2, 0}); const auto end = create_transform(4, 4, 0); const auto point = create_transform(2, 4, 0); const auto ref = tf2::Transform(create_quaternion(0, 0, 1), {3, 3, 0}); - const auto result = closestPointOnSegment(point, start, end, false); + const auto result = closestPoseOnSegment(point, start, end, false); EXPECT_EQ(ref.getOrigin(), result.getOrigin()); // allow for small differences in the basis because of rounding errors in the calculations @@ -285,13 +285,13 @@ TEST(PathTrackingPidCalculations, ClosestPointOnSegment_OtherYaw) } } -TEST(PathTrackingPidCalculations, ClosestPointOnSegment_EstimateAngle) +TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_EstimateAngle) { const auto start = create_transform(2, 2, 0); const auto end = create_transform(4, 4, 0); const auto point = create_transform(2, 4, 0); const auto ref = tf2::Transform(create_quaternion(0, 0, M_PI / 4.0), {3, 3, 0}); - const auto result = closestPointOnSegment(point, start, end, true); + const auto result = closestPoseOnSegment(point, start, end, true); EXPECT_EQ(ref.getOrigin(), result.getOrigin()); // allow for small differences in the basis because of rounding errors in the calculations From ff5bb4580a69e2e83b06490bb2cb147dd246ef07 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Fri, 25 Feb 2022 14:30:14 +0100 Subject: [PATCH 135/156] Fix lowpass & refactor filter structure - Fixed second order lowpass - Made the crossover frequency and damping tuneble - Use the filtered signal for PID calculations - Add a derivative filter --- CMakeLists.txt | 10 +- cfg/Pid.cfg | 4 + doc/second_order_lowpass_tustin.ipynb | 275 ++++++++++++++++++ include/path_tracking_pid/controller.hpp | 5 +- .../path_tracking_pid/details/derivative.hpp | 30 ++ .../path_tracking_pid/details/fifo_array.hpp | 3 + .../details/second_order_lowpass.hpp | 50 +++- src/controller.cpp | 43 ++- src/details/derivative.cpp | 13 + src/details/second_order_lowpass.cpp | 49 ++-- test/unittests/test_derivative.cpp | 23 ++ test/unittests/test_fifo_array.cpp | 11 +- test/unittests/test_second_order_lowpass.cpp | 79 +++++ 13 files changed, 534 insertions(+), 61 deletions(-) create mode 100644 doc/second_order_lowpass_tustin.ipynb create mode 100644 include/path_tracking_pid/details/derivative.hpp create mode 100644 src/details/derivative.cpp create mode 100644 test/unittests/test_derivative.cpp create mode 100644 test/unittests/test_second_order_lowpass.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 27153211..2d550502 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,8 +64,9 @@ add_library(${PROJECT_NAME} src/${PROJECT_NAME}_local_planner.cpp src/controller.cpp src/calculations.cpp - src/visualization.cpp + src/details/derivative.cpp src/details/second_order_lowpass.cpp + src/visualization.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) @@ -108,8 +109,11 @@ install( if(CATKIN_ENABLE_TESTING) add_rostest(test/test_path_tracking_pid.test ARGS rviz:=false reconfigure:=false) catkin_add_gtest(unittests - test/unittests/test_main.cpp + test/unittests/test_calculations.cpp + test/unittests/test_derivative.cpp test/unittests/test_fifo_array.cpp - test/unittests/test_calculations.cpp) + test/unittests/test_main.cpp + test/unittests/test_second_order_lowpass.cpp + ) target_link_libraries(unittests ${catkin_LIBRARIES} ${PROJECT_NAME}) endif() diff --git a/cfg/Pid.cfg b/cfg/Pid.cfg index 69c031cb..46607c36 100755 --- a/cfg/Pid.cfg +++ b/cfg/Pid.cfg @@ -2,6 +2,7 @@ PACKAGE = "path_tracking_pid" from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t +from math import sqrt gen = ParameterGenerator() @@ -33,6 +34,9 @@ gen.add("Kp_ang", double_t, 0, "Kp Angular", 1, 0, 10) gen.add("Ki_ang", double_t, 0, "Ki Angular", 0, 0, 2) gen.add("Kd_ang", double_t, 0, "Kd Angular", 0.3, 0, 10) +gen.add("lowpass_cutoff", double_t, 0, "Lowpass cutoff (Hz), 0 disables the filter", 0, 0, 1000) +gen.add("lowpass_damping", double_t, 0, "Lowpass damping", sqrt(2), 0, 10) + gen.add("feedback_lat", bool_t, 0, "Enable lateral feedback?", True) gen.add("feedback_ang", bool_t, 0, "Enable angular feedback?", False) diff --git a/doc/second_order_lowpass_tustin.ipynb b/doc/second_order_lowpass_tustin.ipynb new file mode 100644 index 00000000..9ef5eca0 --- /dev/null +++ b/doc/second_order_lowpass_tustin.ipynb @@ -0,0 +1,275 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# How to discretize a second order lowpass with Tustin's method\n", + "First a continous time filter is constructed. This filter will be discretized with Tustin's method and converted into C++ code." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from sympy import *" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "a, s, d, T, z = symbols('a,s,d,T,z')" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "First our continous time system" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "# a = 2*pi*c\n", + "sys = 1 / (1/a**2 * s**2 + 2*d/a * s + 1)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate to discrete" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{1}{1 + \\frac{4 d \\left(z - 1\\right)}{T a \\left(z + 1\\right)} + \\frac{4 \\left(z - 1\\right)^{2}}{T^{2} a^{2} \\left(z + 1\\right)^{2}}}$" + ], + "text/plain": [ + "1/(1 + 4*d*(z - 1)/(T*a*(z + 1)) + 4*(z - 1)**2/(T**2*a**2*(z + 1)**2))" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "sys = sys.subs(s, 2 / T * (z - 1) / (z + 1))\n", + "sys" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2)/(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4)\n" + ] + }, + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{T^{2} a^{2} z^{2} + 2 T^{2} a^{2} z + T^{2} a^{2}}{T^{2} a^{2} z^{2} + 2 T^{2} a^{2} z + T^{2} a^{2} + 4 T a d z^{2} - 4 T a d + 4 z^{2} - 8 z + 4}$" + ], + "text/plain": [ + "(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2)/(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4)" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "print(cancel(sys))\n", + "cancel(sys)" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[T**2*a**2, 2*T**2*a**2, T**2*a**2]" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "num = Poly(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2, z)\n", + "num.all_coeffs()" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[T**2*a**2 + 4*T*a*d + 4, 2*T**2*a**2 - 8, T**2*a**2 - 4*T*a*d + 4]" + ] + }, + "execution_count": 7, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "den = Poly(T**2*a**2*z**2 + 2*T**2*a**2*z + T**2*a**2 + 4*T*a*d*z**2 - 4*T*a*d + 4*z**2 - 8*z + 4, z)\n", + "den.all_coeffs()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Try to simplify" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(b**2*z**2 + 2*b**2*z + b**2)/(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4)\n" + ] + }, + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{b^{2} z^{2} + 2 b^{2} z + b^{2}}{b^{2} z^{2} + 2 b^{2} z + b^{2} + 4 b d z^{2} - 4 b d + 4 z^{2} - 8 z + 4}$" + ], + "text/plain": [ + "(b**2*z**2 + 2*b**2*z + b**2)/(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4)" + ] + }, + "execution_count": 8, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "b = symbols('b')\n", + "# a*T = b -> T = b/a\n", + "sys = sys.subs(T, b/a)\n", + "print(cancel(sys))\n", + "cancel(sys)" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[b**2, 2*b**2, b**2]" + ] + }, + "execution_count": 9, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "num = Poly(b**2*z**2 + 2*b**2*z + b**2, z)\n", + "num.all_coeffs()" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[b**2 + 4*b*d + 4, 2*b**2 - 8, b**2 - 4*b*d + 4]" + ] + }, + "execution_count": 10, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "den = Poly(b**2*z**2 + 2*b**2*z + b**2 + 4*b*d*z**2 - 4*b*d + 4*z**2 - 8*z + 4, z)\n", + "den.all_coeffs()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate that to C++" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "```c++\n", + "auto a = 2 * M_PI * c;\n", + "auto b = T * a;\n", + "y_[0] = ((pow(b, 2)) * u_[0] + (2 * pow(b, 2)) * u_[1] + (pow(b, 2)) * u_[2] -\n", + " (2 * pow(b, 2) - 8) * y_[1] - (pow(b, 2) - 4 * T * a * d + 4) * y_[2]) /\n", + " (pow(b, 2) + 4 * T * a * d + 4);\n", + "```" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index b36312cd..bc7fb5f8 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -40,9 +41,9 @@ struct ControllerState double tracking_error_ang = 0.0; // Errors with little history details::SecondOrderLowpass error_lat; - details::SecondOrderLowpass error_deriv_lat; + details::Derivative error_deriv_lat; details::SecondOrderLowpass error_ang; - details::SecondOrderLowpass error_deriv_ang; + details::Derivative error_deriv_ang; }; class Controller : private boost::noncopyable diff --git a/include/path_tracking_pid/details/derivative.hpp b/include/path_tracking_pid/details/derivative.hpp new file mode 100644 index 00000000..a4df7e07 --- /dev/null +++ b/include/path_tracking_pid/details/derivative.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include + +namespace path_tracking_pid::details +{ +/** + * @brief Discrete time derivative filter + */ +class Derivative +{ +public: + /** + * @brief Filter one sample of a signal + * @param u Signal to be filtered + * @param step_size Time step from previous sample + * @return Derivative of the signal + */ + double filter(double u, double step_size); + + /** + * @brief Reset the signal buffers + */ + void reset(); + +private: + FifoArray u_ = {}; +}; + +} // namespace path_tracking_pid::details diff --git a/include/path_tracking_pid/details/fifo_array.hpp b/include/path_tracking_pid/details/fifo_array.hpp index 9ded1e1f..965499e0 100644 --- a/include/path_tracking_pid/details/fifo_array.hpp +++ b/include/path_tracking_pid/details/fifo_array.hpp @@ -24,6 +24,9 @@ class FifoArray // Read-only access to the element at the given index. constexpr const value_type & operator[](std::size_t index) const { return data_[index]; } + // Read-write access to the element at the given index. + value_type & operator[](std::size_t index) { return data_[index]; } + // Read-only access to the element at the given index (with compile-time range check). template constexpr const value_type & at() const diff --git a/include/path_tracking_pid/details/second_order_lowpass.hpp b/include/path_tracking_pid/details/second_order_lowpass.hpp index 857dadcb..bbf873f5 100644 --- a/include/path_tracking_pid/details/second_order_lowpass.hpp +++ b/include/path_tracking_pid/details/second_order_lowpass.hpp @@ -1,29 +1,55 @@ #pragma once +#include #include namespace path_tracking_pid::details { -// Error tracker for the last 3 error and filtered error values. +/** + * @brief Discrete time second order lowpass filter + */ class SecondOrderLowpass { + constexpr static auto NaN = std::numeric_limits::quiet_NaN(); + public: - // Pushes the given value to the errors FIFO buffer. A corresponding filtered error value is calculated and pushed - // to the filtered errors FIFO buffer. - void push(double value); + /** + * @brief Construct a SecondOrderLowpass instance with NaNs + */ + SecondOrderLowpass() = default; - // Resets both errors and filtered errors FIFO buffers. - void reset(); + /** + * @brief Construct a SecondOrderLowpass instance + * @param cutoff frequency in Hz, 0 disables the filter + * @param damping frequency in Hz + */ + SecondOrderLowpass(double cutoff, double damping); - // Read-only access to the errors FIFO buffer. - const FifoArray & errors() const; + /** + * @brief Change the parameters of the filter + * @param cutoff frequency in Hz, 0 disables the filter + * @param damping frequency in Hz + */ + void configure(double cutoff, double damping); - // Read-only access to the filtered errors FIFO buffer. - const FifoArray & filtered_errors() const; + /** + * @brief Filter one sample of a signal + * @param u Signal to be filtered + * @param step_size Time step from previous sample + * @return Lowpass-filtered signal + */ + double filter(double u, double step_size); + + /** + * @brief Reset the signal buffers + */ + void reset(); private: - FifoArray errors_; - FifoArray filtered_errors_; + FifoArray u_ = {}; + FifoArray y_ = {}; + double cutoff_ = NaN; + double damping_ = NaN; }; } // namespace path_tracking_pid::details diff --git a/src/controller.cpp b/src/controller.cpp index f94396a5..8eecdc27 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -399,8 +399,9 @@ Controller::UpdateResult Controller::update( ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for stability."); } - controller_state_.error_lat.push(error.getOrigin().y()); - controller_state_.error_ang.push(angles::normalize_angle(tf2::getYaw(error.getRotation()))); + auto error_lat_filtered = controller_state_.error_lat.filter(error.getOrigin().y(), dt.toSec()); + auto error_ang_filtered = controller_state_.error_ang.filter( + angles::normalize_angle(tf2::getYaw(error.getRotation())), dt.toSec()); // tracking error for diagnostic purposes // Transform current pose into local-path-frame to get tracked-frame-error @@ -422,11 +423,12 @@ Controller::UpdateResult Controller::update( // trackin_error here represents the error between tracked link and position on plan controller_state_.tracking_error_lat = current_tracking_err.y(); - controller_state_.tracking_error_ang = controller_state_.error_ang.errors().at<0>(); + controller_state_.tracking_error_ang = angles::normalize_angle(tf2::getYaw(error.getRotation())), + dt.toSec(); // integrate the error - controller_state_.error_integral_lat += controller_state_.error_lat.errors().at<0>() * dt.toSec(); - controller_state_.error_integral_ang += controller_state_.error_ang.errors().at<0>() * dt.toSec(); + controller_state_.error_integral_lat += error_lat_filtered * dt.toSec(); + controller_state_.error_integral_ang += error_ang_filtered * dt.toSec(); // Apply windup limit to limit the size of the integral term controller_state_.error_integral_lat = @@ -435,25 +437,17 @@ Controller::UpdateResult Controller::update( std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit); // Take derivative of error, first the raw unfiltered data: - controller_state_.error_deriv_lat.push( - (controller_state_.error_lat.errors().at<0>() - controller_state_.error_lat.errors().at<1>()) / - dt.toSec()); - controller_state_.error_deriv_ang.push( - (controller_state_.error_ang.errors().at<0>() - controller_state_.error_ang.errors().at<1>()) / - dt.toSec()); + auto error_deriv_lat = controller_state_.error_deriv_lat.filter(error_lat_filtered, dt.toSec()); + auto error_deriv_ang = controller_state_.error_deriv_ang.filter(error_ang_filtered, dt.toSec()); // calculate the control effort - const auto proportional_lat = - config_.Kp_lat * controller_state_.error_lat.filtered_errors().at<0>(); + const auto proportional_lat = config_.Kp_lat * error_lat_filtered; const auto integral_lat = config_.Ki_lat * controller_state_.error_integral_lat; - const auto derivative_lat = - config_.Kd_lat * controller_state_.error_deriv_lat.filtered_errors().at<0>(); + const auto derivative_lat = config_.Kd_lat * error_deriv_lat; - const auto proportional_ang = - config_.Kp_ang * controller_state_.error_ang.filtered_errors().at<0>(); + const auto proportional_ang = config_.Kp_ang * error_ang_filtered; const auto integral_ang = config_.Ki_ang * controller_state_.error_integral_ang; - const auto derivative_ang = - config_.Kd_ang * controller_state_.error_deriv_ang.filtered_errors().at<0>(); + const auto derivative_ang = config_.Kd_ang * error_deriv_ang; /***** Compute forward velocity *****/ // Apply acceleration limits and end velocity @@ -615,8 +609,8 @@ Controller::UpdateResult Controller::update( // Populate debug output // Error topic containing the 'control' error on which the PID acts result.pid_debug.control_error.linear.x = 0.0; - result.pid_debug.control_error.linear.y = controller_state_.error_lat.errors().at<0>(); - result.pid_debug.control_error.angular.z = controller_state_.error_ang.errors().at<0>(); + result.pid_debug.control_error.linear.y = error_lat_filtered; + result.pid_debug.control_error.angular.z = error_ang_filtered; // Error topic containing the 'tracking' error, i.e. the real error between path and tracked link result.pid_debug.tracking_error.linear.x = 0.0; result.pid_debug.tracking_error.linear.y = controller_state_.tracking_error_lat; @@ -820,7 +814,7 @@ double Controller::mpc_based_max_vel( // max_target_x_vel we can increase it if ( mpc_fwd_iter == config_.mpc_max_fwd_iterations && - fabs(controller_state_.error_lat.errors().at<0>()) <= config_.mpc_max_error_lat && + fabs(controller_state_.tracking_error_lat) <= config_.mpc_max_error_lat && fabs(new_nominal_x_vel) < abs(target_x_vel)) { mpc_vel_optimization_iter += 1; @@ -843,7 +837,7 @@ double Controller::mpc_based_max_vel( mpc_fwd_iter = 0; } // If the robot gets out of bounds earlier we decrease the velocity - else if (abs(controller_state_.error_lat.errors().at<0>()) >= config_.mpc_max_error_lat) { + else if (abs(controller_state_.tracking_error_lat) >= config_.mpc_max_error_lat) { mpc_vel_optimization_iter += 1; // Lower speed @@ -927,6 +921,9 @@ void Controller::printParameters() const void Controller::configure(path_tracking_pid::PidConfig & config) { + controller_state_.error_lat.configure(config.lowpass_cutoff, config.lowpass_damping); + controller_state_.error_ang.configure(config.lowpass_cutoff, config.lowpass_damping); + // Erase all queues when config changes controller_state_.error_lat.reset(); controller_state_.error_deriv_lat.reset(); diff --git a/src/details/derivative.cpp b/src/details/derivative.cpp new file mode 100644 index 00000000..25dc4f63 --- /dev/null +++ b/src/details/derivative.cpp @@ -0,0 +1,13 @@ +#include + +namespace path_tracking_pid::details +{ +double Derivative::filter(double u, double step_size) +{ + // save history + u_.push(u); + return (u_[0] - u_[1]) / step_size; +} + +void Derivative::reset() { u_ = {}; } +} // namespace path_tracking_pid::details diff --git a/src/details/second_order_lowpass.cpp b/src/details/second_order_lowpass.cpp index 8614c5f0..3da54727 100644 --- a/src/details/second_order_lowpass.cpp +++ b/src/details/second_order_lowpass.cpp @@ -3,34 +3,43 @@ namespace path_tracking_pid::details { -namespace +SecondOrderLowpass::SecondOrderLowpass(double cutoff, double damping) +: cutoff_(cutoff), damping_(damping) { -// Used in filter calculations. Default 1.0 corresponds to a cutoff frequency at 1/4 of the sample rate. -constexpr double cutoff = 1.; - -} // namespace +} -void SecondOrderLowpass::push(double value) +void SecondOrderLowpass::configure(double cutoff, double damping) { - errors_.push(value); - - filtered_errors_.push( - (1 / (1 + cutoff * cutoff + M_SQRT2 * cutoff)) * - (errors_.at<2>() + 2 * errors_.at<1>() + errors_.at<0>() - - (cutoff * cutoff - M_SQRT2 * cutoff + 1) * filtered_errors_.at<1>() - - (-2 * cutoff * cutoff + 2) * filtered_errors_.at<0>())); + cutoff_ = cutoff; + damping_ = damping; } -void SecondOrderLowpass::reset() +double SecondOrderLowpass::filter(double u, double step_size) { - errors_.reset(); - filtered_errors_.reset(); + // save history + u_.push(u); + y_.push(u); // increase index so the math below looks correct + + if (cutoff_ == 0) { + return u; + } + + // A continous time second order lowpass was discretized with Tustin's method. For a mathematical + // explanation, see doc/second_order_lowpass_tustin.ipynb + auto c = cutoff_; + auto d = damping_; + auto T = step_size; + auto a = 2 * M_PI * c; + auto b = T * a; + y_[0] = ((pow(b, 2)) * u_[0] + (2 * pow(b, 2)) * u_[1] + (pow(b, 2)) * u_[2] - + (2 * pow(b, 2) - 8) * y_[1] - (pow(b, 2) - 4 * T * a * d + 4) * y_[2]) / + (pow(b, 2) + 4 * T * a * d + 4); + return y_[0]; } -const FifoArray & SecondOrderLowpass::errors() const { return errors_; } -const FifoArray & SecondOrderLowpass::filtered_errors() const +void SecondOrderLowpass::reset() { - return filtered_errors_; + u_ = {}; + y_ = {}; } - } // namespace path_tracking_pid::details diff --git a/test/unittests/test_derivative.cpp b/test/unittests/test_derivative.cpp new file mode 100644 index 00000000..d98c7def --- /dev/null +++ b/test/unittests/test_derivative.cpp @@ -0,0 +1,23 @@ +#include + +#include +#include + +using path_tracking_pid::details::Derivative; + +constexpr double eps = 1e-6; + +TEST(Derivative, StepResponse) +{ + double dt = 0.1; + + Derivative filter; + + std::vector expected_response = {10, 0, 0}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} diff --git a/test/unittests/test_fifo_array.cpp b/test/unittests/test_fifo_array.cpp index 42a1562b..bbf8cfdb 100644 --- a/test/unittests/test_fifo_array.cpp +++ b/test/unittests/test_fifo_array.cpp @@ -4,7 +4,6 @@ namespace { - using path_tracking_pid::details::FifoArray; TEST(PathTrackingPidDetailsFifoArray, Initialize) @@ -114,4 +113,14 @@ TEST(PathTrackingPidDetailsFifoArray, OtherSize) EXPECT_EQ(fifo[4], 2); } +TEST(PathTrackingPidDetailsFifoArray, Assign) +{ + FifoArray fifo; + fifo[0] = 1; + + EXPECT_EQ(fifo[0], 1); + EXPECT_EQ(fifo[1], 0); + EXPECT_EQ(fifo[2], 0); +} + } // namespace diff --git a/test/unittests/test_second_order_lowpass.cpp b/test/unittests/test_second_order_lowpass.cpp new file mode 100644 index 00000000..1489be0b --- /dev/null +++ b/test/unittests/test_second_order_lowpass.cpp @@ -0,0 +1,79 @@ +#include + +#include +#include +#include + +using path_tracking_pid::details::SecondOrderLowpass; + +constexpr double eps = 1e-6; + +TEST(SecondOrderLowpass, StepResponse) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488, + 0.968659, 0.984211, 0.991911, 0.995898, 0.997907}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, Disable) +{ + double dt = 0.1; + double cutoff = 0; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + EXPECT_NEAR(filter.filter(0, dt), 0, eps); + EXPECT_NEAR(filter.filter(1, dt), 1, eps); + EXPECT_NEAR(filter.filter(5, dt), 5, eps); +} + +TEST(SecondOrderLowpass, StepResponseCutoff) +{ + double dt = 0.1; + double cutoff = 1 / dt / 8; // lower cutoff so slower response + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = { + 0.068087, 0.255112, 0.461572, 0.612177, 0.720691, + 0.798844, 0.855129, 0.895665, 0.924859, 0.945884, + }; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, StepResponseDamping) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2) * 2; // more damping + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = { + 0.101795, 0.318258, 0.494899, 0.618187, 0.716157, + 0.786043, 0.84057, 0.880057, 0.91048, 0.932743, + }; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} From d5fdccb5b8365ce485a21a1193462dea688e5146 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Thu, 3 Mar 2022 16:29:31 +0100 Subject: [PATCH 136/156] Enable feedforward_ang during testing for lower error --- param/controllers.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/param/controllers.yaml b/param/controllers.yaml index 5d563b7c..3fe1be86 100644 --- a/param/controllers.yaml +++ b/param/controllers.yaml @@ -15,4 +15,5 @@ PathTrackingPID: abs_minimum_x_vel: 0.0 anti_collision: true use_mpc: false + feedforward_ang: true controller_debug_enabled: true From f63223e71f124e578a23f22339c554dde319a797 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Fri, 4 Mar 2022 15:49:48 +0100 Subject: [PATCH 137/156] Add integral filter abstraction --- CMakeLists.txt | 2 + doc/integral_tustin.ipynb | 130 ++++++++++++++++++ include/path_tracking_pid/controller.hpp | 7 +- .../path_tracking_pid/details/integral.hpp | 48 +++++++ src/controller.cpp | 28 ++-- src/details/integral.cpp | 28 ++++ test/unittests/test_integral.cpp | 24 ++++ 7 files changed, 249 insertions(+), 18 deletions(-) create mode 100644 doc/integral_tustin.ipynb create mode 100644 include/path_tracking_pid/details/integral.hpp create mode 100644 src/details/integral.cpp create mode 100644 test/unittests/test_integral.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2d550502..bb137b63 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,6 +65,7 @@ add_library(${PROJECT_NAME} src/controller.cpp src/calculations.cpp src/details/derivative.cpp + src/details/integral.cpp src/details/second_order_lowpass.cpp src/visualization.cpp ) @@ -112,6 +113,7 @@ if(CATKIN_ENABLE_TESTING) test/unittests/test_calculations.cpp test/unittests/test_derivative.cpp test/unittests/test_fifo_array.cpp + test/unittests/test_integral.cpp test/unittests/test_main.cpp test/unittests/test_second_order_lowpass.cpp ) diff --git a/doc/integral_tustin.ipynb b/doc/integral_tustin.ipynb new file mode 100644 index 00000000..27538c95 --- /dev/null +++ b/doc/integral_tustin.ipynb @@ -0,0 +1,130 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# How to discretize a integral filter with Tustin's method\n", + "First a continous time filter is constructed. This filter will be discretized with Tustin's method and converted into C++ code." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from sympy import *" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "s, T, z = symbols('s,T,z')" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "First our continous time system" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "sys = 1 / s" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate to discrete" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{T \\left(z + 1\\right)}{2 \\left(z - 1\\right)}$" + ], + "text/plain": [ + "T*(z + 1)/(2*(z - 1))" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "sys = sys.subs(s, 2 / T * (z - 1) / (z + 1))\n", + "sys" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Translate that to C++" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "```\n", + "H = y/u = T / 2 * (z + 1)/(z - 1)\n", + "\n", + "u * T / 2 * (z + 1) = y * (z - 1)\n", + "u * T / 2 * (1 + z^-1) = y * (1 - z^-1)\n", + "T / 2 * (u[0] + u[1]) = y[0] - y[1]\n", + "y[0] = T / 2 * (u[0] + u[1]) + y[1]\n", + "```\n", + "\n", + "```c++\n", + "y[0] = T / 2 * (u[0] + u[1]) + y[1]\n", + "```" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index bc7fb5f8..9cdb9e4d 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -35,14 +36,14 @@ struct ControllerState double previous_steering_yaw_vel = 0.0; bool end_phase_enabled = false; bool end_reached = false; - double error_integral_lat = 0.0; - double error_integral_ang = 0.0; double tracking_error_lat = 0.0; double tracking_error_ang = 0.0; // Errors with little history details::SecondOrderLowpass error_lat; - details::Derivative error_deriv_lat; details::SecondOrderLowpass error_ang; + details::Integral error_integral_lat; + details::Integral error_integral_ang; + details::Derivative error_deriv_lat; details::Derivative error_deriv_ang; }; diff --git a/include/path_tracking_pid/details/integral.hpp b/include/path_tracking_pid/details/integral.hpp new file mode 100644 index 00000000..cc0e6bf4 --- /dev/null +++ b/include/path_tracking_pid/details/integral.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include + +namespace path_tracking_pid::details +{ +/** + * @brief Discrete time integral filter + */ +class Integral +{ + constexpr static auto NaN = std::numeric_limits::quiet_NaN(); + +public: + Integral() = default; + + /** + * @brief Construct an integral filter + * @param windup_limit Integral windup limit + */ + explicit Integral(double windup_limit); + + /** + * @brief Change the parameters of the filter + * @param windup_limit Integral windup limit + */ + void configure(double windup_limit); + + /** + * @brief Filter one sample of a signal + * @param u Signal to be filtered + * @param step_size Time step from previous sample + * @return Integral of the signal + */ + double filter(double u, double step_size); + + /** + * @brief Reset the signal buffers + */ + void reset(); + +private: + FifoArray u_ = {}; + FifoArray y_ = {}; + double windup_limit_ = NaN; +}; + +} // namespace path_tracking_pid::details diff --git a/src/controller.cpp b/src/controller.cpp index 8eecdc27..7912d888 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -427,14 +427,10 @@ Controller::UpdateResult Controller::update( dt.toSec(); // integrate the error - controller_state_.error_integral_lat += error_lat_filtered * dt.toSec(); - controller_state_.error_integral_ang += error_ang_filtered * dt.toSec(); - - // Apply windup limit to limit the size of the integral term - controller_state_.error_integral_lat = - std::clamp(controller_state_.error_integral_lat, -windup_limit, windup_limit); - controller_state_.error_integral_ang = - std::clamp(controller_state_.error_integral_ang, -windup_limit, windup_limit); + auto error_integral_lat = + controller_state_.error_integral_lat.filter(error_lat_filtered, dt.toSec()); + auto error_integral_ang = + controller_state_.error_integral_ang.filter(error_lat_filtered, dt.toSec()); // Take derivative of error, first the raw unfiltered data: auto error_deriv_lat = controller_state_.error_deriv_lat.filter(error_lat_filtered, dt.toSec()); @@ -442,11 +438,11 @@ Controller::UpdateResult Controller::update( // calculate the control effort const auto proportional_lat = config_.Kp_lat * error_lat_filtered; - const auto integral_lat = config_.Ki_lat * controller_state_.error_integral_lat; + const auto integral_lat = config_.Ki_lat * error_integral_lat; const auto derivative_lat = config_.Kd_lat * error_deriv_lat; const auto proportional_ang = config_.Kp_ang * error_ang_filtered; - const auto integral_ang = config_.Ki_ang * controller_state_.error_integral_ang; + const auto integral_ang = config_.Ki_ang * error_integral_ang; const auto derivative_ang = config_.Kd_ang * error_deriv_ang; /***** Compute forward velocity *****/ @@ -728,8 +724,8 @@ Controller::UpdateResult Controller::update( // Publish control effort if controller enabled if (!enabled_) // Do nothing reset integral action and all filters { - controller_state_.error_integral_lat = 0.0; - controller_state_.error_integral_ang = 0.0; + controller_state_.error_integral_lat.reset(); + controller_state_.error_integral_ang.reset(); } controller_state_.current_x_vel = new_x_vel; @@ -923,6 +919,8 @@ void Controller::configure(path_tracking_pid::PidConfig & config) { controller_state_.error_lat.configure(config.lowpass_cutoff, config.lowpass_damping); controller_state_.error_ang.configure(config.lowpass_cutoff, config.lowpass_damping); + controller_state_.error_integral_lat.configure(windup_limit); + controller_state_.error_integral_ang.configure(windup_limit); // Erase all queues when config changes controller_state_.error_lat.reset(); @@ -975,11 +973,11 @@ void Controller::reset() controller_state_.previous_steering_angle = 0.0; controller_state_.previous_steering_yaw_vel = 0.0; controller_state_.previous_steering_x_vel = 0.0; - controller_state_.error_integral_lat = 0.0; - controller_state_.error_integral_ang = 0.0; controller_state_.error_lat.reset(); - controller_state_.error_deriv_lat.reset(); controller_state_.error_ang.reset(); + controller_state_.error_integral_lat.reset(); + controller_state_.error_integral_ang.reset(); + controller_state_.error_deriv_lat.reset(); controller_state_.error_deriv_ang.reset(); } diff --git a/src/details/integral.cpp b/src/details/integral.cpp new file mode 100644 index 00000000..3b8997cb --- /dev/null +++ b/src/details/integral.cpp @@ -0,0 +1,28 @@ +#include + +namespace path_tracking_pid::details +{ +Integral::Integral(double windup_limit) : windup_limit_(windup_limit) {} + +void Integral::configure(double windup_limit) { windup_limit_ = windup_limit; } + +double Integral::filter(double u, double step_size) +{ + // save history + u_.push(u); + y_.push(u); // increase index so the math below looks correct + + // A continous time integrator was discretized with Tustin's method. For a mathematical + // explanation, see doc/integral_tustin.ipynb + auto T = step_size; + y_[0] = T / 2 * (u_[0] + u_[1]) + y_[1]; + y_[0] = std::clamp(y_[0], -windup_limit_, windup_limit_); + return y_[0]; +} + +void Integral::reset() +{ + u_ = {}; + y_ = {}; +} +} // namespace path_tracking_pid::details diff --git a/test/unittests/test_integral.cpp b/test/unittests/test_integral.cpp new file mode 100644 index 00000000..1510fcc7 --- /dev/null +++ b/test/unittests/test_integral.cpp @@ -0,0 +1,24 @@ +#include + +#include +#include + +using path_tracking_pid::details::Integral; + +constexpr double eps = 1e-6; + +TEST(Integral, StepResponse) +{ + double dt = 0.1; + double windup_limit = 0.5; + + Integral filter{windup_limit}; + + std::vector expected_response = {0.05, 0.15, 0.25, 0.35, 0.45, 0.5, 0.5}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} From b485259ab4cc52e28797593aeef8c8756818650d Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Mon, 7 Mar 2022 11:12:24 +0100 Subject: [PATCH 138/156] Add reset() and configure() tests --- test/unittests/test_derivative.cpp | 11 ++++ test/unittests/test_integral.cpp | 29 +++++++++++ test/unittests/test_second_order_lowpass.cpp | 54 ++++++++++++++++++++ 3 files changed, 94 insertions(+) diff --git a/test/unittests/test_derivative.cpp b/test/unittests/test_derivative.cpp index d98c7def..23f033e5 100644 --- a/test/unittests/test_derivative.cpp +++ b/test/unittests/test_derivative.cpp @@ -21,3 +21,14 @@ TEST(Derivative, StepResponse) EXPECT_NEAR(result, expected_response[i], eps); } } + +TEST(Derivative, Reset) +{ + double dt = 0.1; + + Derivative filter; + + EXPECT_NEAR(filter.filter(1, dt), 10, eps); + filter.reset(); + EXPECT_NEAR(filter.filter(0, dt), 0, eps); +} diff --git a/test/unittests/test_integral.cpp b/test/unittests/test_integral.cpp index 1510fcc7..1032cfc9 100644 --- a/test/unittests/test_integral.cpp +++ b/test/unittests/test_integral.cpp @@ -22,3 +22,32 @@ TEST(Integral, StepResponse) EXPECT_NEAR(result, expected_response[i], eps); } } + +TEST(Integral, Reset) +{ + double dt = 0.1; + double windup_limit = 0.5; + + Integral filter{windup_limit}; + + EXPECT_NEAR(filter.filter(1, dt), 0.05, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.15, eps); + filter.reset(); + EXPECT_NEAR(filter.filter(1, dt), 0.05, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.15, eps); +} + +TEST(Integral, Configure) +{ + double dt = 0.1; + double windup_limit = 0.2; + + Integral filter{windup_limit}; + + EXPECT_NEAR(filter.filter(1, dt), 0.05, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.15, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.20, eps); + filter.configure(0.35); + EXPECT_NEAR(filter.filter(1, dt), 0.30, eps); + EXPECT_NEAR(filter.filter(1, dt), 0.35, eps); +} diff --git a/test/unittests/test_second_order_lowpass.cpp b/test/unittests/test_second_order_lowpass.cpp index 1489be0b..93d23ea8 100644 --- a/test/unittests/test_second_order_lowpass.cpp +++ b/test/unittests/test_second_order_lowpass.cpp @@ -77,3 +77,57 @@ TEST(SecondOrderLowpass, StepResponseDamping) EXPECT_NEAR(result, expected_response[i], eps); } } + +TEST(SecondOrderLowpass, Reset) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } + + filter.reset(); + + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} + +TEST(SecondOrderLowpass, Configure) +{ + double dt = 0.1; + double cutoff = 1 / dt / 4; + double damping = sqrt(2); + + SecondOrderLowpass filter(cutoff, damping); + + std::vector expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } + + filter.configure(cutoff / 2, damping); + + // no configure step response is {0.968659, 0.984211, 0.991911, 0.995898, 0.997907} + expected_response = {0.957154, 0.969162, 0.977792, 0.984006, 0.988481}; + for (int i = 0; i < static_cast(expected_response.size()); ++i) { + SCOPED_TRACE(i); + + auto result = filter.filter(1, dt); + EXPECT_NEAR(result, expected_response[i], eps); + } +} From d424e22f47ced168ec4c28341950b25123d1cbb8 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Mon, 7 Mar 2022 16:11:38 +0100 Subject: [PATCH 139/156] Increase timeout to reduce test flakeyness --- test/test_path_tracking_pid.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_path_tracking_pid.py b/test/test_path_tracking_pid.py index aa4efb8e..74ec6f68 100755 --- a/test/test_path_tracking_pid.py +++ b/test/test_path_tracking_pid.py @@ -104,7 +104,7 @@ def test_exepath_action(self): rospy.sleep(10.0) self.assertTrue(checker.slowed_down) - finished_in_time = client.wait_for_result(timeout=rospy.Duration(60)) + finished_in_time = client.wait_for_result(timeout=rospy.Duration(120)) self.assertTrue(finished_in_time, msg="Action call didn't return in time") self.assertEqual(client.get_state(), outcome_exp, msg="Wrong action outcome") From b084218dad1d2d435b6587d7479d170fe5038817 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 15 Mar 2022 11:51:10 +0000 Subject: [PATCH 140/156] Cleanup Controller::update(). --- src/controller.cpp | 54 +++++++++++++++++++++++----------------------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 7912d888..5fbd4b50 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -60,6 +60,23 @@ bool check_plan(const std::vector & plan) return true; } +/** + * Determine the control point pose based on the given pose and control distance. + * + * @param[in] pose Pose to transform. + * @param[in] control_distance Control distance to use. + * @return Control point pose. + */ +tf2::Transform getControlPointPose(const tf2::Transform & pose, double control_distance) +{ + const auto theda_rp = tf2::getYaw(pose.getRotation()); + const auto origin = tf2::Vector3{ + pose.getOrigin().x() + control_distance * cos(theda_rp), + pose.getOrigin().y() + control_distance * sin(theda_rp), 0}; + + return tf2::Transform{pose.getRotation(), origin}; +} + } // namespace void Controller::setHolonomic(bool holonomic) @@ -349,41 +366,21 @@ Controller::UpdateResult Controller::update( { UpdateResult result; - const double current_x_vel = controller_state_.current_x_vel; - const double current_yaw_vel = controller_state_.current_yaw_vel; - - // Compute location of the point to be controlled - const double theda_rp = tf2::getYaw(current_tf.getRotation()); - tf2::Vector3 current_with_carrot_origin; - current_with_carrot_origin.setX(current_tf.getOrigin().x() + config_.l * cos(theda_rp)); - current_with_carrot_origin.setY(current_tf.getOrigin().y() + config_.l * sin(theda_rp)); - current_with_carrot_origin.setZ(0); - - current_with_carrot_.setOrigin(current_with_carrot_origin); - current_with_carrot_.setRotation(current_tf.getRotation()); + current_with_carrot_ = getControlPointPose(current_tf, config_.l); + const auto & reference_pose = config_.track_base_link ? current_tf : current_with_carrot_; size_t path_pose_idx; + current_pos_on_plan_ = current_goal_ = + findPositionOnPlan(reference_pose, controller_state_, path_pose_idx); + if (config_.track_base_link) { - // Find closes robot position to path and then project carrot on goal - current_pos_on_plan_ = current_goal_ = - findPositionOnPlan(current_tf, controller_state_, path_pose_idx); - // To track the base link the goal is then transform to the control point goal - double theda_rp = tf2::getYaw(current_goal_.getRotation()); - tf2::Vector3 newControlOrigin; - newControlOrigin.setX(current_goal_.getOrigin().x() + config_.l * cos(theda_rp)); - newControlOrigin.setY(current_goal_.getOrigin().y() + config_.l * sin(theda_rp)); - newControlOrigin.setZ(0); - current_goal_.setOrigin(newControlOrigin); - } else { - // find position of current position with projected carrot - current_pos_on_plan_ = current_goal_ = - findPositionOnPlan(current_with_carrot_, controller_state_, path_pose_idx); + current_goal_ = getControlPointPose(current_goal_, config_.l); } result.progress = 1.0 - distance_to_goal_ / distance_to_goal_vector_[0]; // Compute errorPose between controlPose and currentGoalPose - tf2::Transform error = current_with_carrot_.inverseTimes(current_goal_); + const auto error = current_with_carrot_.inverseTimes(current_goal_); //***** Feedback control *****// if (!((config_.Kp_lat <= 0. && config_.Ki_lat <= 0. && config_.Kd_lat <= 0.) || @@ -450,6 +447,9 @@ Controller::UpdateResult Controller::update( double t_end_phase_current; double d_end_phase; + const double current_x_vel = controller_state_.current_x_vel; + const double current_yaw_vel = controller_state_.current_yaw_vel; + // Compute time to reach end velocity from current velocity // Compute estimate overall distance during end_phase // The estimates are done a bit conservative to account that robot will take longer From be0176db29c939e877a1743913bc31f943ae69d4 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 15 Mar 2022 12:16:39 +0000 Subject: [PATCH 141/156] Replaced path pose parameter with return value. --- include/path_tracking_pid/controller.hpp | 21 ++++++++++----------- src/controller.cpp | 23 ++++++++++------------- src/path_tracking_pid_local_planner.cpp | 10 +++++----- 3 files changed, 25 insertions(+), 29 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 9cdb9e4d..b3c60703 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -96,22 +96,21 @@ class Controller : private boost::noncopyable const geometry_msgs::Twist & steering_odom_twist, const std::vector & global_plan); + /** Result of findPositionOnPlan(). */ + struct FindPositionOnPlanResult + { + tf2::Transform position; + std::size_t path_pose_idx = 0; + }; + /** * Find position on plan by looking at the surroundings of last known pose. * @param current Where is the robot now? * @param controller_state The current state of the controller that gets updated by this function - * @return tf of found position on plan - * @return index of current path-pose if requested + * @return Found position on plan and the index of current path-pose */ - tf2::Transform findPositionOnPlan( - const tf2::Transform & current_tf, ControllerState & controller_state, size_t & path_pose_idx); - // Overloaded function definition for users that don't require the segment index - tf2::Transform findPositionOnPlan( - const tf2::Transform & current_tf, ControllerState & controller_state) - { - size_t path_pose_idx; - return findPositionOnPlan(current_tf, controller_state, path_pose_idx); - } + FindPositionOnPlanResult findPositionOnPlan( + const tf2::Transform & current_tf, ControllerState & controller_state); // Result of update() and update_with_limits(). struct UpdateResult diff --git a/src/controller.cpp b/src/controller.cpp index 5fbd4b50..91f79043 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -251,8 +251,8 @@ bool Controller::setPlan( return result; } -tf2::Transform Controller::findPositionOnPlan( - const tf2::Transform & current_tf, ControllerState & controller_state, size_t & path_pose_idx) +Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( + const tf2::Transform & current_tf, ControllerState & controller_state) { auto current_tf2 = current_tf; // 'Project' current_tf by removing z-component @@ -313,9 +313,8 @@ tf2::Transform Controller::findPositionOnPlan( distance_to_goal_ = distance_to_goal_vector_[1] + sqrt(distSquared(global_plan_tf_[1], closest_pose)); controller_state.last_visited_pose_index = 0; - path_pose_idx = controller_state.current_global_plan_index; - return closest_pose; + return {closest_pose, controller_state.current_global_plan_index}; } if (controller_state.current_global_plan_index == global_plan_tf_.size() - 1) { @@ -326,9 +325,8 @@ tf2::Transform Controller::findPositionOnPlan( distance_to_goal_ = sqrt(distSquared(global_plan_tf_[controller_state.current_global_plan_index], closest_pose)); controller_state.last_visited_pose_index = global_plan_tf_.size() - 2; - path_pose_idx = controller_state.current_global_plan_index - 1; - return closest_pose; + return {closest_pose, controller_state.current_global_plan_index - 1}; } const auto closest_pose_ahead = closestPoseOnSegment( @@ -345,9 +343,8 @@ tf2::Transform Controller::findPositionOnPlan( sqrt(distSquared( global_plan_tf_[controller_state.current_global_plan_index + 1], closest_pose_ahead)); controller_state.last_visited_pose_index = controller_state.current_global_plan_index; - path_pose_idx = controller_state.current_global_plan_index; - return closest_pose_ahead; + return {closest_pose_ahead, controller_state.current_global_plan_index}; } distance_to_goal_ = @@ -355,9 +352,8 @@ tf2::Transform Controller::findPositionOnPlan( sqrt(distSquared( global_plan_tf_[controller_state.current_global_plan_index], closest_pose_behind)); controller_state.last_visited_pose_index = controller_state.current_global_plan_index - 1; - path_pose_idx = controller_state.current_global_plan_index; - return closest_pose_behind; + return {closest_pose_behind, controller_state.current_global_plan_index}; } Controller::UpdateResult Controller::update( @@ -369,9 +365,10 @@ Controller::UpdateResult Controller::update( current_with_carrot_ = getControlPointPose(current_tf, config_.l); const auto & reference_pose = config_.track_base_link ? current_tf : current_with_carrot_; - size_t path_pose_idx; - current_pos_on_plan_ = current_goal_ = - findPositionOnPlan(reference_pose, controller_state_, path_pose_idx); + const auto find_result = findPositionOnPlan(reference_pose, controller_state_); + const auto & path_pose_idx = find_result.path_pose_idx; + + current_pos_on_plan_ = current_goal_ = find_result.position; if (config_.track_base_link) { current_goal_ = getControlPointPose(current_goal_, config_.l); diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index be7b153e..97ccf202 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -328,12 +328,13 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() auto projected_step_tf = tf2_convert(tfCurPoseStamped_.transform); projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link projected_step_tf = - pid_controller_.findPositionOnPlan(projected_step_tf, projected_controller_state); + pid_controller_.findPositionOnPlan(projected_step_tf, projected_controller_state).position; projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose for (uint step = 0; step < n_steps; step++) { tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf; projected_step_tf = - pid_controller_.findPositionOnPlan(next_straight_step_tf, projected_controller_state); + pid_controller_.findPositionOnPlan(next_straight_step_tf, projected_controller_state) + .position; projected_steps_tf.push_back(projected_step_tf); // Fill markers: @@ -341,7 +342,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() poses_on_path_points.push_back(projected_step_tf.getOrigin()); } - costmap_2d::Costmap2D* costmap2d = costmap_->getCostmap(); + costmap_2d::Costmap2D * costmap2d = costmap_->getCostmap(); std::vector collision_footprint_points; polygon_t previous_footprint_xy; polygon_t collision_polygon; @@ -356,8 +357,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() int map_x, map_y; costmap2d->worldToMapEnforceBounds(x, y, map_x, map_y); uint8_t projected_step_cost = costmap2d->getCost(map_x, map_y); - if (projected_step_cost > max_projected_step_cost) - { + if (projected_step_cost > max_projected_step_cost) { max_projected_step_cost = projected_step_cost; } From e9701f3e4c17d58cc1f662b172960f72bb21dbfb Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 15 Mar 2022 12:28:22 +0000 Subject: [PATCH 142/156] Made Controller::findPositionOnPlan() const. --- include/path_tracking_pid/controller.hpp | 5 +++-- src/controller.cpp | 21 ++++++++++----------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index b3c60703..5b8270c0 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -101,16 +101,17 @@ class Controller : private boost::noncopyable { tf2::Transform position; std::size_t path_pose_idx = 0; + double distance_to_goal = 0; }; /** * Find position on plan by looking at the surroundings of last known pose. * @param current Where is the robot now? * @param controller_state The current state of the controller that gets updated by this function - * @return Found position on plan and the index of current path-pose + * @return Found position on plan, index of current path-pose and the distance to the goal. */ FindPositionOnPlanResult findPositionOnPlan( - const tf2::Transform & current_tf, ControllerState & controller_state); + const tf2::Transform & current_tf, ControllerState & controller_state) const; // Result of update() and update_with_limits(). struct UpdateResult diff --git a/src/controller.cpp b/src/controller.cpp index 91f79043..4bd511ae 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -252,7 +252,7 @@ bool Controller::setPlan( } Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( - const tf2::Transform & current_tf, ControllerState & controller_state) + const tf2::Transform & current_tf, ControllerState & controller_state) const { auto current_tf2 = current_tf; // 'Project' current_tf by removing z-component @@ -309,24 +309,22 @@ Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( if (controller_state.current_global_plan_index == 0) { const auto closest_pose = closestPoseOnSegment( current_tf2, global_plan_tf_[0], global_plan_tf_[1], estimate_pose_angle_); - - distance_to_goal_ = + const auto distance_to_goal = distance_to_goal_vector_[1] + sqrt(distSquared(global_plan_tf_[1], closest_pose)); controller_state.last_visited_pose_index = 0; - return {closest_pose, controller_state.current_global_plan_index}; + return {closest_pose, controller_state.current_global_plan_index, distance_to_goal}; } if (controller_state.current_global_plan_index == global_plan_tf_.size() - 1) { const auto closest_pose = closestPoseOnSegment( current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); - - distance_to_goal_ = + const auto distance_to_goal = sqrt(distSquared(global_plan_tf_[controller_state.current_global_plan_index], closest_pose)); controller_state.last_visited_pose_index = global_plan_tf_.size() - 2; - return {closest_pose, controller_state.current_global_plan_index - 1}; + return {closest_pose, controller_state.current_global_plan_index - 1, distance_to_goal}; } const auto closest_pose_ahead = closestPoseOnSegment( @@ -338,22 +336,22 @@ Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( if ( distSquared(current_tf2, closest_pose_ahead) < distSquared(current_tf2, closest_pose_behind)) { - distance_to_goal_ = + const auto distance_to_goal = distance_to_goal_vector_[controller_state.current_global_plan_index + 1] + sqrt(distSquared( global_plan_tf_[controller_state.current_global_plan_index + 1], closest_pose_ahead)); controller_state.last_visited_pose_index = controller_state.current_global_plan_index; - return {closest_pose_ahead, controller_state.current_global_plan_index}; + return {closest_pose_ahead, controller_state.current_global_plan_index, distance_to_goal}; } - distance_to_goal_ = + const auto distance_to_goal = distance_to_goal_vector_[controller_state.current_global_plan_index] + sqrt(distSquared( global_plan_tf_[controller_state.current_global_plan_index], closest_pose_behind)); controller_state.last_visited_pose_index = controller_state.current_global_plan_index - 1; - return {closest_pose_behind, controller_state.current_global_plan_index}; + return {closest_pose_behind, controller_state.current_global_plan_index, distance_to_goal}; } Controller::UpdateResult Controller::update( @@ -368,6 +366,7 @@ Controller::UpdateResult Controller::update( const auto find_result = findPositionOnPlan(reference_pose, controller_state_); const auto & path_pose_idx = find_result.path_pose_idx; + distance_to_goal_ = find_result.distance_to_goal; current_pos_on_plan_ = current_goal_ = find_result.position; if (config_.track_base_link) { From 9d4b602bf443a903415b6699cdeb2c06356211af Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 15 Mar 2022 12:31:43 +0000 Subject: [PATCH 143/156] Replaced distance_to_goal_ data member with local. --- include/path_tracking_pid/controller.hpp | 1 - src/controller.cpp | 12 ++++++------ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 5b8270c0..364cbf7d 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -210,7 +210,6 @@ class Controller : private boost::noncopyable std::vector global_plan_tf_; // Global plan vector std::vector distance_to_goal_vector_; // Vector with distances to goal std::vector turning_radius_inv_vector_; // Vector with computed turning radius inverse - double distance_to_goal_ = NAN; tf2::Transform current_goal_; tf2::Transform current_pos_on_plan_; tf2::Transform current_with_carrot_; diff --git a/src/controller.cpp b/src/controller.cpp index 4bd511ae..4e18cc9c 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -365,15 +365,15 @@ Controller::UpdateResult Controller::update( const auto & reference_pose = config_.track_base_link ? current_tf : current_with_carrot_; const auto find_result = findPositionOnPlan(reference_pose, controller_state_); const auto & path_pose_idx = find_result.path_pose_idx; + const auto & distance_to_goal = find_result.distance_to_goal; - distance_to_goal_ = find_result.distance_to_goal; current_pos_on_plan_ = current_goal_ = find_result.position; if (config_.track_base_link) { current_goal_ = getControlPointPose(current_goal_, config_.l); } - result.progress = 1.0 - distance_to_goal_ / distance_to_goal_vector_[0]; + result.progress = 1.0 - distance_to_goal / distance_to_goal_vector_[0]; // Compute errorPose between controlPose and currentGoalPose const auto error = current_with_carrot_.inverseTimes(current_goal_); @@ -467,7 +467,7 @@ Controller::UpdateResult Controller::update( } ROS_DEBUG("t_end_phase_current: %f", t_end_phase_current); ROS_DEBUG("d_end_phase: %f", d_end_phase); - ROS_DEBUG("distance_to_goal: %f", distance_to_goal_); + ROS_DEBUG("distance_to_goal: %f", distance_to_goal); const auto in_direction_of_goal = is_in_direction_of_target(current_tf, current_goal_.getOrigin(), target_x_vel); @@ -475,7 +475,7 @@ Controller::UpdateResult Controller::update( // If we are as close to our goal or closer then we need to reach end velocity, enable end_phase. // However, if robot is not facing to the same direction as the local velocity target vector, don't enable end_phase. // This is to avoid skipping paths that start with opposite velocity. - if ((distance_to_goal_ <= fabs(d_end_phase)) && in_direction_of_goal) { + if ((distance_to_goal <= fabs(d_end_phase)) && in_direction_of_goal) { // This state will be remebered to avoid jittering on target_x_vel controller_state_.end_phase_enabled = true; } @@ -540,7 +540,7 @@ Controller::UpdateResult Controller::update( // Or when the end velocity is reached. // Warning! If target_end_x_vel == 0 and min_vel = 0 then the robot might not reach end pose if ( - (distance_to_goal_ == 0.0 && target_end_x_vel >= VELOCITY_EPS) || + (distance_to_goal == 0.0 && target_end_x_vel >= VELOCITY_EPS) || (controller_state_.end_phase_enabled && new_x_vel >= target_end_x_vel - VELOCITY_EPS && new_x_vel <= target_end_x_vel + VELOCITY_EPS)) { controller_state_.end_reached = true; @@ -553,7 +553,7 @@ Controller::UpdateResult Controller::update( // eda (Estimated duration of arrival) estimation if (fabs(target_x_vel) > VELOCITY_EPS) { const double t_const = - (copysign(distance_to_goal_, target_x_vel) - d_end_phase) / target_x_vel; + (copysign(distance_to_goal, target_x_vel) - d_end_phase) / target_x_vel; result.eda = fmin(fmax(t_end_phase_current, 0.0) + fmax(t_const, 0.0), LONG_DURATION); } else { result.eda = LONG_DURATION; From b7e143cebbff4bc41a9d2a2c371555506c24c53d Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 15 Mar 2022 12:46:59 +0000 Subject: [PATCH 144/156] Moved last visited pose index to find result. --- include/path_tracking_pid/controller.hpp | 2 +- src/controller.cpp | 24 +++++++++++++----------- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 364cbf7d..3dd8f54b 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -28,7 +28,6 @@ struct TricycleSteeringCmdVel struct ControllerState { size_t current_global_plan_index = 0; - size_t last_visited_pose_index = 0; double current_x_vel = 0.0; double current_yaw_vel = 0.0; double previous_steering_angle = 0.0; @@ -102,6 +101,7 @@ class Controller : private boost::noncopyable tf2::Transform position; std::size_t path_pose_idx = 0; double distance_to_goal = 0; + std::size_t last_visited_pose_index = 0; }; /** diff --git a/src/controller.cpp b/src/controller.cpp index 4e18cc9c..c2ca958b 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -311,9 +311,8 @@ Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( current_tf2, global_plan_tf_[0], global_plan_tf_[1], estimate_pose_angle_); const auto distance_to_goal = distance_to_goal_vector_[1] + sqrt(distSquared(global_plan_tf_[1], closest_pose)); - controller_state.last_visited_pose_index = 0; - return {closest_pose, controller_state.current_global_plan_index, distance_to_goal}; + return {closest_pose, controller_state.current_global_plan_index, distance_to_goal, 0}; } if (controller_state.current_global_plan_index == global_plan_tf_.size() - 1) { @@ -322,9 +321,10 @@ Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); const auto distance_to_goal = sqrt(distSquared(global_plan_tf_[controller_state.current_global_plan_index], closest_pose)); - controller_state.last_visited_pose_index = global_plan_tf_.size() - 2; - return {closest_pose, controller_state.current_global_plan_index - 1, distance_to_goal}; + return { + closest_pose, controller_state.current_global_plan_index - 1, distance_to_goal, + global_plan_tf_.size() - 2}; } const auto closest_pose_ahead = closestPoseOnSegment( @@ -340,18 +340,20 @@ Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( distance_to_goal_vector_[controller_state.current_global_plan_index + 1] + sqrt(distSquared( global_plan_tf_[controller_state.current_global_plan_index + 1], closest_pose_ahead)); - controller_state.last_visited_pose_index = controller_state.current_global_plan_index; - return {closest_pose_ahead, controller_state.current_global_plan_index, distance_to_goal}; + return { + closest_pose_ahead, controller_state.current_global_plan_index, distance_to_goal, + controller_state.current_global_plan_index}; } const auto distance_to_goal = distance_to_goal_vector_[controller_state.current_global_plan_index] + sqrt(distSquared( global_plan_tf_[controller_state.current_global_plan_index], closest_pose_behind)); - controller_state.last_visited_pose_index = controller_state.current_global_plan_index - 1; - return {closest_pose_behind, controller_state.current_global_plan_index, distance_to_goal}; + return { + closest_pose_behind, controller_state.current_global_plan_index, distance_to_goal, + controller_state.current_global_plan_index - 1}; } Controller::UpdateResult Controller::update( @@ -584,10 +586,10 @@ Controller::UpdateResult Controller::update( if (config_.feedforward_ang) { feedforward_ang_ = - turning_radius_inv_vector_[controller_state_.last_visited_pose_index] * control_effort_long_; + turning_radius_inv_vector_[find_result.last_visited_pose_index] * control_effort_long_; ROS_DEBUG( - "turning_radius_inv_vector[%lu] = %f", controller_state_.last_visited_pose_index, - turning_radius_inv_vector_[controller_state_.last_visited_pose_index]); + "turning_radius_inv_vector[%lu] = %f", find_result.last_visited_pose_index, + turning_radius_inv_vector_[find_result.last_visited_pose_index]); control_effort_ang_ = control_effort_ang_ + feedforward_ang_; } else { From 3ac6955d540bf0d4c6e48b8df30dfe80980d9739 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 15 Mar 2022 13:21:32 +0000 Subject: [PATCH 145/156] Replaced controller_state parameter of findPositionOnPlan(). --- include/path_tracking_pid/controller.hpp | 9 ++-- src/controller.cpp | 59 ++++++++++-------------- src/path_tracking_pid_local_planner.cpp | 8 ++-- 3 files changed, 34 insertions(+), 42 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 3dd8f54b..104b248a 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -106,12 +106,13 @@ class Controller : private boost::noncopyable /** * Find position on plan by looking at the surroundings of last known pose. - * @param current Where is the robot now? - * @param controller_state The current state of the controller that gets updated by this function - * @return Found position on plan, index of current path-pose and the distance to the goal. + * @param[in] current_tf Where is the robot now? + * @param[in,out] global_plan_index Global plan index where the search should start. Updated to + * current global plan index once found. + * @return Found position on plan and related data. */ FindPositionOnPlanResult findPositionOnPlan( - const tf2::Transform & current_tf, ControllerState & controller_state) const; + const tf2::Transform & current_tf, std::size_t & global_plan_index) const; // Result of update() and update_with_limits(). struct UpdateResult diff --git a/src/controller.cpp b/src/controller.cpp index c2ca958b..18d83083 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -252,7 +252,7 @@ bool Controller::setPlan( } Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( - const tf2::Transform & current_tf, ControllerState & controller_state) const + const tf2::Transform & current_tf, std::size_t & global_plan_index) const { auto current_tf2 = current_tf; // 'Project' current_tf by removing z-component @@ -272,8 +272,7 @@ Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( // Hence, when idx_path==i we are currently tracking the section connection pose i and pose i+1 // First look in current position and in front - for (auto idx_path = controller_state.current_global_plan_index; - idx_path < global_plan_tf_.size(); idx_path++) { + for (auto idx_path = global_plan_index; idx_path < global_plan_tf_.size(); idx_path++) { error = current_tf2.inverseTimes(global_plan_tf_[idx_path]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! @@ -281,14 +280,14 @@ Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( if (distance_to_path <= minimum_distance_to_path) { minimum_distance_to_path = distance_to_path; - controller_state.current_global_plan_index = idx_path; + global_plan_index = idx_path; } else { break; } } // Then look backwards - for (auto idx_path = controller_state.current_global_plan_index; idx_path > 0; --idx_path) { + for (auto idx_path = global_plan_index; idx_path > 0; --idx_path) { error = current_tf2.inverseTimes(global_plan_tf_[idx_path - 1]); // Calculate 3D distance, since current_tf2 might have significant z-offset and roll/pitch values w.r.t. path-pose // When not doing this, we're brutely projecting in robot's frame and might snap to another segment! @@ -296,64 +295,55 @@ Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( if (distance_to_path < minimum_distance_to_path) { minimum_distance_to_path = distance_to_path; - controller_state.current_global_plan_index = idx_path - 1; + global_plan_index = idx_path - 1; } else { break; } } - ROS_DEBUG( - "progress: %lu of %lu", controller_state.current_global_plan_index, global_plan_tf_.size() - 1); + ROS_DEBUG("progress: %lu of %lu", global_plan_index, global_plan_tf_.size() - 1); // To finalize, compute the indexes of the start and end points of // the closest line segment to the current carrot - if (controller_state.current_global_plan_index == 0) { + if (global_plan_index == 0) { const auto closest_pose = closestPoseOnSegment( current_tf2, global_plan_tf_[0], global_plan_tf_[1], estimate_pose_angle_); const auto distance_to_goal = distance_to_goal_vector_[1] + sqrt(distSquared(global_plan_tf_[1], closest_pose)); - return {closest_pose, controller_state.current_global_plan_index, distance_to_goal, 0}; + return {closest_pose, global_plan_index, distance_to_goal, 0}; } - if (controller_state.current_global_plan_index == global_plan_tf_.size() - 1) { + if (global_plan_index == global_plan_tf_.size() - 1) { const auto closest_pose = closestPoseOnSegment( - current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], - global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); + current_tf2, global_plan_tf_[global_plan_index - 1], global_plan_tf_[global_plan_index], + estimate_pose_angle_); const auto distance_to_goal = - sqrt(distSquared(global_plan_tf_[controller_state.current_global_plan_index], closest_pose)); + sqrt(distSquared(global_plan_tf_[global_plan_index], closest_pose)); - return { - closest_pose, controller_state.current_global_plan_index - 1, distance_to_goal, - global_plan_tf_.size() - 2}; + return {closest_pose, global_plan_index - 1, distance_to_goal, global_plan_tf_.size() - 2}; } const auto closest_pose_ahead = closestPoseOnSegment( - current_tf2, global_plan_tf_[controller_state.current_global_plan_index], - global_plan_tf_[controller_state.current_global_plan_index + 1], estimate_pose_angle_); + current_tf2, global_plan_tf_[global_plan_index], global_plan_tf_[global_plan_index + 1], + estimate_pose_angle_); const auto closest_pose_behind = closestPoseOnSegment( - current_tf2, global_plan_tf_[controller_state.current_global_plan_index - 1], - global_plan_tf_[controller_state.current_global_plan_index], estimate_pose_angle_); + current_tf2, global_plan_tf_[global_plan_index - 1], global_plan_tf_[global_plan_index], + estimate_pose_angle_); if ( distSquared(current_tf2, closest_pose_ahead) < distSquared(current_tf2, closest_pose_behind)) { const auto distance_to_goal = - distance_to_goal_vector_[controller_state.current_global_plan_index + 1] + - sqrt(distSquared( - global_plan_tf_[controller_state.current_global_plan_index + 1], closest_pose_ahead)); + distance_to_goal_vector_[global_plan_index + 1] + + sqrt(distSquared(global_plan_tf_[global_plan_index + 1], closest_pose_ahead)); - return { - closest_pose_ahead, controller_state.current_global_plan_index, distance_to_goal, - controller_state.current_global_plan_index}; + return {closest_pose_ahead, global_plan_index, distance_to_goal, global_plan_index}; } const auto distance_to_goal = - distance_to_goal_vector_[controller_state.current_global_plan_index] + - sqrt(distSquared( - global_plan_tf_[controller_state.current_global_plan_index], closest_pose_behind)); + distance_to_goal_vector_[global_plan_index] + + sqrt(distSquared(global_plan_tf_[global_plan_index], closest_pose_behind)); - return { - closest_pose_behind, controller_state.current_global_plan_index, distance_to_goal, - controller_state.current_global_plan_index - 1}; + return {closest_pose_behind, global_plan_index, distance_to_goal, global_plan_index - 1}; } Controller::UpdateResult Controller::update( @@ -365,7 +355,8 @@ Controller::UpdateResult Controller::update( current_with_carrot_ = getControlPointPose(current_tf, config_.l); const auto & reference_pose = config_.track_base_link ? current_tf : current_with_carrot_; - const auto find_result = findPositionOnPlan(reference_pose, controller_state_); + const auto find_result = + findPositionOnPlan(reference_pose, controller_state_.current_global_plan_index); const auto & path_pose_idx = find_result.path_pose_idx; const auto & distance_to_goal = find_result.distance_to_goal; diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 97ccf202..738167f8 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -318,8 +318,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() double max_abs_x_vel = std::abs(x_vel) > std::abs(target_x_vel) ? x_vel : target_x_vel; x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, max_abs_x_vel), 0.0, 0.0)); - // Use a controller state to forward project the position on the path - auto projected_controller_state = pid_controller_.getControllerState(); + // Keep track of the projected position on the path. + auto projected_global_plan_index = pid_controller_.getControllerState().current_global_plan_index; // Step until lookahead is reached, for every step project the pose back to the path std::vector step_points; @@ -328,12 +328,12 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() auto projected_step_tf = tf2_convert(tfCurPoseStamped_.transform); projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link projected_step_tf = - pid_controller_.findPositionOnPlan(projected_step_tf, projected_controller_state).position; + pid_controller_.findPositionOnPlan(projected_step_tf, projected_global_plan_index).position; projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose for (uint step = 0; step < n_steps; step++) { tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf; projected_step_tf = - pid_controller_.findPositionOnPlan(next_straight_step_tf, projected_controller_state) + pid_controller_.findPositionOnPlan(next_straight_step_tf, projected_global_plan_index) .position; projected_steps_tf.push_back(projected_step_tf); From cd11f02a3fd6456d95e4a4a464882c1ded67fefa Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Tue, 15 Mar 2022 15:43:42 +0000 Subject: [PATCH 146/156] Replaced Controller::getControllerState(). --- include/path_tracking_pid/controller.hpp | 58 ++++++++++++++---------- src/path_tracking_pid_local_planner.cpp | 15 +++--- 2 files changed, 42 insertions(+), 31 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 104b248a..3f3eaea8 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -25,27 +25,6 @@ struct TricycleSteeringCmdVel double acceleration = 0.0; }; -struct ControllerState -{ - size_t current_global_plan_index = 0; - double current_x_vel = 0.0; - double current_yaw_vel = 0.0; - double previous_steering_angle = 0.0; - double previous_steering_x_vel = 0.0; - double previous_steering_yaw_vel = 0.0; - bool end_phase_enabled = false; - bool end_reached = false; - double tracking_error_lat = 0.0; - double tracking_error_ang = 0.0; - // Errors with little history - details::SecondOrderLowpass error_lat; - details::SecondOrderLowpass error_ang; - details::Integral error_integral_lat; - details::Integral error_integral_ang; - details::Derivative error_deriv_lat; - details::Derivative error_deriv_ang; -}; - class Controller : private boost::noncopyable { public: @@ -183,8 +162,20 @@ class Controller : private boost::noncopyable tf2::Transform getCurrentWithCarrot() const { return current_with_carrot_; } tf2::Transform getCurrentPosOnPlan() const { return current_pos_on_plan_; } - // Inline get-function for controller-state - ControllerState getControllerState() const { return controller_state_; } + /** Get current forward velocity. */ + double getCurrentForwardVelocity() const { return controller_state_.current_x_vel; } + + /** Get current yaw velocity. */ + double getCurrentYawVelocity() const { return controller_state_.current_yaw_vel; } + + /** Indicates if the end is reached. */ + bool isEndReached() const { return controller_state_.end_reached; } + + /** Gets the current global plan index. */ + std::size_t getCurrentGlobalPlanIndex() const + { + return controller_state_.current_global_plan_index; + } // Set new vel_max_external value void setVelMaxExternal(double value); @@ -196,6 +187,27 @@ class Controller : private boost::noncopyable double getVelMaxObstacle() const; private: + struct ControllerState + { + size_t current_global_plan_index = 0; + double current_x_vel = 0.0; + double current_yaw_vel = 0.0; + double previous_steering_angle = 0.0; + double previous_steering_x_vel = 0.0; + double previous_steering_yaw_vel = 0.0; + bool end_phase_enabled = false; + bool end_reached = false; + double tracking_error_lat = 0.0; + double tracking_error_ang = 0.0; + // Errors with little history + details::SecondOrderLowpass error_lat; + details::SecondOrderLowpass error_ang; + details::Integral error_integral_lat; + details::Integral error_integral_ang; + details::Derivative error_deriv_lat; + details::Derivative error_deriv_ang; + }; + geometry_msgs::Twist computeTricycleModelForwardKinematics(double x_vel, double steering_angle); TricycleSteeringCmdVel computeTricycleModelInverseKinematics( const geometry_msgs::Twist & cmd_vel); diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 738167f8..e5f07b5a 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -159,12 +159,11 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector= 0.0 && - std::abs( - latest_odom_.twist.twist.linear.x - pid_controller_.getControllerState().current_x_vel) > + std::abs(latest_odom_.twist.twist.linear.x - pid_controller_.getCurrentForwardVelocity()) > pid_controller_.getConfig().init_vel_max_diff) { ROS_ERROR( "Significant diff between odom (%f) and controller_state (%f) detected. Aborting!", - latest_odom_.twist.twist.linear.x, pid_controller_.getControllerState().current_x_vel); + latest_odom_.twist.twist.linear.x, pid_controller_.getCurrentForwardVelocity()); return false; } @@ -220,8 +219,8 @@ std::optional TrackingPidLocalPlanner::computeVelocityComm ROS_ERROR_THROTTLE( 5, "dt=0 detected, skipping loop(s). Possible overloaded cpu or simulating too fast"); auto cmd_vel = geometry_msgs::Twist(); - cmd_vel.linear.x = pid_controller_.getControllerState().current_x_vel; - cmd_vel.angular.z = pid_controller_.getControllerState().current_yaw_vel; + cmd_vel.linear.x = pid_controller_.getCurrentForwardVelocity(); + cmd_vel.angular.z = pid_controller_.getCurrentYawVelocity(); // At the first call of computeVelocityCommands() we can't calculate a cmd_vel. We can't return // false because of https://github.com/magazino/move_base_flex/issues/195 so the current // velocity is send instead. @@ -304,7 +303,7 @@ std::optional TrackingPidLocalPlanner::computeVelocityComm uint8_t TrackingPidLocalPlanner::projectedCollisionCost() { // Check how far we should check forward - double x_vel = pid_controller_.getControllerState().current_x_vel; + double x_vel = pid_controller_.getCurrentForwardVelocity(); double collision_look_ahead_distance = x_vel * x_vel / (2 * pid_controller_.getConfig().target_x_decc) + pid_controller_.getConfig().collision_look_ahead_length_offset; @@ -319,7 +318,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() x_step_tf.setOrigin(tf2::Vector3(copysign(x_resolution, max_abs_x_vel), 0.0, 0.0)); // Keep track of the projected position on the path. - auto projected_global_plan_index = pid_controller_.getControllerState().current_global_plan_index; + auto projected_global_plan_index = pid_controller_.getCurrentGlobalPlanIndex(); // Step until lookahead is reached, for every step project the pose back to the path std::vector step_points; @@ -494,7 +493,7 @@ uint32_t TrackingPidLocalPlanner::computeVelocityCommands( bool TrackingPidLocalPlanner::isGoalReached() const { // Return reached boolean, but never succeed when we're preempting - return pid_controller_.getControllerState().end_reached && !cancel_in_progress_; + return pid_controller_.isEndReached() && !cancel_in_progress_; } bool TrackingPidLocalPlanner::isGoalReached( From a9db24df4ff2387d8a06de9f4993b9a9ef925545 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Thu, 17 Mar 2022 11:03:39 +0000 Subject: [PATCH 147/156] Renamed position to pose for findPositionOnPlan. --- include/path_tracking_pid/controller.hpp | 12 ++++++------ src/controller.cpp | 6 +++--- src/path_tracking_pid_local_planner.cpp | 5 ++--- 3 files changed, 11 insertions(+), 12 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 104b248a..bb60f94a 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -95,23 +95,23 @@ class Controller : private boost::noncopyable const geometry_msgs::Twist & steering_odom_twist, const std::vector & global_plan); - /** Result of findPositionOnPlan(). */ - struct FindPositionOnPlanResult + /** Result of findPoseOnPlan(). */ + struct FindPoseOnPlanResult { - tf2::Transform position; + tf2::Transform pose; std::size_t path_pose_idx = 0; double distance_to_goal = 0; std::size_t last_visited_pose_index = 0; }; /** - * Find position on plan by looking at the surroundings of last known pose. + * Find pose on plan by looking at the surroundings of last known pose. * @param[in] current_tf Where is the robot now? * @param[in,out] global_plan_index Global plan index where the search should start. Updated to * current global plan index once found. - * @return Found position on plan and related data. + * @return Found pose on plan and related data. */ - FindPositionOnPlanResult findPositionOnPlan( + FindPoseOnPlanResult findPoseOnPlan( const tf2::Transform & current_tf, std::size_t & global_plan_index) const; // Result of update() and update_with_limits(). diff --git a/src/controller.cpp b/src/controller.cpp index 18d83083..e4ba212d 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -251,7 +251,7 @@ bool Controller::setPlan( return result; } -Controller::FindPositionOnPlanResult Controller::findPositionOnPlan( +Controller::FindPoseOnPlanResult Controller::findPoseOnPlan( const tf2::Transform & current_tf, std::size_t & global_plan_index) const { auto current_tf2 = current_tf; @@ -356,11 +356,11 @@ Controller::UpdateResult Controller::update( const auto & reference_pose = config_.track_base_link ? current_tf : current_with_carrot_; const auto find_result = - findPositionOnPlan(reference_pose, controller_state_.current_global_plan_index); + findPoseOnPlan(reference_pose, controller_state_.current_global_plan_index); const auto & path_pose_idx = find_result.path_pose_idx; const auto & distance_to_goal = find_result.distance_to_goal; - current_pos_on_plan_ = current_goal_ = find_result.position; + current_pos_on_plan_ = current_goal_ = find_result.pose; if (config_.track_base_link) { current_goal_ = getControlPointPose(current_goal_, config_.l); diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 738167f8..54b64c34 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -328,13 +328,12 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() auto projected_step_tf = tf2_convert(tfCurPoseStamped_.transform); projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link projected_step_tf = - pid_controller_.findPositionOnPlan(projected_step_tf, projected_global_plan_index).position; + pid_controller_.findPoseOnPlan(projected_step_tf, projected_global_plan_index).pose; projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose for (uint step = 0; step < n_steps; step++) { tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf; projected_step_tf = - pid_controller_.findPositionOnPlan(next_straight_step_tf, projected_global_plan_index) - .position; + pid_controller_.findPoseOnPlan(next_straight_step_tf, projected_global_plan_index).pose; projected_steps_tf.push_back(projected_step_tf); // Fill markers: From e10e6b0c22a834d7c7d34f360f9dd7e6586e686c Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Mon, 21 Mar 2022 17:19:59 +0100 Subject: [PATCH 148/156] Move getControlPointPose to a separate function & test --- src/calculations.cpp | 11 ++++++++++- src/calculations.hpp | 10 +++++++++- src/controller.cpp | 17 ----------------- test/unittests/test_calculations.cpp | 19 ++++++++++++++++++- 4 files changed, 37 insertions(+), 20 deletions(-) diff --git a/src/calculations.cpp b/src/calculations.cpp index 9aa369b1..8978325d 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -9,7 +9,6 @@ namespace path_tracking_pid { - std::vector deltas_of_plan(const std::vector & plan) { auto result = std::vector{}; @@ -115,4 +114,14 @@ tf2::Transform closestPoseOnSegment( return result; } +tf2::Transform getControlPointPose(const tf2::Transform & pose, double control_distance) +{ + const auto theda_rp = tf2::getYaw(pose.getRotation()); + const auto origin = tf2::Vector3{ + pose.getOrigin().x() + control_distance * cos(theda_rp), + pose.getOrigin().y() + control_distance * sin(theda_rp), 0}; + + return tf2::Transform{pose.getRotation(), origin}; +} + } // namespace path_tracking_pid diff --git a/src/calculations.hpp b/src/calculations.hpp index 4d1ced22..29235402 100644 --- a/src/calculations.hpp +++ b/src/calculations.hpp @@ -6,7 +6,6 @@ namespace path_tracking_pid { - /** * Determine the deltas between consecutive poses in the given plan. Each delta is the inverse of a * pose times the next pose. If the plan contains fewer than 2 poses, the output is empty. @@ -73,4 +72,13 @@ tf2::Transform closestPoseOnSegment( const tf2::Transform & point, const tf2::Transform & segment_start, const tf2::Transform & segment_end, bool estimate_pose_angle); +/** + * Determine the control point pose based on the given pose and control distance. + * + * @param[in] pose Pose to transform. + * @param[in] control_distance Control distance to use. + * @return Control point pose. + */ +tf2::Transform getControlPointPose(const tf2::Transform & pose, double control_distance); + } // namespace path_tracking_pid diff --git a/src/controller.cpp b/src/controller.cpp index e4ba212d..0466ff29 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -60,23 +60,6 @@ bool check_plan(const std::vector & plan) return true; } -/** - * Determine the control point pose based on the given pose and control distance. - * - * @param[in] pose Pose to transform. - * @param[in] control_distance Control distance to use. - * @return Control point pose. - */ -tf2::Transform getControlPointPose(const tf2::Transform & pose, double control_distance) -{ - const auto theda_rp = tf2::getYaw(pose.getRotation()); - const auto origin = tf2::Vector3{ - pose.getOrigin().x() + control_distance * cos(theda_rp), - pose.getOrigin().y() + control_distance * sin(theda_rp), 0}; - - return tf2::Transform{pose.getRotation(), origin}; -} - } // namespace void Controller::setHolonomic(bool holonomic) diff --git a/test/unittests/test_calculations.cpp b/test/unittests/test_calculations.cpp index 3850d4c3..c845da7f 100644 --- a/test/unittests/test_calculations.cpp +++ b/test/unittests/test_calculations.cpp @@ -8,14 +8,16 @@ namespace { - using path_tracking_pid::closestPoseOnSegment; using path_tracking_pid::deltas_of_plan; using path_tracking_pid::distances_to_goal; using path_tracking_pid::distSquared; +using path_tracking_pid::getControlPointPose; using path_tracking_pid::inverse_turning_radiuses; using path_tracking_pid::is_in_direction_of_target; +constexpr auto eps = 1e-6; + // Create a transform (with an identity basis) based on the given coordinates. tf2::Transform create_transform(double x, double y, double z) { @@ -302,4 +304,19 @@ TEST(PathTrackingPidCalculations, ClosestPoseOnSegment_EstimateAngle) } } +TEST(PathTrackingPidCalculations, getControlPointPose) +{ + auto pose = tf2::Transform{create_quaternion(0, 0, M_PI_4), tf2::Vector3{2, 1, 0}}; + auto result = getControlPointPose(pose, M_SQRT2); + auto ref = tf2::Transform(create_quaternion(0, 0, M_PI_4), {3, 2, 0}); + + EXPECT_EQ(ref.getOrigin(), result.getOrigin()); + // allow for small differences in the basis because of rounding errors in the calculations + for (auto r = 0; r < 3; ++r) { + for (auto c = 0; c < 3; ++c) { + EXPECT_NEAR(ref.getBasis()[r][c], result.getBasis()[r][c], eps); + } + } +} + } // namespace From e3bd43c5252c45c61da35efcfa51e4a6b7f1651b Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Mon, 21 Mar 2022 17:22:15 +0100 Subject: [PATCH 149/156] Simplify closestPoseOnSegment math --- src/calculations.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/calculations.cpp b/src/calculations.cpp index 8978325d..991f1d89 100644 --- a/src/calculations.cpp +++ b/src/calculations.cpp @@ -116,12 +116,7 @@ tf2::Transform closestPoseOnSegment( tf2::Transform getControlPointPose(const tf2::Transform & pose, double control_distance) { - const auto theda_rp = tf2::getYaw(pose.getRotation()); - const auto origin = tf2::Vector3{ - pose.getOrigin().x() + control_distance * cos(theda_rp), - pose.getOrigin().y() + control_distance * sin(theda_rp), 0}; - - return tf2::Transform{pose.getRotation(), origin}; + return tf2::Transform{pose.getBasis(), pose * tf2::Vector3{control_distance, 0, 0}}; } } // namespace path_tracking_pid From d5198be0f9e95d06275aa933bdd46df2a7dc4389 Mon Sep 17 00:00:00 2001 From: Paul Verhoeckx Date: Tue, 22 Mar 2022 13:48:03 +0100 Subject: [PATCH 150/156] fix: Use absolute target velocity to calculate end phase distance --- src/controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index e4ba212d..56bbc3f5 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -451,12 +451,12 @@ Controller::UpdateResult Controller::update( t_end_phase_current = (target_end_x_vel - current_x_vel) / (-config_.target_x_decc); d_end_phase = current_x_vel * t_end_phase_current - 0.5 * (config_.target_x_decc) * t_end_phase_current * t_end_phase_current + - target_x_vel * 2.0 * dt.toSec(); + fabs(target_x_vel) * 2.0 * dt.toSec(); } else { t_end_phase_current = (target_end_x_vel - current_x_vel) / (config_.target_x_acc); d_end_phase = current_x_vel * t_end_phase_current + 0.5 * (config_.target_x_acc) * t_end_phase_current * t_end_phase_current + - target_x_vel * 2.0 * dt.toSec(); + fabs(target_x_vel) * 2.0 * dt.toSec(); } ROS_DEBUG("t_end_phase_current: %f", t_end_phase_current); ROS_DEBUG("d_end_phase: %f", d_end_phase); From ec82e90f140a0864769321783a77c26d0c0308eb Mon Sep 17 00:00:00 2001 From: Paul Verhoeckx Date: Tue, 22 Mar 2022 15:54:53 +0100 Subject: [PATCH 151/156] fix: Enforce positive end phase time and simplify end phase distance --- src/controller.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 56bbc3f5..75047f4c 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -448,16 +448,12 @@ Controller::UpdateResult Controller::update( if ( (current_target_x_vel_ > 0.0 && current_x_vel > target_end_x_vel) || (current_target_x_vel_ < 0.0 && current_x_vel < target_end_x_vel)) { - t_end_phase_current = (target_end_x_vel - current_x_vel) / (-config_.target_x_decc); - d_end_phase = current_x_vel * t_end_phase_current - - 0.5 * (config_.target_x_decc) * t_end_phase_current * t_end_phase_current + - fabs(target_x_vel) * 2.0 * dt.toSec(); + t_end_phase_current = fabs((target_end_x_vel - current_x_vel) / config_.target_x_decc); } else { - t_end_phase_current = (target_end_x_vel - current_x_vel) / (config_.target_x_acc); - d_end_phase = current_x_vel * t_end_phase_current + - 0.5 * (config_.target_x_acc) * t_end_phase_current * t_end_phase_current + - fabs(target_x_vel) * 2.0 * dt.toSec(); + t_end_phase_current = fabs((target_end_x_vel - current_x_vel) / config_.target_x_acc); } + d_end_phase = (current_x_vel + target_end_x_vel) * 0.5 * t_end_phase_current + + target_x_vel * 2.0 * dt.toSec(); ROS_DEBUG("t_end_phase_current: %f", t_end_phase_current); ROS_DEBUG("d_end_phase: %f", d_end_phase); ROS_DEBUG("distance_to_goal: %f", distance_to_goal); From 4d51759cbfaf6532b04555f5c2e5c8602a88d633 Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Thu, 24 Mar 2022 15:33:14 +0100 Subject: [PATCH 152/156] Refactor projectedCollisionCost to input-output function --- .../path_tracking_pid_local_planner.hpp | 5 +++- src/path_tracking_pid_local_planner.cpp | 29 +++++++++++++------ 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index fbc0a63e..f19729ab 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -112,7 +112,10 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, void velMaxExternalCallback(const std_msgs::Float64 & msg); - uint8_t projectedCollisionCost(); + std::vector projectionSteps(); + + uint8_t projectedCollisionCost( + const std::vector & projected_steps, std::unique_ptr & viz) const; nav_msgs::Odometry latest_odom_; ros::Time prev_time_; diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 8f825f96..5bdd4e4c 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -32,7 +32,7 @@ constexpr double DT_MAX = 1.5; /** * Convert the plan from geometry message format to tf2 format. - * + * * @param[in] plan Plan to convert. * @return Converted plan. */ @@ -241,7 +241,7 @@ std::optional TrackingPidLocalPlanner::computeVelocityComm // Handle obstacles if (pid_controller_.getConfig().anti_collision) { - auto cost = projectedCollisionCost(); + auto cost = projectedCollisionCost(projectionSteps(), visualization_); if (cost >= costmap_2d::LETHAL_OBSTACLE) { pid_controller_.setVelMaxObstacle(0.0); @@ -300,7 +300,7 @@ std::optional TrackingPidLocalPlanner::computeVelocityComm return update_result.velocity_command; } -uint8_t TrackingPidLocalPlanner::projectedCollisionCost() +std::vector TrackingPidLocalPlanner::projectionSteps() { // Check how far we should check forward double x_vel = pid_controller_.getCurrentForwardVelocity(); @@ -340,12 +340,25 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() poses_on_path_points.push_back(projected_step_tf.getOrigin()); } + // Visualize + std_msgs::Header header; + header.stamp = ros::Time::now(); + header.frame_id = map_frame_; + visualization_->publishExtrapolatedPoses(header, step_points); + visualization_->publishgGoalPosesOnPath(header, poses_on_path_points); + + return projected_steps_tf; +} + +uint8_t TrackingPidLocalPlanner::projectedCollisionCost( + const std::vector & projected_steps, std::unique_ptr & viz) const +{ costmap_2d::Costmap2D * costmap2d = costmap_->getCostmap(); std::vector collision_footprint_points; polygon_t previous_footprint_xy; polygon_t collision_polygon; uint8_t max_projected_step_cost = 0; - for (const auto & projection_tf : projected_steps_tf) { + for (const auto & projection_tf : projected_steps) { // Project footprint forward double x = projection_tf.getOrigin().x(); double y = projection_tf.getOrigin().y(); @@ -435,11 +448,9 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost() std_msgs::Header header; header.stamp = ros::Time::now(); header.frame_id = map_frame_; - visualization_->publishCollisionObject(header, max_cost, collision_point); - visualization_->publishExtrapolatedPoses(header, step_points); - visualization_->publishgGoalPosesOnPath(header, poses_on_path_points); - visualization_->publishCollisionFootprint(header, collision_footprint_points); - visualization_->publishCollisionPolygon(header, collision_hull_points); + viz->publishCollisionObject(header, max_cost, collision_point); + viz->publishCollisionFootprint(header, collision_footprint_points); + viz->publishCollisionPolygon(header, collision_hull_points); return max_projected_step_cost; } From 1512fbc7750a40946d51baa865fc99ef1af269ef Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Thu, 24 Mar 2022 16:26:53 +0100 Subject: [PATCH 153/156] Make projectedCollisionCost static --- .../path_tracking_pid_local_planner.hpp | 19 +++++++++++++++-- src/path_tracking_pid_local_planner.cpp | 21 +++++++++++-------- 2 files changed, 29 insertions(+), 11 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index f19729ab..249b6bc3 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -112,10 +112,25 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, void velMaxExternalCallback(const std_msgs::Float64 & msg); + /** + * @brief Project an amount of steps in the direction of movement based on velocity + * @return Projected steps + */ std::vector projectionSteps(); - uint8_t projectedCollisionCost( - const std::vector & projected_steps, std::unique_ptr & viz) const; + /** + * @brief Projects the footprint along the projected steps and determines maximum cost in that area + * @param costmap2d + * @param footprint + * @param projected_steps + * @param viz Used for marker publishing + * @param viz_frame Used for marker publishing + * @return Maximum cost + */ + static uint8_t projectedCollisionCost( + costmap_2d::Costmap2D * costmap2d, const std::vector & footprint, + const std::vector & projected_steps, std::unique_ptr & viz, + const std::string viz_frame); nav_msgs::Odometry latest_odom_; ros::Time prev_time_; diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 5bdd4e4c..d3aafd1f 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -241,7 +241,9 @@ std::optional TrackingPidLocalPlanner::computeVelocityComm // Handle obstacles if (pid_controller_.getConfig().anti_collision) { - auto cost = projectedCollisionCost(projectionSteps(), visualization_); + const std::vector footprint = costmap_->getRobotFootprint(); + auto cost = projectedCollisionCost( + costmap_->getCostmap(), footprint, projectionSteps(), visualization_, map_frame_); if (cost >= costmap_2d::LETHAL_OBSTACLE) { pid_controller_.setVelMaxObstacle(0.0); @@ -351,9 +353,10 @@ std::vector TrackingPidLocalPlanner::projectionSteps() } uint8_t TrackingPidLocalPlanner::projectedCollisionCost( - const std::vector & projected_steps, std::unique_ptr & viz) const + costmap_2d::Costmap2D * costmap2d, const std::vector & footprint, + const std::vector & projected_steps, std::unique_ptr & viz, + const std::string viz_frame) { - costmap_2d::Costmap2D * costmap2d = costmap_->getCostmap(); std::vector collision_footprint_points; polygon_t previous_footprint_xy; polygon_t collision_polygon; @@ -373,13 +376,13 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost( } // Project footprint forward - std::vector footprint; - costmap_2d::transformFootprint(x, y, yaw, costmap_->getRobotFootprint(), footprint); + std::vector footprint_proj; + costmap_2d::transformFootprint(x, y, yaw, footprint, footprint_proj); // Append footprint to polygon polygon_t two_footprints = previous_footprint_xy; previous_footprint_xy.clear(); - for (const auto & point : footprint) { + for (const auto & point : footprint_proj) { boost::geometry::append(two_footprints, point); boost::geometry::append(previous_footprint_xy, point); } @@ -390,8 +393,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost( collision_polygon = union_(collision_polygon, two_footprint_hull); // Add footprint to marker - geometry_msgs::Point previous_point = footprint.back(); - for (const auto & point : footprint) { + geometry_msgs::Point previous_point = footprint_proj.back(); + for (const auto & point : footprint_proj) { collision_footprint_points.push_back(tf2_convert(previous_point)); collision_footprint_points.push_back(tf2_convert(point)); previous_point = point; @@ -447,7 +450,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost( } std_msgs::Header header; header.stamp = ros::Time::now(); - header.frame_id = map_frame_; + header.frame_id = viz_frame; viz->publishCollisionObject(header, max_cost, collision_point); viz->publishCollisionFootprint(header, collision_footprint_points); viz->publishCollisionPolygon(header, collision_hull_points); From 28d90cc579acddc0e37a2a3dd19ec753b356a6b9 Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Fri, 25 Mar 2022 07:14:16 +0100 Subject: [PATCH 154/156] Factored out creation of projected footprint --- .../path_tracking_pid_local_planner.hpp | 15 +++++- src/path_tracking_pid_local_planner.cpp | 51 ++++++++++++------- 2 files changed, 48 insertions(+), 18 deletions(-) diff --git a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp index 249b6bc3..ef4087c6 100644 --- a/include/path_tracking_pid/path_tracking_pid_local_planner.hpp +++ b/include/path_tracking_pid/path_tracking_pid_local_planner.hpp @@ -29,7 +29,7 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, private boost::noncopyable { private: - typedef boost::geometry::model::ring polygon_t; + using polygon_t = boost::geometry::model::ring; static inline polygon_t union_(const polygon_t & polygon1, const polygon_t & polygon2) { @@ -118,6 +118,19 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController, */ std::vector projectionSteps(); + /** + * @brief Expand the footprint with the projected steps + * @param footprint + * @param projected_steps + * @param viz Used for marker publishing + * @param viz_frame Used for marker publishing + * @return Projected footprint + */ + static polygon_t projectionFootprint( + const std::vector & footprint, + const std::vector & projected_steps, std::unique_ptr & viz, + const std::string viz_frame); + /** * @brief Projects the footprint along the projected steps and determines maximum cost in that area * @param costmap2d diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index d3aafd1f..49a48dbe 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -352,29 +352,20 @@ std::vector TrackingPidLocalPlanner::projectionSteps() return projected_steps_tf; } -uint8_t TrackingPidLocalPlanner::projectedCollisionCost( - costmap_2d::Costmap2D * costmap2d, const std::vector & footprint, +boost::geometry::model::ring TrackingPidLocalPlanner::projectionFootprint( + const std::vector & footprint, const std::vector & projected_steps, std::unique_ptr & viz, const std::string viz_frame) { - std::vector collision_footprint_points; + std::vector projected_footprint_points; polygon_t previous_footprint_xy; - polygon_t collision_polygon; - uint8_t max_projected_step_cost = 0; + polygon_t projected_polygon; for (const auto & projection_tf : projected_steps) { // Project footprint forward double x = projection_tf.getOrigin().x(); double y = projection_tf.getOrigin().y(); double yaw = tf2::getYaw(projection_tf.getRotation()); - // Calculate cost by checking base link location in costmap - int map_x, map_y; - costmap2d->worldToMapEnforceBounds(x, y, map_x, map_y); - uint8_t projected_step_cost = costmap2d->getCost(map_x, map_y); - if (projected_step_cost > max_projected_step_cost) { - max_projected_step_cost = projected_step_cost; - } - // Project footprint forward std::vector footprint_proj; costmap_2d::transformFootprint(x, y, yaw, footprint, footprint_proj); @@ -390,17 +381,44 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost( boost::geometry::correct(two_footprints); polygon_t two_footprint_hull; boost::geometry::convex_hull(two_footprints, two_footprint_hull); - collision_polygon = union_(collision_polygon, two_footprint_hull); + projected_polygon = union_(projected_polygon, two_footprint_hull); // Add footprint to marker geometry_msgs::Point previous_point = footprint_proj.back(); for (const auto & point : footprint_proj) { - collision_footprint_points.push_back(tf2_convert(previous_point)); - collision_footprint_points.push_back(tf2_convert(point)); + projected_footprint_points.push_back(tf2_convert(previous_point)); + projected_footprint_points.push_back(tf2_convert(point)); previous_point = point; } } + std_msgs::Header header; + header.stamp = ros::Time::now(); + header.frame_id = viz_frame; + viz->publishCollisionFootprint(header, projected_footprint_points); + + return projected_polygon; +} + +uint8_t TrackingPidLocalPlanner::projectedCollisionCost( + costmap_2d::Costmap2D * costmap2d, const std::vector & footprint, + const std::vector & projected_steps, std::unique_ptr & viz, + const std::string viz_frame) +{ + auto collision_polygon = projectionFootprint(footprint, projected_steps, viz, viz_frame); + + // Calculate cost by checking base link location in costmap + uint8_t max_projected_step_cost = 0; + for (const auto & projection_tf : projected_steps) { + int map_x, map_y; + costmap2d->worldToMapEnforceBounds( + projection_tf.getOrigin().x(), projection_tf.getOrigin().y(), map_x, map_y); + uint8_t projected_step_cost = costmap2d->getCost(map_x, map_y); + if (projected_step_cost > max_projected_step_cost) { + max_projected_step_cost = projected_step_cost; + } + } + // Create a convex hull so we can use costmap2d->convexFillCells polygon_t collision_polygon_hull; boost::geometry::convex_hull(collision_polygon, collision_polygon_hull); @@ -452,7 +470,6 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost( header.stamp = ros::Time::now(); header.frame_id = viz_frame; viz->publishCollisionObject(header, max_cost, collision_point); - viz->publishCollisionFootprint(header, collision_footprint_points); viz->publishCollisionPolygon(header, collision_hull_points); return max_projected_step_cost; From 826bd16ca88eb93f2e0eb9b63fc54d21e8639798 Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Mon, 7 Mar 2022 09:51:45 +0000 Subject: [PATCH 155/156] 2.19.0 ros-dynamic_reconfigure 1.7.2 ros-path_tracking_pid 2.19.0 ff653a6 ros-roslint 0.12.0 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 99e2b721..ed4f7197 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ path_tracking_pid - 2.18.0 + 2.19.0 Follows a trajectory with open-loop speed and closed loop (pid) lateral control Cesar Lopez From c0e598d970f2ccd1e6813024918d66babff5044c Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Fri, 6 Jan 2023 11:39:13 +0100 Subject: [PATCH 156/156] Relicense the code under Apache-2.0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The old gpl license might make us vulnerable to oudside contributers, as discussed in the tech share meeting. Let's relicense under Apache-2.0 to also address some potential patent related issues. Relicensing requires the consent of all copyright holders. I’ve tracked down all contributions, and they were all done by people working at Nobleo. This means Nobleo is the sole copyright holder. --- LICENSE | 876 ++++++++++++---------------------------------------- package.xml | 2 +- 2 files changed, 203 insertions(+), 675 deletions(-) diff --git a/LICENSE b/LICENSE index f288702d..03c0b94e 100644 --- a/LICENSE +++ b/LICENSE @@ -1,674 +1,202 @@ - GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2023 Nobleo Technology B.V. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/package.xml b/package.xml index ed4f7197..553668e9 100644 --- a/package.xml +++ b/package.xml @@ -9,7 +9,7 @@ Cesar Lopez Michiel Francke - TBD + Apache-2.0 catkin message_generation