├── .github └── workflows │ └── workflow.yaml ├── CMakeLists.txt ├── Dock.stl ├── README.md ├── config └── autodock_params.yaml ├── include └── lidar_auto_docking │ ├── autodock.h │ ├── controller.h │ ├── dock_candidate.h │ ├── icp_2d.h │ ├── laser_processor.h │ ├── linear_pose_filter_2d.h │ ├── perception.h │ └── tf2listener.h ├── initial_dock_pose └── test.txt ├── launch ├── autodock_launch.py ├── dockpose_saver_launch.py ├── dockrobot_launch.py ├── nav2_dock.launch.py └── undock_robot_launch.py ├── lidar_auto_docking └── __init__.py ├── package.xml ├── scripts ├── dock_robot.py ├── dock_saver.py ├── nav2_dock.py └── undock_robot.py └── src ├── autodock.cpp ├── controller.cpp ├── dock_coordinates.cpp ├── icp_2d.cpp ├── laser_processor.cpp ├── linear_pose_filter_2d.cpp ├── perception.cpp └── tf2listener.cpp /.github/workflows/workflow.yaml: -------------------------------------------------------------------------------- 1 | name: Docking_Package_Test 2 | 3 | on: 4 | pull_request: 5 | push: 6 | 7 | 8 | jobs: 9 | build-and-test: 10 | runs-on: ${{ matrix.os }} 11 | strategy: 12 | matrix: 13 | os: [ubuntu-latest] 14 | ros_distribution: 15 | - foxy 16 | fail-fast: false 17 | steps: 18 | - name: Setup ROS 2 19 | uses: ros-tooling/setup-ros@v0.2 20 | with: 21 | required-ros-distributions: foxy 22 | - name: Run Tests 23 | uses: ros-tooling/action-ros-ci@v0.2 24 | with: 25 | package-name: lidar_auto_docking 26 | target-ros2-distro: foxy 27 | 28 | - name: Upload Logs 29 | uses: actions/upload-artifact@v1 30 | with: 31 | name: colcon-logs 32 | path: ros_ws/log 33 | if: always() 34 | 35 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(lidar_auto_docking) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(ament_cmake_python REQUIRED) 21 | # uncomment the following section in order to fill in 22 | # further dependencies manually. 23 | find_package(geometry_msgs REQUIRED) 24 | find_package(tf2 REQUIRED) 25 | find_package(tf2_ros REQUIRED) 26 | find_package(tf2_geometry_msgs REQUIRED) 27 | find_package(angles REQUIRED) 28 | find_package(sensor_msgs REQUIRED) 29 | find_package(Eigen3 REQUIRED) 30 | find_package(rclcpp REQUIRED) 31 | find_package(rclpy REQUIRED) 32 | find_package(std_msgs REQUIRED) 33 | find_package(nav_msgs REQUIRED) 34 | find_package(nav2_msgs REQUIRED) 35 | find_package(lidar_auto_docking_messages REQUIRED) 36 | find_package(rclcpp_action REQUIRED) 37 | 38 | 39 | 40 | add_executable(dock_coordinates src/dock_coordinates.cpp src/perception.cpp src/laser_processor.cpp src/linear_pose_filter_2d.cpp src/icp_2d.cpp src/tf2listener.cpp) 41 | ament_target_dependencies(dock_coordinates geometry_msgs tf2 sensor_msgs rclcpp Eigen3 angles tf2_ros tf2_geometry_msgs lidar_auto_docking_messages) 42 | #allow target to access header files defined in include 43 | target_include_directories( dock_coordinates 44 | PUBLIC 45 | $ 46 | $) 47 | 48 | 49 | 50 | add_executable(auto_dock src/autodock.cpp src/controller.cpp src/perception.cpp src/laser_processor.cpp src/linear_pose_filter_2d.cpp src/icp_2d.cpp src/tf2listener.cpp) 51 | ament_target_dependencies(auto_dock nav_msgs geometry_msgs tf2 sensor_msgs rclcpp rclcpp_action Eigen3 angles tf2_ros tf2_geometry_msgs lidar_auto_docking_messages) 52 | target_include_directories( auto_dock 53 | PUBLIC 54 | $ 55 | $) 56 | 57 | 58 | ament_python_install_package(${PROJECT_NAME}) 59 | # Install Python executables 60 | install(PROGRAMS 61 | scripts/dock_robot.py 62 | scripts/undock_robot.py 63 | scripts/dock_saver.py 64 | scripts/nav2_dock.py 65 | DESTINATION lib/${PROJECT_NAME} 66 | ) 67 | 68 | install( 69 | DIRECTORY include/ 70 | DESTINATION include 71 | ) 72 | 73 | install( 74 | DIRECTORY 75 | config 76 | launch 77 | initial_dock_pose 78 | DESTINATION share/${PROJECT_NAME} 79 | ) 80 | 81 | 82 | 83 | 84 | 85 | 86 | install(TARGETS 87 | auto_dock 88 | dock_coordinates 89 | DESTINATION lib/${PROJECT_NAME}) 90 | 91 | 92 | if(BUILD_TESTING) 93 | find_package(ament_lint_auto REQUIRED) 94 | # the following line skips the linter which checks for copyrights 95 | # uncomment the line when a copyright and license is not present in all source files 96 | set(ament_cmake_copyright_FOUND TRUE) 97 | # the following line skips cpplint (only works in a git repo) 98 | # uncomment the line when this package is not in a git repo 99 | #set(ament_cmake_cpplint_FOUND TRUE) 100 | ament_lint_auto_find_test_dependencies() 101 | endif() 102 | 103 | ament_package() 104 | -------------------------------------------------------------------------------- /Dock.stl: -------------------------------------------------------------------------------- 1 | solid ASCII 2 | facet normal 7.071068e-01 0.000000e+00 -7.071068e-01 3 | outer loop 4 | vertex -7.071000e+01 1.500000e+03 -7.071000e+01 5 | vertex 0.000000e+00 1.500000e+03 0.000000e+00 6 | vertex -7.071000e+01 0.000000e+00 -7.071000e+01 7 | endloop 8 | endfacet 9 | facet normal 7.071068e-01 0.000000e+00 -7.071068e-01 10 | outer loop 11 | vertex -7.071000e+01 0.000000e+00 -7.071000e+01 12 | vertex 0.000000e+00 1.500000e+03 0.000000e+00 13 | vertex 0.000000e+00 0.000000e+00 0.000000e+00 14 | endloop 15 | endfacet 16 | facet normal -7.071068e-01 -1.387218e-18 -7.071068e-01 17 | outer loop 18 | vertex -4.414200e+02 1.500000e+03 7.105427e-14 19 | vertex -3.707100e+02 1.500000e+03 -7.071000e+01 20 | vertex -4.414200e+02 0.000000e+00 7.399701e-14 21 | endloop 22 | endfacet 23 | facet normal -7.071068e-01 0.000000e+00 -7.071068e-01 24 | outer loop 25 | vertex -4.414200e+02 0.000000e+00 7.399701e-14 26 | vertex -3.707100e+02 1.500000e+03 -7.071000e+01 27 | vertex -3.707100e+02 0.000000e+00 -7.071000e+01 28 | endloop 29 | endfacet 30 | facet normal -0.000000e+00 -1.000000e+00 0.000000e+00 31 | outer loop 32 | vertex 0.000000e+00 0.000000e+00 0.000000e+00 33 | vertex -4.414200e+02 0.000000e+00 7.399701e-14 34 | vertex -7.071000e+01 0.000000e+00 -7.071000e+01 35 | endloop 36 | endfacet 37 | facet normal 0.000000e+00 -1.000000e+00 0.000000e+00 38 | outer loop 39 | vertex -7.071000e+01 0.000000e+00 -7.071000e+01 40 | vertex -4.414200e+02 0.000000e+00 7.399701e-14 41 | vertex -3.707100e+02 0.000000e+00 -7.071000e+01 42 | endloop 43 | endfacet 44 | facet normal 0.000000e+00 0.000000e+00 -1.000000e+00 45 | outer loop 46 | vertex -7.071000e+01 0.000000e+00 -7.071000e+01 47 | vertex -3.707100e+02 0.000000e+00 -7.071000e+01 48 | vertex -7.071000e+01 1.500000e+03 -7.071000e+01 49 | endloop 50 | endfacet 51 | facet normal -0.000000e+00 0.000000e+00 -1.000000e+00 52 | outer loop 53 | vertex -7.071000e+01 1.500000e+03 -7.071000e+01 54 | vertex -3.707100e+02 0.000000e+00 -7.071000e+01 55 | vertex -3.707100e+02 1.500000e+03 -7.071000e+01 56 | endloop 57 | endfacet 58 | facet normal 1.676340e-16 1.961822e-18 1.000000e+00 59 | outer loop 60 | vertex -4.414200e+02 0.000000e+00 7.399701e-14 61 | vertex 0.000000e+00 0.000000e+00 0.000000e+00 62 | vertex -4.414200e+02 1.500000e+03 7.105427e-14 63 | endloop 64 | endfacet 65 | facet normal 1.609675e-16 0.000000e+00 1.000000e+00 66 | outer loop 67 | vertex -4.414200e+02 1.500000e+03 7.105427e-14 68 | vertex 0.000000e+00 0.000000e+00 0.000000e+00 69 | vertex 0.000000e+00 1.500000e+03 0.000000e+00 70 | endloop 71 | endfacet 72 | facet normal 0.000000e+00 1.000000e+00 0.000000e+00 73 | outer loop 74 | vertex 0.000000e+00 1.500000e+03 0.000000e+00 75 | vertex -7.071000e+01 1.500000e+03 -7.071000e+01 76 | vertex -4.414200e+02 1.500000e+03 7.105427e-14 77 | endloop 78 | endfacet 79 | facet normal 0.000000e+00 1.000000e+00 0.000000e+00 80 | outer loop 81 | vertex -4.414200e+02 1.500000e+03 7.105427e-14 82 | vertex -7.071000e+01 1.500000e+03 -7.071000e+01 83 | vertex -3.707100e+02 1.500000e+03 -7.071000e+01 84 | endloop 85 | endfacet 86 | endsolid 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # lidar_auto_docking 2 | The lidar_auto_docking package is a heavily refactored version of the fetch_open_auto_dock package. 3 | The package would send appropriate cmd_vel messages to bring the robot to a dock with a trapezium-shaped landmark. 4 | The distance between dock and base_link 5 | is used to determine if the robot has reached the dock.
6 | There are two versions of this package. ROS2 Foxy on the (foxy-main) branch, ROS2 Humble on the (humble-main) branch. Do take note that the humble version of this package requires a separate package for messages, whereas the foxy version does not. 7 | 8 | ## Prerequisites 9 | This package would require the nav2 stack to be set-up on your robot.
10 | 11 | This package requires a trapezium-shaped landmark with the following dimensions:
12 | 1. The front face of the trapezium is 300mm long
13 | 2. Each side of the trapezium is 100mm long at a 45 degree angle
14 | ![2d dock](https://lh6.googleusercontent.com/hKKNImeuQS3SKO0GsVIWoCwcAshb19PlKeyQyippt9KKMChIYAWHXclFADV_T-i8tX8Mb5gN9x-TTPmVKu4plnbSJ5fVldQgFbaYW0w1__4KQ0NGzd4OBlNoq9-WCSL15Ao01HHg) 15 | 16 | You can create this trapezium out of any material you desire. 17 | Of course, the trapezium should be placed in the scan plane of the lidar.
18 | 19 | ## Subscribed Topics 20 | 21 | 1. /scan (laserscan topic) 22 | 2. /tf (transformation from map->odom->base_link) 23 | 3. /joy (joystick topic. Used to reset the chosen dock by the dock saver program) 24 | 25 | ## Published Topics 26 | 1. /autodock/cmd_vel (topic used to send velocity commands to the robot.) 27 | 28 | ## Installation 29 | 1. git clone the following messages package into your src folder: 30 | ``` 31 | git clone https://github.com/SynapseProgramming/lidar_auto_docking_messages.git 32 | ``` 33 | 2. Next, git clone this package into your colcon workspace and then run colcon build. 34 | If there are some dependencies that are not met, please run rosdep before building.
35 | 36 | ## Usage 37 | 38 | 1. Firstly, run nav2 on your robot. Place the robot about 1 metre in front of the dock. 39 | 40 | ![robot infront dock](https://lh6.googleusercontent.com/8Bhh3YMV-MircatDYsx8YG8mphGAk7jwq_btky8D_jP0gS2d4w302htnVr6KYWOZFiSRBV4eat1G4qggBHFO2356E8PyecWl6l5oJp2Vzma1c5vQQwapfKBD_2fvummDPAaZR79n) 41 | 42 | 2. Next, open a new terminal and launch: 43 | ``` 44 | ros2 launch lidar_auto_docking dockpose_saver_launch.py 45 | ``` 46 | ![detected dock](https://lh5.googleusercontent.com/f_thEFUo0MaHhae_t5IQLtsMoXNCjN9nMtkIbg_wU8xAvI1kamIzzChXznGhzMZI8_L_NPUZtnj03LMXFwShTe0XpiP9SK_x1RdUcfJ0qZEreRBf03iVrSkOiVlTLHBx4mFphJbJ) 47 | 48 | Once the robot has detected a suitable dock candidate, the dock frame would appear in rviz2.
49 | 50 | ![gui window](https://lh6.googleusercontent.com/Tm8OtZGG0-LW-NvTaaJr61TdrMGYs7cF6ZYNBUHqJh7zc_2xwIRk9vG3_ZrP7_6fyCdxO5V1CBZcA_EPJO_ksADJ9CUGiI7FqUfRM6gdxbCHZmV8ZJiJg0PEnaDlSBwaam0P6XXX) 51 | 52 | Furthermore, a GUI window would pop up. The GUI would display the absolute distance(in metres) between base_link and the dock frame. 53 | You may wish to use this tool to decide a suitable distance for the robot to stop at the dock.
54 | 55 | 3. Once you are satisfied with the selected dock, click on the save dock and bot pose button. This button would save the dock's position in the map frame. 56 | Also, the transformation between base_link and map would be saved.
57 | 58 | ![saved dock coord](https://lh5.googleusercontent.com/-xrFyCcjy1bGXcnQKNx6KXZ9sbuVeAdOU-31zJC7PZb5fE74XymiNMyQLMhU3CvuH4JY2ljEeD6sOnCSmmwGKgBWaEXLLvLBLCPYWFGjqPP0sdgRMQeift8254R7bPHKJ8_KnYif) 59 | 60 | 4. Next, please modify the config/autodock_params.yaml file and key in suitable parameters. 61 | Please run colcon build once suitable parameters have been keyed in.
62 | 63 | ### Docking 64 | 1. please position the robot about 1-1.5m in front of the dock and run the following command to send the robot to the dock. 65 | ``` 66 | ros2 launch lidar_auto_docking dockrobot_launch.py 67 | 68 | ``` 69 | The robot should begin to move towards the dock. 70 | The robot would stop once the distance threshold(docked_distance_threshold) between dock and base_link has been reached.
71 | 72 | ### Undocking 73 | 1. To undock the robot, please run the following command: 74 | ``` 75 | ros2 launch lidar_auto_docking undock_robot_launch.py 76 | ``` 77 | The robot would undock from the dock, and it should turn 180 degrees. 78 | 79 | ## Testing in simulation 80 | 81 | Before testing docking on real hardware, 82 | I strongly recommend testing this package in simulation first(gazebo etc.)
83 | 84 | 1. Firstly, place the Dock.stl file in the same folder as your .world file. 85 | 86 | 2. Next, you may wish to copy and paste this code and place it in the world tag. To add the dock into your world. 87 | 88 | ``` 89 | 90 | 91 | -1.0 -0.35 0 1.57 0 1.57 92 | true 93 | 94 | 95 | 96 | file://Dock.stl 97 | 0.001 0.001 0.001 98 | 99 | 100 | 101 | 102 | 103 | file://Dock.stl 104 | 0.001 0.001 0.001 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | ``` 113 | 114 | ## System Integration 115 | As the main docking program is an action server, you could write action clients to customise where and when you want the robot to engage in the docking/undocking sequence. 116 | 117 | If you wish, you could refer to dock_robot.py and undock_robot.py in the scripts folder as a reference. 118 | -------------------------------------------------------------------------------- /config/autodock_params.yaml: -------------------------------------------------------------------------------- 1 | docking_server: 2 | ros__parameters: 3 | #Number of times the robot gets to attempt 4 | retries: 6 5 | # Distance below which to check abort criteria. should be slightly above docked_distance_threshold (m) 6 | abort_distance: 0.37 7 | # Angle offset that triggers abort (radians) 8 | abort_angle: 0.10 9 | # Y-offset that triggers abort. (m) 10 | y_abort_threshold: 0.05 11 | # The distance to back-off in order to clear the dock connector 12 | connector_clearance_distance: 0.41 13 | # Threshold distance that indicates that the robot is docked. (m) 14 | docked_distance_threshold: 0.31 15 | 16 | undocking_server: 17 | ros__parameters: 18 | # The distance to back-off in order to clear the dock connector 19 | connector_clearance_distance: 0.42 20 | -------------------------------------------------------------------------------- /include/lidar_auto_docking/autodock.h: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_AUTO_DOCKING_AUTODOCK_H 2 | #define LIDAR_AUTO_DOCKING_AUTODOCK_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "lidar_auto_docking_messages/action/dock.hpp" 16 | #include "lidar_auto_docking_messages/action/undock.hpp" 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "rclcpp_action/rclcpp_action.hpp" 19 | using namespace std::chrono_literals; 20 | 21 | class DockingServer : public rclcpp::Node { 22 | public: 23 | using Dock = lidar_auto_docking_messages::action::Dock; 24 | using GoalHandleDock = rclcpp_action::ServerGoalHandle; 25 | explicit DockingServer( 26 | const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) 27 | : Node("docking_server", options) { 28 | using namespace std::placeholders; 29 | // get parameters from yaml file 30 | 31 | this->declare_parameter("retries", 5); 32 | this->get_parameter("retries", NUM_OF_RETRIES_); 33 | 34 | this->declare_parameter("abort_distance", 0.32); 35 | this->get_parameter("abort_distance", abort_distance_); 36 | 37 | this->declare_parameter("abort_angle", 0.090); 38 | this->get_parameter("abort_angle", abort_angle_); 39 | 40 | this->declare_parameter("y_abort_threshold", 0.04); 41 | this->get_parameter("y_abort_threshold", abort_threshold_); 42 | 43 | this->declare_parameter("connector_clearance_distance", 0.4); 44 | this->get_parameter("connector_clearance_distance", 45 | DOCK_CONNECTOR_CLEARANCE_DISTANCE_); 46 | 47 | this->declare_parameter("docked_distance_threshold", 0.30); 48 | this->get_parameter("docked_distance_threshold", 49 | DOCKED_DISTANCE_THRESHOLD_); 50 | // initialise the action server object 51 | // the string is the action topic 52 | this->action_server_ = rclcpp_action::create_server( 53 | this->get_node_base_interface(), this->get_node_clock_interface(), 54 | this->get_node_logging_interface(), 55 | this->get_node_waitables_interface(), "Dock", 56 | std::bind(&DockingServer::handle_goal, this, _1, _2), 57 | std::bind(&DockingServer::handle_cancel, this, _1), 58 | std::bind(&DockingServer::handle_accepted, this, _1)); 59 | } 60 | 61 | // init_objects function creates instances of helper classes. 62 | void init_objects(); 63 | 64 | // shared_ptr_from_this would return a shared pointer of the current class 65 | std::shared_ptr shared_ptr_from_this(); 66 | 67 | private: 68 | rclcpp_action::Server::SharedPtr action_server_; 69 | rclcpp_action::GoalResponse handle_goal( 70 | const rclcpp_action::GoalUUID& uuid, 71 | std::shared_ptr goal); 72 | 73 | rclcpp_action::CancelResponse handle_cancel( 74 | const std::shared_ptr goal_handle); 75 | 76 | // function which is called to spin a new thread to run execute function 77 | void handle_accepted(const std::shared_ptr goal_handle); 78 | 79 | /** 80 | * @brief Method sets the docking deadline and number of retries. 81 | */ 82 | void initDockTimeout(); 83 | 84 | /** 85 | * @brief Method checks to see if we have run out of time or retries. 86 | * @return True if we are out of time or tries. 87 | */ 88 | bool isDockingTimedOut(); 89 | 90 | /** 91 | * @brief Method that checks success or failure of docking. 92 | * @param result Dock result message used to set the dock action server state. 93 | * @return True if we have neither succeeded nor failed to dock. 94 | */ 95 | bool continueDocking(std::shared_ptr result, 96 | const std::shared_ptr goal_handle); 97 | 98 | /** 99 | * @brief Method to compute the distance the robot should backup when 100 | * attemping a docking correction. Method uses a number of state variables in 101 | * the class to compute distance. TODO(enhancement): Should these be 102 | * parameterized instead? 103 | * @return Distance for robot to backup in meters. 104 | */ 105 | double backupDistance(); 106 | 107 | // method to reverse the robot when the abort flag is set. 108 | void executeBackupSequence(rclcpp::Rate& r); 109 | 110 | /** 111 | * @brief Method to check approach abort conditions. If we are close to the 112 | * dock but the robot is too far off side-to-side or at a bad angle, it should 113 | * abort. Method also returns through the parameter the orientation of 114 | * the dock wrt the robot for use in correction behaviors. 115 | * @param dock_yaw Yaw angle of the dock wrt the robot in radians. 116 | * @return True if the robot should abort the approach. 117 | */ 118 | bool isApproachBad(double& dock_yaw); 119 | 120 | // main function which is called when a goal is received 121 | void execute(const std::shared_ptr goal_handle); 122 | 123 | // Configuration Constants. 124 | int NUM_OF_RETRIES_; // Number of times the robot gets to attempt 125 | double DOCK_CONNECTOR_CLEARANCE_DISTANCE_; // The amount to back off in order 126 | // to clear the dock connector. 127 | double DOCKED_DISTANCE_THRESHOLD_; // Threshold distance that indicates 128 | // that the robot might be docked 129 | 130 | std::shared_ptr perception_; 131 | std::shared_ptr controller_; 132 | // determine if charging 133 | bool charging_; 134 | 135 | // Failure detection 136 | double abort_distance_; // Distance below which to check abort criteria. 137 | double abort_threshold_; // Y-offset that triggers abort. 138 | double abort_angle_; // Angle offset that triggers abort. 139 | double correction_angle_; // Yaw correction angle the robot should use to 140 | // line up with the dock. 141 | double 142 | backup_limit_; // Maximum distance the robot will backup when trying to 143 | // retry. Based on range of initial dock pose estimate. 144 | bool aborting_; // If the robot realizes it won't be sucessful, it needs to 145 | // abort. 146 | int num_of_retries_; // The number of times the robot gets to abort before 147 | // failing. This variable will count down. 148 | bool cancel_docking_; // Signal that docking has failed and the action 149 | // server should abort the goal. 150 | rclcpp::Time deadline_docking_; // Time when the docking times out. 151 | rclcpp::Time deadline_not_charging_; // Time when robot gives up on the 152 | // charge 153 | // state and retries docking. 154 | bool charging_timeout_set_; // Flag to indicate if the 155 | // deadline_not_charging has been set. 156 | }; // class DockingServer 157 | 158 | class UndockingServer : public rclcpp::Node { 159 | public: 160 | using Undock = lidar_auto_docking_messages::action::Undock; 161 | using GoalHandleUndock = rclcpp_action::ServerGoalHandle; 162 | 163 | explicit UndockingServer( 164 | const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) 165 | : Node("undocking_server", options) { 166 | using namespace std::placeholders; 167 | 168 | this->declare_parameter("connector_clearance_distance", 0.4); 169 | this->get_parameter("connector_clearance_distance", 170 | DOCK_CONNECTOR_CLEARANCE_DISTANCE_); 171 | 172 | // initialise the action server object 173 | // the string is the action topic 174 | this->undock_action_server_ = rclcpp_action::create_server( 175 | this->get_node_base_interface(), this->get_node_clock_interface(), 176 | this->get_node_logging_interface(), 177 | this->get_node_waitables_interface(), "Undock", 178 | std::bind(&UndockingServer::handle_goal, this, _1, _2), 179 | std::bind(&UndockingServer::handle_cancel, this, _1), 180 | std::bind(&UndockingServer::handle_accepted, this, _1)); 181 | } 182 | 183 | // init_objects function creates instances of helper classes. 184 | void init_objects(); 185 | 186 | // shared_ptr_from_this would return a shared pointer of the current class 187 | std::shared_ptr shared_ptr_from_this(); 188 | 189 | private: 190 | std::shared_ptr controller_; 191 | rclcpp_action::Server::SharedPtr undock_action_server_; 192 | 193 | rclcpp_action::GoalResponse handle_goal( 194 | const rclcpp_action::GoalUUID& uuid, 195 | std::shared_ptr goal); 196 | 197 | rclcpp_action::CancelResponse handle_cancel( 198 | const std::shared_ptr goal_handle); 199 | 200 | // function which is called to spin a new thread to run execute function 201 | void handle_accepted(const std::shared_ptr goal_handle); 202 | 203 | // main function which is called when a goal is received 204 | void execute(const std::shared_ptr goal_handle); 205 | 206 | double DOCK_CONNECTOR_CLEARANCE_DISTANCE_; // The amount to back off in order 207 | // to clear the dock 208 | }; 209 | 210 | #endif 211 | -------------------------------------------------------------------------------- /include/lidar_auto_docking/controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fetch Robotics Inc 3 | * Author: Michael Ferguson 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU Lesser General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #ifndef LIDAR_AUTO_DOCKING_CONTROLLER_H 20 | #define LIDAR_AUTO_DOCKING_CONTROLLER_H 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include "lidar_auto_docking/tf2listener.h" 28 | #include "rclcpp/rclcpp.hpp" 29 | 30 | class BaseController { 31 | public: 32 | explicit BaseController(std::shared_ptr node_ptr); 33 | 34 | /** 35 | * @brief Implements something loosely based on "A Smooth Control Law for 36 | * Graceful Motion of Differential Wheeled Mobile Robots in 2D Environments" 37 | * by Park and Kuipers, ICRA 2011 38 | * @returns true if base has reached goal. 39 | */ 40 | bool approach(const geometry_msgs::msg::PoseStamped& target); 41 | 42 | /** 43 | * @brief Back off dock, then rotate. Robot is first reversed by the 44 | * prescribed distance, and then rotates with respect to it's current 45 | * orientation. 46 | * @param distance Distance in meters to backup. 47 | * @param rotate_distance Amount of angle in radians for the robot to yaw. 48 | */ 49 | bool backup(double distance, double rotate_distance); 50 | 51 | /** 52 | * @brief Get the last command sent 53 | */ 54 | bool getCommand(geometry_msgs::msg::Twist& command); 55 | 56 | /** @brief send stop command to robot base */ 57 | void stop(); 58 | 59 | private: 60 | rclcpp::Publisher::SharedPtr cmd_vel_pub_; 61 | rclcpp::Publisher::SharedPtr path_pub_; 62 | 63 | tf2_listener listener_; 64 | geometry_msgs::msg::Twist command_; 65 | /* 66 | * Parameters for approach controller 67 | */ 68 | 69 | double k1_; // ratio in change of theta to rate of change in r 70 | double k2_; // speed at which we converge to slow system 71 | double min_velocity_; 72 | double max_velocity_; 73 | double max_angular_velocity_; 74 | double beta_; // how fast velocity drops as k increases 75 | double lambda_; // ?? 76 | double dist_; // used to create the tracking line 77 | 78 | /* 79 | * Parameters for backup controller 80 | */ 81 | 82 | geometry_msgs::msg::PoseStamped start_; 83 | bool ready_; 84 | bool turning_; 85 | }; 86 | 87 | #endif // FETCH_AUTO_DOCK_CONTROLLER_H 88 | -------------------------------------------------------------------------------- /include/lidar_auto_docking/dock_candidate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fetch Robotics Inc. 3 | * Author: Michael Ferguson, Sriramvarun Nidamarthy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU Lesser General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #ifndef LIDAR_AUTO_DOCKING_DOCK_CANDIDATE_H 20 | #define LIDAR_AUTO_DOCKING_DOCK_CANDIDATE_H 21 | 22 | #include 23 | #include 24 | 25 | #include "geometry_msgs/msg/point.hpp" 26 | 27 | /** 28 | * @brief A cluster which is a candidate for a dock 29 | */ 30 | struct DockCandidate { 31 | std::vector points; 32 | double dist; // distance from initial/previous pose 33 | 34 | /** @brief Get the width of this segment */ 35 | double width() { 36 | // If there are no points then there is no width. 37 | if (points.empty()) { 38 | return 0; 39 | } 40 | 41 | geometry_msgs::msg::Point& pt1 = points.front(); 42 | geometry_msgs::msg::Point& pt2 = points.back(); 43 | return ( 44 | std::sqrt(std::pow((pt1.x - pt2.x), 2) + std::pow((pt1.y - pt2.y), 2))); 45 | } 46 | 47 | /** 48 | * @brief Determine if this candidate meets our basic criteria 49 | * @param dock_found Has the dock been found in a previous frame? 50 | */ 51 | bool valid(bool dock_found) { 52 | // If there are no points this cannot be valid. 53 | if (points.empty()) { 54 | return false; 55 | } 56 | 57 | // Check overall size 58 | if (width() > 0.5 || width() < 0.25) return false; 59 | 60 | // If dock is found, we want to avoid large jumps 61 | if (dock_found) { 62 | // dist is squared 63 | return dist < (0.25 * 0.25); 64 | } 65 | 66 | // Not too far off from initial pose 67 | return dist < 1.0; 68 | } 69 | }; 70 | typedef std::shared_ptr DockCandidatePtr; 71 | 72 | struct CompareCandidates { 73 | bool operator()(DockCandidatePtr a, DockCandidatePtr b) { 74 | return (a->dist > b->dist); 75 | } 76 | }; 77 | 78 | #endif // FETCH_AUTO_DOCK_DOCK_CANDIDATE_H 79 | -------------------------------------------------------------------------------- /include/lidar_auto_docking/icp_2d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fetch Robotics Inc. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #ifndef LIDAR_AUTO_DOCKING_ICP_2D_H 19 | #define LIDAR_AUTO_DOCKING_ICP_2D_H 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace icp_2d { 26 | 27 | /** 28 | * @brief Get the 2d rotation from a quaternion. 29 | * @param q The quaternion to convert. 30 | * @return The orientation in 2d. 31 | * 32 | * Note: will throw if q.x or q.y is not equal to zero 33 | */ 34 | double thetaFromQuaternion(const geometry_msgs::msg::Quaternion& q); 35 | 36 | /** 37 | * @brief Transform a vector of points in 2d. 38 | * @param points The points to transform. 39 | * @param x The x offset to transform, in the current frame of points. 40 | * @param y The y offset to transform, in the current frame of points. 41 | * @param theta The rotation, in the current frame of points. 42 | * @return The transformed points. 43 | */ 44 | 45 | std::vector transform( 46 | const std::vector& points, double x, double y, 47 | double theta); 48 | 49 | /** 50 | * @brief Get the centroid of a set of points. 51 | * @param points The points to find centroid of. 52 | */ 53 | geometry_msgs::msg::Point getCentroid( 54 | const std::vector points); 55 | 56 | /** 57 | * @brief Perform PCA algorithm to align two point clouds in 58 | * a two dimensional plane. 59 | * @param source The cloud to be aligned with target. 60 | * @param target The cloud to be aligned to. 61 | * @param transform The transformation to align source with target. 62 | * @return True if successful, false otherwise. 63 | */ 64 | bool alignPCA(const std::vector source, 65 | const std::vector target, 66 | geometry_msgs::msg::Transform& transform); 67 | 68 | /** 69 | * @brief Perform SVD optimization to align two point clouds 70 | * in a two dimensional plane. 71 | * @param source The cloud to be aligned with target. 72 | * @param target The cloud to be aligned to. 73 | * @param transform The transformation to align source with target. 74 | * @return True if successful, false otherwise. 75 | */ 76 | bool alignSVD(const std::vector source, 77 | const std::vector target, 78 | geometry_msgs::msg::Transform& transform); 79 | 80 | /** 81 | * @brief Perform Iterative Closest Point (ICP) algorithm to 82 | * align two point clouds in a two dimensional plane. 83 | * @param source The cloud to be aligned with target. 84 | * @param target The cloud to be aligned to. 85 | * @param transform The transformation to align source with target. 86 | * @return Fitness score, negative if error. 87 | */ 88 | 89 | double alignICP(const std::vector source, 90 | const std::vector target, 91 | geometry_msgs::msg::Transform& transform, 92 | size_t max_iterations = 10, double min_delta_rmsd = 0.000001); 93 | 94 | } // namespace icp_2d 95 | 96 | #endif // FETCH_AUTO_DOCK_ICP_2D_H 97 | -------------------------------------------------------------------------------- /include/lidar_auto_docking/laser_processor.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef LIDAR_AUTO_DOCKING_LASER_PROCESSOR_H 36 | #define LIDAR_AUTO_DOCKING_LASER_PROCESSOR_H 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include "geometry_msgs/msg/point.hpp" 49 | #include "sensor_msgs/msg/laser_scan.hpp" 50 | #include "sensor_msgs/msg/point_cloud.hpp" 51 | #include "tf2/LinearMath/Vector3.h" 52 | #include "tf2/transform_datatypes.h" 53 | 54 | namespace laser_processor { 55 | /// A struct representing a single sample from the laser. 56 | class Sample { 57 | public: 58 | int index; 59 | float range; 60 | float intensity; 61 | float x; 62 | float y; 63 | 64 | static Sample* Extract(int ind, const sensor_msgs::msg::LaserScan& scan); 65 | 66 | private: 67 | Sample(){}; 68 | }; 69 | 70 | /// The comparator allowing the creation of an ordered "SampleSet" 71 | struct CompareSample { 72 | CompareSample() {} 73 | 74 | inline bool operator()(const Sample* a, const Sample* b) const { 75 | return (a->index < b->index); 76 | } 77 | }; 78 | 79 | /// An ordered set of Samples 80 | class SampleSet : public std::set { 81 | public: 82 | SampleSet() {} 83 | 84 | ~SampleSet() { clear(); } 85 | 86 | void clear(); 87 | 88 | void appendToCloud(sensor_msgs::msg::PointCloud& cloud, int r = 0, int g = 0, 89 | int b = 0); 90 | // TODO: center is now returned as a Vector3 91 | tf2::Vector3 center(); 92 | std_msgs::msg::Header header; 93 | }; 94 | 95 | /// A mask for filtering out Samples based on range 96 | class ScanMask { 97 | SampleSet mask_; 98 | 99 | bool filled; 100 | float angle_min; 101 | float angle_max; 102 | uint32_t size; 103 | 104 | public: 105 | ScanMask() : filled(false), angle_min(0), angle_max(0), size(0) {} 106 | 107 | inline void clear() { 108 | mask_.clear(); 109 | filled = false; 110 | } 111 | 112 | void addScan(sensor_msgs::msg::LaserScan& scan); 113 | 114 | bool hasSample(Sample* s, float thresh); 115 | }; 116 | 117 | typedef SampleSet* SampleSetPtr; 118 | typedef SampleSet* SampleSetConstPtr; 119 | 120 | class ScanProcessor { 121 | std::list clusters_; 122 | sensor_msgs::msg::LaserScan scan_; 123 | 124 | public: 125 | std::list& getClusters() { return clusters_; } 126 | 127 | ScanProcessor(const sensor_msgs::msg::LaserScan& scan, ScanMask& mask_, 128 | float mask_threshold = 0.03); 129 | 130 | ~ScanProcessor(); 131 | 132 | void removeLessThan(uint32_t num); 133 | 134 | void splitConnected(float thresh); 135 | }; 136 | 137 | } // namespace laser_processor 138 | 139 | #endif // FETCH_AUTO_DOCK_LASER_PROCESSOR_H 140 | -------------------------------------------------------------------------------- /include/lidar_auto_docking/linear_pose_filter_2d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fetch Robotics Inc. 3 | * Author: Griswald Brooks 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU Lesser General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #ifndef LIDAR_AUTO_DOCKING_LINEAR_POSE_FILTER_H 20 | #define LIDAR_AUTO_DOCKING_LINEAR_POSE_FILTER_H 21 | 22 | // STL includes. 23 | #include 24 | #include 25 | 26 | // ROS includes. 27 | #include 28 | 29 | class LinearPoseFilter2D { 30 | public: 31 | /** 32 | * @brief Constructor for LinearPoseFilter2D. 33 | * @param b Filter coefficients for the input samples. 34 | * The number of coefficients is the order of the filter + 1. 35 | * @param a Filter coefficients for the output samples. 36 | * Vector a should have the same number of elements as b, 37 | * with the first element being 1. (OK) 38 | */ 39 | LinearPoseFilter2D(const std::vector& b, const std::vector& a); 40 | 41 | /** 42 | * @brief Method to set the filter coefficients. 43 | * @param b Filter coefficients for the input samples. 44 | * The number of coefficients is the order of the filter + 1. 45 | * @param a Filter coefficients for the output samples. 46 | * Vector a should have the same number of elements as b, 47 | * with the first element being 1. (OK) 48 | */ 49 | void setCoeff(const std::vector& b, const std::vector& a); 50 | 51 | /** 52 | * @brief Method to filter pose objects. Filter only filters position.x, 53 | * position.y and the yaw of the pose. 54 | * @param pose Input pose to be filtered. 55 | * @return Filtered pose. 56 | */ // (OK) 57 | geometry_msgs::msg::Pose filter(const geometry_msgs::msg::Pose& pose); 58 | 59 | /** 60 | * @brief Method to reset the filter to ensure that if the filter is being 61 | * reused, it is not corrupted by old values. Sets sample histories to the 62 | * origin. 63 | */ //(OK) 64 | void reset(); 65 | 66 | /** 67 | * @brief Method to set the sample history of the filter to some state. 68 | * All of the sample histories will be set to the parameterized values. 69 | * @param input_pose The initial input pose that will be set for all previous 70 | * inputs. 71 | * @param output_pose The initial output pose that will be set for all 72 | * previous outputs. 73 | *///(OK) 74 | void setFilterState(const geometry_msgs::msg::Pose& input_pose, 75 | const geometry_msgs::msg::Pose& output_pose); 76 | 77 | /** 78 | * @brief Method to set the sample history of the filter to some (set of) 79 | * state(s). new_poses[0] is the oldest sample and new_poses[new_poses.size() 80 | * - 1] is the newest sample. If the number of poses is greater than the 81 | * length of the filter memory, only the newest poses will be used. If the 82 | * number of poses is less than the length of the filter memory, the poses 83 | * will be copied to the most recent history and the original samples will 84 | * retain their original location in the buffer. Example: 85 | * // New poses. 86 | * np = [1, 2, 3] 87 | * // Original poses. 88 | * p = [9, 8, 7, 6, 5] 89 | * // Poses after setFilterState 90 | * p = [9, 8, 1, 2, 3] 91 | * @param input_poses Set of input poses to set the input state time history 92 | * to. 93 | * @param output_poses Set of output poses to set the output state time 94 | * history to. 95 | */ //(OK) 96 | void setFilterState( 97 | const std::vector& input_poses, 98 | const std::vector& output_poses); 99 | 100 | /** 101 | * @brief Not yet implemented but need to have methods that take only input 102 | * histories and then recreate the appropriate output histories. This is 103 | * because the input history encodes the entire filter history. 104 | */ 105 | // void setFilterState(const geometry_msgs::msg::Pose& input_pose); 106 | // void setFilterState(const std::vector& 107 | // input_poses); 108 | 109 | private: 110 | /** 111 | * @brief Method to get origin pose. 112 | * @return Pose with the linear pose set to the origin and the quaternion set 113 | * to Roll, Pitch, Yaw set to zero. 114 | */ 115 | geometry_msgs::msg::Pose originPose(); 116 | 117 | /** 118 | * @brief Method to return the unnormalized/wrapped-up yaw. 119 | * 120 | * This method adds shortest angular distance between the pose and the 121 | * reference yaw and returns it. Example: pose = -178 degrees, reference_yaw = 122 | * 177 degrees, return = 182 degrees. pose = 175 degrees, reference_yaw = 123 | * -170 degrees, return = -185 degrees. 124 | * @param pose The pose to be unnormalized. 125 | * @param reference_yaw The yaw with which to reference and whose sign is 126 | * respected. 127 | * @return Unnormalized yaw in radians. 128 | */ //(OK) 129 | float getUnnormalizedYaw(geometry_msgs::msg::Pose pose, float reference_yaw); 130 | 131 | /** 132 | * @brief Method to get the most recent filtered unnormalized yaw. 133 | * If the filter has never produced an output before, the zero will be 134 | * given. 135 | * @return Most recently filtered yaw. 136 | */ 137 | float getNewestOutputYaw(); 138 | 139 | std::deque 140 | out_; /// Vector of previous output poses. 141 | std::deque in_; /// Vector of previous input poses. 142 | std::deque 143 | yaw_out_; /// Vector of previous output yaws that aren't normalized. 144 | std::deque 145 | yaw_in_; /// Vector of previous input yaws that aren't normalized. 146 | std::vector b_; /// Vector of input coefficients. 147 | std::vector a_; /// Vector of output coefficients. 148 | }; 149 | 150 | typedef std::shared_ptr LinearPoseFilter2DPtr; 151 | 152 | #endif // LIDAR_AUTO_DOCK_LINEAR_POSE_FILTER_H 153 | -------------------------------------------------------------------------------- /include/lidar_auto_docking/perception.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fetch Robotics Inc. 3 | * Author: Michael Ferguson, Sriramvarun Nidamarthy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU Lesser General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #ifndef LIDAR_AUTO_DOCKING_PERCEPTION_H 20 | #define LIDAR_AUTO_DOCKING_PERCEPTION_H 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include "lidar_auto_docking/tf2listener.h" 44 | using std::placeholders::_1; 45 | 46 | class DockPerception { 47 | public: 48 | explicit DockPerception(std::shared_ptr node_ptr); 49 | 50 | /** 51 | * @brief Start dock detection. 52 | * @param pose The initial estimate of dock pose 53 | */ 54 | bool start(const geometry_msgs::msg::PoseStamped& pose); 55 | 56 | /** @brief Stop tracking the dock. */ 57 | bool stop(); 58 | 59 | /** @brief Get the pose of the dock. */ 60 | bool getPose(geometry_msgs::msg::PoseStamped& pose, std::string frame = ""); 61 | 62 | private: 63 | /** @brief Callback to process laser scans */ 64 | void callback(const sensor_msgs::msg::LaserScan::SharedPtr scan); 65 | 66 | /** 67 | * @brief Extract a DockCandidate from a cluster, filling in the 68 | * lengths and slopes of each line found using ransac. 69 | * @param cluster The pointer to the cluster to extract from. 70 | */ 71 | DockCandidatePtr extract(laser_processor::SampleSet* cluster); 72 | 73 | /** 74 | * @brief Try to fit a dock to candidate 75 | * @param candidate The candidate to fit to. 76 | * @param pose The fitted pose, if successful. 77 | * @returns Fitness score (>0 if successful) 78 | */ 79 | double fit(const DockCandidatePtr& candidate, geometry_msgs::msg::Pose& pose); 80 | /** 81 | * @brief Method to check if the quaternion is valid. 82 | * @param q Quaternion to check. 83 | * @return True if quaternion is valid. 84 | */ 85 | static bool isValid(const tf2::Quaternion& q); 86 | 87 | rclcpp::Subscription::SharedPtr scan_sub_; 88 | tf2_listener listener_; 89 | 90 | bool running_; // Should we be trying to find the dock 91 | bool debug_; // Should we output debugging info 92 | 93 | LinearPoseFilter2DPtr 94 | dock_pose_filter_; /// Low pass filter object for filtering dock poses. 95 | // TF frame to track dock within 96 | std::string tracking_frame_; 97 | // Best estimate of where the dock is 98 | geometry_msgs::msg::PoseStamped dock_; 99 | // Mutex on dock_ 100 | std::mutex dock_mutex_; 101 | // If true, then dock_ is based on actual sensor data 102 | bool found_dock_; 103 | 104 | // Last time that dock pose was updated 105 | rclcpp::Time dock_stamp_; 106 | // Maximum allowable error between scan and "ideal" scan 107 | double max_alignment_error_; 108 | // local version of node_ptr_ 109 | std::shared_ptr node_ptr_; 110 | 111 | // Publish visualization of which points are detected as dock 112 | rclcpp::Publisher::SharedPtr debug_points_; 113 | // The ideal cloud, located at origin of dock frame 114 | std::vector ideal_cloud_; 115 | // The ideal cloud (when only front is visible) 116 | std::vector front_cloud_; 117 | }; 118 | 119 | #endif // LIDAR_AUTO_DOCKING_PERCEPTION_H 120 | -------------------------------------------------------------------------------- /include/lidar_auto_docking/tf2listener.h: -------------------------------------------------------------------------------- 1 | #ifndef LIDAR_AUTO_DOCKING_TF2LISTENER_H 2 | #define LIDAR_AUTO_DOCKING_TF2LISTENER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | class tf2_listener { 19 | public: 20 | tf2_listener(rclcpp::Clock::SharedPtr clock) : buffer_(clock), rate(10) { 21 | tfl_ = std::make_shared(buffer_); 22 | } 23 | // would block code till the transform is received. 24 | void waitTransform(std::string origin, std::string destination); 25 | 26 | // would return the transfomation from origin to destination as a transform 27 | // stamped 28 | geometry_msgs::msg::TransformStamped getTransform(std::string origin, 29 | std::string destination); 30 | 31 | // would tranform the input pose to be with reference to the tracking frame. 32 | void transformPose(std::string tracking_frame, 33 | geometry_msgs::msg::PoseStamped &input_pose, 34 | geometry_msgs::msg::PoseStamped &output_pose); 35 | 36 | private: 37 | tf2_ros::Buffer buffer_; 38 | std::shared_ptr tfl_; 39 | rclcpp::Rate rate; 40 | }; 41 | 42 | #endif // TF2LISTENER_H 43 | -------------------------------------------------------------------------------- /initial_dock_pose/test.txt: -------------------------------------------------------------------------------- 1 | This file exists to enable the initial_dock_pose folder to be sent to github 2 | -------------------------------------------------------------------------------- /launch/autodock_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | 11 | 12 | def generate_launch_description(): 13 | 14 | current_dir = get_package_share_directory("lidar_auto_docking") 15 | ld = LaunchDescription() 16 | 17 | config = os.path.join(current_dir, "config", "autodock_params.yaml") 18 | 19 | run_autodock = Node( 20 | package="lidar_auto_docking", 21 | executable="auto_dock", 22 | parameters=[config], 23 | output="screen", 24 | ) 25 | ld.add_action(run_autodock) 26 | return ld 27 | -------------------------------------------------------------------------------- /launch/dockpose_saver_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | 10 | # goal file to load poses from 11 | dock_pose_file = "init_dock.json" 12 | 13 | load_file_path = os.path.join( 14 | get_package_share_directory("lidar_auto_docking"), 15 | "initial_dock_pose", 16 | dock_pose_file, 17 | ) 18 | run_dock_coordinates = Node( 19 | package="lidar_auto_docking", 20 | executable="dock_coordinates", 21 | name="dock_coordinates", 22 | parameters=[{"reset_goal_button": 1}], 23 | ) 24 | run_dock_saver = Node( 25 | package="lidar_auto_docking", 26 | executable="dock_saver.py", 27 | name="dock_saver", 28 | emulate_tty=True, 29 | parameters=[{"load_file_path": load_file_path}], 30 | output="screen", 31 | ) 32 | 33 | ld = LaunchDescription() 34 | ld.add_action(run_dock_saver) 35 | ld.add_action(run_dock_coordinates) 36 | 37 | return ld 38 | -------------------------------------------------------------------------------- /launch/dockrobot_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | 11 | 12 | def generate_launch_description(): 13 | 14 | current_dir = get_package_share_directory("lidar_auto_docking") 15 | ld = LaunchDescription() 16 | 17 | config = os.path.join(current_dir, "config", "autodock_params.yaml") 18 | 19 | # goal file to load poses from 20 | dock_pose_file = "init_dock.json" 21 | 22 | load_file_path = os.path.join( 23 | get_package_share_directory("lidar_auto_docking"), 24 | "initial_dock_pose", 25 | dock_pose_file, 26 | ) 27 | 28 | run_autodock_client = Node( 29 | package="lidar_auto_docking", 30 | executable="dock_robot.py", 31 | name="dock_bot", 32 | emulate_tty=True, 33 | parameters=[{"load_file_path": load_file_path}], 34 | output="screen", 35 | ) 36 | 37 | run_autodock = Node( 38 | package="lidar_auto_docking", 39 | executable="auto_dock", 40 | parameters=[config], 41 | output="screen", 42 | remappings=[("/cmd_vel", "/autodock/cmd_vel")], 43 | ) 44 | ld.add_action(run_autodock) 45 | ld.add_action(run_autodock_client) 46 | return ld 47 | -------------------------------------------------------------------------------- /launch/nav2_dock.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | 11 | 12 | def generate_launch_description(): 13 | 14 | current_dir = get_package_share_directory("lidar_auto_docking") 15 | ld = LaunchDescription() 16 | 17 | config = os.path.join(current_dir, "config", "autodock_params.yaml") 18 | 19 | # goal file to load poses from 20 | dock_pose_file = "init_dock.json" 21 | 22 | load_file_path = os.path.join( 23 | get_package_share_directory("lidar_auto_docking"), 24 | "initial_dock_pose", 25 | dock_pose_file, 26 | ) 27 | 28 | run_autodock_client = Node( 29 | package="lidar_auto_docking", 30 | executable="nav2_dock.py", 31 | name="nav2_dock", 32 | emulate_tty=True, 33 | parameters=[{"load_file_path": load_file_path}], 34 | output="screen", 35 | ) 36 | 37 | run_autodock = Node( 38 | package="lidar_auto_docking", 39 | executable="auto_dock", 40 | parameters=[config], 41 | output="screen", 42 | remappings=[("/cmd_vel", "/autodock/cmd_vel")], 43 | ) 44 | ld.add_action(run_autodock) 45 | ld.add_action(run_autodock_client) 46 | return ld 47 | -------------------------------------------------------------------------------- /launch/undock_robot_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch_ros.actions import Node 10 | 11 | 12 | def generate_launch_description(): 13 | 14 | current_dir = get_package_share_directory("lidar_auto_docking") 15 | ld = LaunchDescription() 16 | 17 | config = os.path.join(current_dir, "config", "autodock_params.yaml") 18 | 19 | run_autodock_client = Node( 20 | package="lidar_auto_docking", 21 | executable="undock_robot.py", 22 | name="undock_bot", 23 | emulate_tty=True, 24 | output="screen", 25 | ) 26 | 27 | run_autodock = Node( 28 | package="lidar_auto_docking", 29 | executable="auto_dock", 30 | parameters=[config], 31 | output="screen", 32 | remappings=[("/cmd_vel", "/autodock/cmd_vel")], 33 | ) 34 | ld.add_action(run_autodock) 35 | ld.add_action(run_autodock_client) 36 | return ld 37 | -------------------------------------------------------------------------------- /lidar_auto_docking/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SynapseProgramming/lidar_auto_docking/4755e841f40903841c587d908732279a0a5d648f/lidar_auto_docking/__init__.py -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lidar_auto_docking 5 | 0.0.0 6 | TODO: Package description 7 | ro 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | rclcpp 13 | rclpy 14 | rclcpp_action 15 | geometry_msgs 16 | tf2_geometry_msgs 17 | tf2 18 | lidar_auto_docking_messages 19 | action_msgs 20 | tf2_ros 21 | eigen 22 | nav_msgs 23 | angles 24 | sensor_msgs 25 | nav2_msgs 26 | std_msgs 27 | ament_lint_auto 28 | ament_lint_common 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /scripts/dock_robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | ... 3 | 4 | 5 | from action_msgs.msg import GoalStatus 6 | from lidar_auto_docking_messages.action import Dock 7 | 8 | import rclpy 9 | import json 10 | from rclpy.action import ActionClient 11 | from rclpy.node import Node 12 | 13 | 14 | class docking_client: 15 | def __init__(self, action_client): 16 | self._action_client = action_client 17 | self.goal_status = False 18 | self.goal_accept_status = False 19 | 20 | def goal_response_callback(self, future): 21 | goal_handle = future.result() 22 | if not goal_handle.accepted: 23 | print("goal rejected") 24 | self.goal_accept_status = False 25 | return 26 | 27 | print("goal accepted") 28 | self.goal_accept_status = True 29 | 30 | self._get_result_future = goal_handle.get_result_async() 31 | self._get_result_future.add_done_callback(self.get_result_callback) 32 | 33 | def get_result_callback(self, future): 34 | result = future.result().result 35 | status = future.result().status 36 | if status == GoalStatus.STATUS_SUCCEEDED: 37 | print("goal succeeded") 38 | self.goal_status = True 39 | else: 40 | print("goal failed ") 41 | self.goal_status = False 42 | 43 | def send_goal(self, dock_pose): 44 | print("waiting for action server") 45 | self._action_client.wait_for_server() 46 | goal_msg = Dock.Goal() 47 | goal_msg.dock_pose.pose.position.x = dock_pose["x"] 48 | goal_msg.dock_pose.pose.position.y = dock_pose["y"] 49 | goal_msg.dock_pose.pose.orientation.z = dock_pose["z"] 50 | goal_msg.dock_pose.pose.orientation.w = dock_pose["w"] 51 | goal_msg.dock_pose.header.frame_id = "map" 52 | # fill up the rest later 53 | print("Sending goal request") 54 | self._send_goal_future = self._action_client.send_goal_async(goal_msg) 55 | 56 | self._send_goal_future.add_done_callback(self.goal_response_callback) 57 | 58 | def get_status(self): 59 | status = {} 60 | status["gs"] = self.goal_status 61 | status["gas"] = self.goal_accept_status 62 | return status 63 | 64 | def reset_status(self): 65 | self.goal_status = False 66 | self.goal_accept_status = False 67 | 68 | 69 | class dock(Node): 70 | def __init__(self): 71 | super().__init__("minimal_action_client") 72 | self.docking_client_ = docking_client(ActionClient(self, Dock, "Dock")) 73 | self.declare_parameter("load_file_path") 74 | self.dock_file_path = ( 75 | self.get_parameter("load_file_path").get_parameter_value().string_value 76 | ) 77 | # load in dock coordinates 78 | with open(self.dock_file_path) as outfile: 79 | self.initial_dock_pose = json.load(outfile) 80 | 81 | def send_goal(self): 82 | self.docking_client_.send_goal(self.initial_dock_pose) 83 | 84 | 85 | def main(args=None): 86 | rclpy.init(args=args) 87 | 88 | action_client = dock() 89 | 90 | action_client.send_goal() 91 | 92 | rclpy.spin(action_client) 93 | 94 | action_client.destroy_node() 95 | rclpy.shutdown() 96 | 97 | 98 | if __name__ == "__main__": 99 | main() 100 | -------------------------------------------------------------------------------- /scripts/dock_saver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | ... 3 | 4 | import rclpy 5 | import tkinter 6 | from tkinter import messagebox 7 | from tkinter import Label 8 | import json 9 | from rclpy.node import Node 10 | 11 | from std_msgs.msg import String 12 | from lidar_auto_docking_messages.msg import Initdock 13 | import math 14 | from tf2_ros import LookupException 15 | from tf2_ros.buffer import Buffer 16 | from tf2_ros.transform_listener import TransformListener 17 | 18 | 19 | class gui(object): 20 | def __init__(self): 21 | 22 | self.top_ = tkinter.Tk() 23 | self.top_.geometry("600x500") 24 | 25 | def get_selected_goal(self): 26 | return self.selected_goal_.get() 27 | 28 | # callback_function is the address of the function to callback when the button is pressed. 29 | 30 | def create_button(self, posx, posy, button_name, callback_function, button_colour): 31 | self.send_goal_button_ = tkinter.Button( 32 | self.top_, text=button_name, command=callback_function, bg=button_colour 33 | ) 34 | self.send_goal_button_.place(x=posx, y=posy) 35 | 36 | def update_tk(self): 37 | self.top_.update_idletasks() 38 | self.top_.update() 39 | 40 | def show_message(self, msg): 41 | messagebox.showinfo(title="Save Status", message=msg) 42 | 43 | def show_variable(self, msg, posx, posy): 44 | self.label = Label(self.top_, text=str(msg)) 45 | self.label.place(x=posx, y=posy) 46 | 47 | 48 | class dock_pose_subscriber(Node): 49 | def __init__(self): 50 | super().__init__("dock_subscriber") 51 | self.subscription = self.create_subscription( 52 | Initdock, "init_dock", self.listener_callback, 10 53 | ) 54 | self.subscription # prevent unused variable warning 55 | self.declare_parameter("load_file_path","null") 56 | self.dock_file_path = ( 57 | self.get_parameter("load_file_path").get_parameter_value().string_value 58 | ) 59 | self._tf_buffer = Buffer() 60 | self._tf_listener = TransformListener(self._tf_buffer, self) 61 | # Try to get the transform every second. 62 | # If the transform is unavailable the timer callback will wait for it. 63 | self._output_timer = self.create_timer(0.1, self.on_timer) 64 | self.obj_gui_ = gui() 65 | self.x_pos = 0.0 66 | self.y_pos = 0.0 67 | self.obj_gui_.create_button( 68 | posx=250, 69 | posy=300, 70 | button_name="save dock and bot pose", 71 | button_colour="green", 72 | callback_function=self.save_dock_callback, 73 | ) 74 | 75 | async def on_timer(self): 76 | from_frame = "base_link" 77 | to_frame = "map" 78 | 79 | # Get latest available 80 | when = rclpy.time.Time() 81 | 82 | try: 83 | # Suspends callback until transform becomes available 84 | self.robot_pose = await self._tf_buffer.lookup_transform_async( 85 | to_frame, from_frame, when 86 | ) 87 | self.bot_x = self.robot_pose.transform.translation.x 88 | self.bot_y = self.robot_pose.transform.translation.y 89 | self.bot_z = self.robot_pose.transform.rotation.z 90 | self.bot_w = self.robot_pose.transform.rotation.w 91 | self.dock_x_diff = abs(self.bot_x - self.x_pos) 92 | self.dock_y_diff = abs(self.bot_y - self.y_pos) 93 | self.dist_to_dock = math.sqrt( 94 | self.dock_x_diff * self.dock_x_diff 95 | + self.dock_y_diff * self.dock_y_diff 96 | ) 97 | self.obj_gui_.show_variable( 98 | posx=250, posy=250, msg="Distance to dock: " + str(self.dist_to_dock) 99 | ) 100 | except LookupException as e: 101 | self.get_logger().error("failed to get transform {}".format(repr(e))) 102 | 103 | def save_dock_callback(self): 104 | print(self.x_pos) 105 | print(self.y_pos) 106 | print(self.z_pos) 107 | print(self.w_pos) 108 | output_dict = {} 109 | output_dict["x"] = self.x_pos 110 | output_dict["y"] = self.y_pos 111 | output_dict["z"] = self.z_pos 112 | output_dict["w"] = self.w_pos 113 | 114 | output_dict["bx"] = self.bot_x 115 | output_dict["by"] = self.bot_y 116 | output_dict["bz"] = self.bot_z 117 | output_dict["bw"] = self.bot_w 118 | with open( 119 | self.dock_file_path, 120 | "w", 121 | ) as outfile: 122 | json.dump(output_dict, outfile) 123 | self.obj_gui_.show_message("Dock and robot Coordinates have been saved!") 124 | 125 | def listener_callback(self, msg): 126 | # we should update our tkinter gui with the current dock coordinates here 127 | self.x_pos = msg.x 128 | self.z_pos = msg.z 129 | self.y_pos = msg.y 130 | self.w_pos = msg.w 131 | 132 | # refresh tkinter 133 | self.obj_gui_.update_tk() 134 | 135 | 136 | def main(args=None): 137 | rclpy.init(args=args) 138 | 139 | minimal_subscriber = dock_pose_subscriber() 140 | rclpy.spin(minimal_subscriber) 141 | 142 | minimal_subscriber.destroy_node() 143 | rclpy.shutdown() 144 | 145 | 146 | if __name__ == "__main__": 147 | main() 148 | -------------------------------------------------------------------------------- /scripts/nav2_dock.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from rclpy.node import Node 3 | from rclpy.action import ActionClient 4 | import rclpy 5 | import json 6 | from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy 7 | from nav2_msgs.action import NavigateToPose 8 | from lidar_auto_docking_messages.action import Undock 9 | from lidar_auto_docking_messages.action import Dock 10 | from std_msgs.msg import Int32 11 | from action_msgs.msg import GoalStatus 12 | ... 13 | 14 | 15 | class undocking_client: 16 | def __init__(self, action_client): 17 | self._action_client = action_client 18 | self.goal_status = False 19 | self.goal_accept_status = False 20 | self.failure_flag = False 21 | 22 | def goal_response_callback(self, future): 23 | goal_handle = future.result() 24 | if not goal_handle.accepted: 25 | print("Goal Rejected") 26 | self.goal_accept_status = False 27 | return 28 | 29 | print("Goal accepted") 30 | self.goal_accept_status = True 31 | 32 | self._get_result_future = goal_handle.get_result_async() 33 | self._get_result_future.add_done_callback(self.get_result_callback) 34 | 35 | def get_result_callback(self, future): 36 | 37 | result = future.result().result 38 | status = future.result().status 39 | if status == GoalStatus.STATUS_SUCCEEDED: 40 | print("Undocking Succeeded!") 41 | self.goal_status = True 42 | print(str(result.undocked)) 43 | else: 44 | print("Undocking Failed!") 45 | self.goal_status = False 46 | self.failure_flag = True 47 | 48 | def send_goal(self): 49 | print("Waiting for action server") 50 | self._action_client.wait_for_server() 51 | goal_msg = Undock.Goal() 52 | goal_msg.rotate_in_place = True 53 | print("Sending goal request") 54 | 55 | self._send_goal_future = self._action_client.send_goal_async(goal_msg) 56 | 57 | self._send_goal_future.add_done_callback(self.goal_response_callback) 58 | 59 | def get_status(self): 60 | status = {} 61 | status["gs"] = self.goal_status 62 | status["gas"] = self.goal_accept_status 63 | status["f_flag"] = self.failure_flag 64 | return status 65 | 66 | def reset_status(self): 67 | self.goal_status = False 68 | self.goal_accept_status = False 69 | self.failure_flag = False 70 | 71 | 72 | class docking_client: 73 | def __init__(self, action_client): 74 | self._action_client = action_client 75 | self.goal_status = False 76 | self.goal_accept_status = False 77 | self.failure_flag = False 78 | 79 | def goal_response_callback(self, future): 80 | goal_handle = future.result() 81 | if not goal_handle.accepted: 82 | print("goal rejected") 83 | self.goal_accept_status = False 84 | return 85 | 86 | print("goal accepted") 87 | self.goal_accept_status = True 88 | 89 | self._get_result_future = goal_handle.get_result_async() 90 | self._get_result_future.add_done_callback(self.get_result_callback) 91 | 92 | def get_result_callback(self, future): 93 | result = future.result().result 94 | status = future.result().status 95 | if status == GoalStatus.STATUS_SUCCEEDED: 96 | print("docking succeeded") 97 | self.goal_status = True 98 | else: 99 | print("docking failed ") 100 | self.goal_status = False 101 | self.failure_flag = True 102 | 103 | def send_goal(self, dock_pose): 104 | print("waiting for action server") 105 | self._action_client.wait_for_server() 106 | goal_msg = Dock.Goal() 107 | goal_msg.dock_pose.pose.position.x = dock_pose["x"] 108 | goal_msg.dock_pose.pose.position.y = dock_pose["y"] 109 | goal_msg.dock_pose.pose.orientation.z = dock_pose["z"] 110 | goal_msg.dock_pose.pose.orientation.w = dock_pose["w"] 111 | goal_msg.dock_pose.header.frame_id = "map" 112 | # fill up the rest later 113 | print("Sending goal request") 114 | self._send_goal_future = self._action_client.send_goal_async(goal_msg) 115 | 116 | self._send_goal_future.add_done_callback(self.goal_response_callback) 117 | 118 | def get_status(self): 119 | status = {} 120 | status["gs"] = self.goal_status 121 | status["gas"] = self.goal_accept_status 122 | status["f_flag"] = self.failure_flag 123 | return status 124 | 125 | def reset_status(self): 126 | self.goal_status = False 127 | self.goal_accept_status = False 128 | self.failure_flag = False 129 | 130 | 131 | class goto_pose: 132 | def __init__(self, action_client): 133 | self._action_client = action_client 134 | self.goal_status = False 135 | self.goal_accept_status = False 136 | self.failure_flag = False 137 | 138 | def goal_response_callback(self, future): 139 | goal_handle = future.result() 140 | if not goal_handle.accepted: 141 | print("nav2 Goal Rejected") 142 | self.goal_accept_status = False 143 | return 144 | 145 | print("nav2 Goal accepted") 146 | self.goal_accept_status = True 147 | 148 | self._get_result_future = goal_handle.get_result_async() 149 | self._get_result_future.add_done_callback(self.get_result_callback) 150 | 151 | def get_result_callback(self, future): 152 | 153 | result = future.result().result 154 | status = future.result().status 155 | if status == GoalStatus.STATUS_SUCCEEDED: 156 | print("navigation succeeded") 157 | self.goal_status = True 158 | else: 159 | print("navigation failed!") 160 | self.goal_status = False 161 | self.failure_flag = True 162 | 163 | def send_goal(self, goal_pose): 164 | print("Waiting for action server") 165 | self._action_client.wait_for_server() 166 | goal_msg = NavigateToPose.Goal() 167 | goal_msg.pose.header.frame_id= 'map' 168 | goal_msg.pose.pose.position.x = goal_pose["bx"] 169 | goal_msg.pose.pose.position.y = goal_pose["by"] 170 | goal_msg.pose.pose.orientation.z = goal_pose["bz"] 171 | goal_msg.pose.pose.orientation.w = goal_pose["bw"] 172 | 173 | # fill up the rest later 174 | print("Sending goal request") 175 | 176 | self._send_goal_future = self._action_client.send_goal_async(goal_msg) 177 | 178 | self._send_goal_future.add_done_callback(self.goal_response_callback) 179 | 180 | def get_status(self): 181 | status = {} 182 | status["gs"] = self.goal_status 183 | status["gas"] = self.goal_accept_status 184 | status["f_flag"] = self.failure_flag 185 | return status 186 | 187 | def reset_status(self): 188 | self.goal_status = False 189 | self.goal_accept_status = False 190 | self.failure_flag = False 191 | 192 | 193 | class MainLogic(Node): 194 | def __init__(self): 195 | super().__init__("dock_logic") 196 | self.dock_cmd = 0 197 | self.dock_stat = 0 198 | self.timer = self.create_timer(0.1, self.timed_callback) 199 | self.qos_profile_ = QoSProfile( 200 | reliability=QoSReliabilityPolicy.BEST_EFFORT, 201 | history=QoSHistoryPolicy.KEEP_LAST, 202 | depth=10, 203 | ) 204 | self.subscription = self.create_subscription( 205 | Int32, "dock_cmd", self.update_cmd, self.qos_profile_ 206 | ) 207 | self.status_publisher = self.create_publisher(Int32, "dock_status", 10) 208 | self.goto_pose_ = goto_pose( 209 | ActionClient(self, NavigateToPose, "navigate_to_pose") 210 | ) 211 | self.docking_client_ = docking_client(ActionClient(self, Dock, "Dock")) 212 | self.declare_parameter("load_file_path", "null") 213 | 214 | self.undocking_client_ = undocking_client( 215 | ActionClient(self, Undock, "Undock")) 216 | self.dock_file_path = ( 217 | self.get_parameter( 218 | "load_file_path").get_parameter_value().string_value 219 | ) 220 | # load in dock coordinates 221 | with open(self.dock_file_path) as outfile: 222 | self.initial_poses = json.load(outfile) 223 | 224 | def update_cmd(self, msg): 225 | self.dock_cmd = msg.data 226 | 227 | def timed_callback(self): 228 | # update status of all classes 229 | nav2_status = self.goto_pose_.get_status() 230 | dock_status = self.docking_client_.get_status() 231 | undock_status = self.undocking_client_.get_status() 232 | 233 | # check failure flags 234 | if ( 235 | nav2_status["f_flag"] == True 236 | or dock_status["f_flag"] == True 237 | or undock_status["f_flag"] == True 238 | ) and self.dock_stat != 10: 239 | print("Docking failure!") 240 | self.dock_stat = 10 241 | # Docking command received. Navigate to initial goal pose first 242 | elif self.dock_cmd == 1 and self.dock_stat == 0: 243 | 244 | self.goto_pose_.send_goal(self.initial_poses) 245 | self.dock_stat = 1 246 | # Engage in docking sequence once the robot has reached the goal 247 | elif ( 248 | self.dock_stat == 1 249 | and nav2_status["gs"] == True 250 | and nav2_status["gas"] == True 251 | ): 252 | print("docking robot!") 253 | self.docking_client_.send_goal(self.initial_poses) 254 | self.dock_stat = 2 255 | # once the robot has docked, we will wait for an input to undock. 256 | elif ( 257 | self.dock_stat == 2 258 | and dock_status["gs"] == True 259 | and dock_status["gas"] == True 260 | ): 261 | print("Waiting for undock command!") 262 | self.dock_stat = 3 263 | # Undock command received. Will initiate undocking sequence. 264 | elif self.dock_stat == 3 and self.dock_cmd == 2: 265 | print("undocking") 266 | self.undocking_client_.send_goal() 267 | self.dock_stat = 4 268 | # Once undocking sequence is complete, reset all boolean flags. 269 | elif ( 270 | self.dock_stat == 4 271 | and undock_status["gs"] == True 272 | and undock_status["gas"] == True 273 | ): 274 | print("Docking Sequence Complete") 275 | self.dock_stat = 0 276 | self.undocking_client_.reset_status() 277 | self.docking_client_.reset_status() 278 | self.goto_pose_.reset_status() 279 | elif self.dock_stat == 10 and self.dock_cmd == 10: 280 | print("Resetting docking state!") 281 | self.dock_stat = 0 282 | self.undocking_client_.reset_status() 283 | self.docking_client_.reset_status() 284 | self.goto_pose_.reset_status() 285 | 286 | self.pub_dock_status(self.dock_stat) 287 | 288 | def pub_dock_status(self, stat): 289 | msg = Int32() 290 | msg.data = stat 291 | self.status_publisher.publish(msg) 292 | 293 | 294 | def main(args=None): 295 | rclpy.init(args=args) 296 | 297 | action_client = MainLogic() 298 | 299 | rclpy.spin(action_client) 300 | 301 | action_client.destroy_node() 302 | rclpy.shutdown() 303 | 304 | 305 | if __name__ == "__main__": 306 | main() 307 | -------------------------------------------------------------------------------- /scripts/undock_robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | ... 3 | 4 | 5 | from action_msgs.msg import GoalStatus 6 | from lidar_auto_docking_messages.action import Undock 7 | 8 | import rclpy 9 | from rclpy.action import ActionClient 10 | from rclpy.node import Node 11 | 12 | 13 | class undocking_client: 14 | def __init__(self, action_client): 15 | self._action_client = action_client 16 | self.goal_status = False 17 | self.goal_accept_status = False 18 | 19 | def goal_response_callback(self, future): 20 | goal_handle = future.result() 21 | if not goal_handle.accepted: 22 | print("Goal Rejected") 23 | self.goal_accept_status = False 24 | return 25 | 26 | print("Goal accepted") 27 | self.goal_accept_status = True 28 | 29 | self._get_result_future = goal_handle.get_result_async() 30 | self._get_result_future.add_done_callback(self.get_result_callback) 31 | 32 | def get_result_callback(self, future): 33 | 34 | result = future.result().result 35 | status = future.result().status 36 | if status == GoalStatus.STATUS_SUCCEEDED: 37 | print("Goal Succeeded!") 38 | self.goal_status = True 39 | print(str(result.undocked)) 40 | else: 41 | print("Goal Failed!") 42 | self.goal_status = False 43 | 44 | def send_goal(self): 45 | print("Waiting for action server") 46 | self._action_client.wait_for_server() 47 | goal_msg = Undock.Goal() 48 | goal_msg.rotate_in_place = True 49 | print("Sending goal request") 50 | 51 | self._send_goal_future = self._action_client.send_goal_async(goal_msg) 52 | 53 | self._send_goal_future.add_done_callback(self.goal_response_callback) 54 | 55 | def get_status(self): 56 | status = {} 57 | status["gs"] = self.goal_status 58 | status["gas"] = self.goal_accept_status 59 | return status 60 | 61 | def reset_status(self): 62 | self.goal_status = False 63 | self.goal_accept_status = False 64 | 65 | 66 | class UndockClient(Node): 67 | def __init__(self): 68 | super().__init__("undock_client") 69 | self.undocking_client_ = undocking_client(ActionClient(self, Undock, "Undock")) 70 | 71 | def send_goal(self): 72 | self.undocking_client_.send_goal() 73 | 74 | 75 | def main(args=None): 76 | rclpy.init(args=args) 77 | 78 | action_client = UndockClient() 79 | 80 | action_client.send_goal() 81 | 82 | rclpy.spin(action_client) 83 | 84 | action_client.destroy_node() 85 | rclpy.shutdown() 86 | 87 | 88 | if __name__ == "__main__": 89 | main() 90 | -------------------------------------------------------------------------------- /src/autodock.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace std::chrono_literals; 4 | 5 | void DockingServer::init_objects() { 6 | std::shared_ptr new_ptr = shared_ptr_from_this(); 7 | perception_ = std::make_shared(new_ptr); 8 | controller_ = std::make_shared(new_ptr); 9 | } 10 | 11 | // shared_ptr_from_this would return a shared pointer of the current class 12 | std::shared_ptr DockingServer::shared_ptr_from_this() { 13 | return shared_from_this(); 14 | } 15 | 16 | rclcpp_action::GoalResponse DockingServer::handle_goal( 17 | const rclcpp_action::GoalUUID& uuid, 18 | std::shared_ptr goal) { 19 | RCLCPP_INFO(this->get_logger(), "Received goal request!"); 20 | (void)uuid; 21 | if (goal->dock_id != "") { 22 | RCLCPP_INFO(this->get_logger(), "received dock name!"); 23 | } 24 | return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; 25 | } 26 | 27 | rclcpp_action::CancelResponse DockingServer::handle_cancel( 28 | const std::shared_ptr goal_handle) { 29 | RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); 30 | (void)goal_handle; 31 | return rclcpp_action::CancelResponse::ACCEPT; 32 | } 33 | 34 | // this function initialises the dock timeout. 35 | void DockingServer::initDockTimeout() { 36 | // get current time and fill up the header 37 | rclcpp::Time time_now = rclcpp::Clock().now(); 38 | deadline_docking_ = time_now + rclcpp::Duration(120s); 39 | num_of_retries_ = NUM_OF_RETRIES_; 40 | } 41 | 42 | bool DockingServer::isDockingTimedOut() { 43 | // Have we exceeded our deadline or tries? 44 | rclcpp::Time time_now = rclcpp::Clock().now(); 45 | if (time_now > deadline_docking_ || !num_of_retries_) { 46 | return true; 47 | } 48 | return false; 49 | } 50 | 51 | bool DockingServer::continueDocking( 52 | std::shared_ptr result, 53 | const std::shared_ptr goal_handle) { 54 | // If charging, stop and return success. 55 | 56 | if (charging_) { 57 | result->docked = true; 58 | goal_handle->succeed(result); 59 | RCLCPP_INFO(this->get_logger(), "DOCK REACHED!"); 60 | return false; 61 | } 62 | 63 | // Timeout on time or retries. 64 | else if (isDockingTimedOut() || cancel_docking_) { 65 | result->docked = false; 66 | // return the current result 67 | RCLCPP_INFO(this->get_logger(), "Docking Cancelled"); 68 | return false; 69 | } 70 | 71 | else if (goal_handle->is_canceling()) { 72 | return false; 73 | } 74 | 75 | return true; 76 | } 77 | 78 | double DockingServer::backupDistance() { 79 | // Initialized to 1.0 meter as our basic backup amount. 80 | double distance = 1.0; 81 | 82 | // Distance should be proportional to the amount of yaw correction. 83 | // The constants are purely arbitrary because they seemed good at the time. 84 | // distance *= 3.5*fabs(correction_angle_); 85 | distance *= 1.5 * fabs(correction_angle_); 86 | // We should backup more the more times we try. This function should range 87 | // from 1 to 2. num_of_retries is initially equal to NUM_OF_RETRIES and 88 | // decrements as the robot retries. 89 | double retry_constant = 90 | 2 - static_cast(num_of_retries_) / NUM_OF_RETRIES_; 91 | retry_constant = std::max(1.0, std::min(2.0, retry_constant)); 92 | distance *= retry_constant; 93 | 94 | // Cap the backup limit to 1 meter, just in case. 95 | backup_limit_ = std::min(1.0, backup_limit_); 96 | // Threshold distance. 97 | distance = std::max(0.2, std::min(backup_limit_, distance)); 98 | 99 | return distance; 100 | } 101 | 102 | // method to reverse the robot when the abort flag is set. 103 | void DockingServer::executeBackupSequence(rclcpp::Rate& r) { 104 | RCLCPP_ERROR(this->get_logger(), "Poor Approach! Backing up!"); 105 | // Get off of the dock. Try to straighten out. 106 | while (!controller_->backup(DOCK_CONNECTOR_CLEARANCE_DISTANCE_, 107 | correction_angle_)) { 108 | if (isDockingTimedOut()) { 109 | return; 110 | } 111 | r.sleep(); // Sleep the rate control object. 112 | } 113 | // Move to recovery pose. 114 | while (!controller_->backup(backupDistance(), 0.0)) { 115 | if (isDockingTimedOut()) { 116 | return; 117 | } 118 | r.sleep(); 119 | } 120 | } 121 | 122 | bool DockingServer::isApproachBad(double& dock_yaw) { 123 | // Grab the dock pose in the base_link so we can evaluate it wrt the robot. 124 | geometry_msgs::msg::PoseStamped dock_pose_base_link; 125 | perception_->getPose(dock_pose_base_link, "base_link"); 126 | 127 | dock_yaw = angles::normalize_angle( 128 | tf2::getYaw(dock_pose_base_link.pose.orientation)); 129 | 130 | // If we are close to the dock but not quite docked, check other approach 131 | // parameters. 132 | if (dock_pose_base_link.pose.position.x < abort_distance_ && 133 | dock_pose_base_link.pose.position.x > DOCKED_DISTANCE_THRESHOLD_) { 134 | // Check to see if we are too far side-to-side or at a bad angle. 135 | if (fabs(dock_pose_base_link.pose.position.y) > abort_threshold_ || 136 | fabs(dock_yaw) > abort_angle_) { 137 | // Things are bad, abort. 138 | return true; 139 | } 140 | } 141 | // Everything is ok. 142 | return false; 143 | } 144 | 145 | // main function which is called when a goal is received 146 | void DockingServer::execute(const std::shared_ptr goal_handle) { 147 | RCLCPP_INFO(this->get_logger(), "Executing goal"); 148 | 149 | rclcpp::Rate loop_rate(50); 150 | const auto goal = goal_handle->get_goal(); 151 | auto feedback = std::make_shared(); 152 | auto result = std::make_shared(); 153 | 154 | // Reset flags. 155 | result->docked = false; 156 | aborting_ = false; 157 | charging_timeout_set_ = false; 158 | cancel_docking_ = false; 159 | charging_ = false; 160 | 161 | // start perception 162 | perception_->start(goal->dock_pose); 163 | // start the timeout counter 164 | initDockTimeout(); 165 | // get the first dock pose wrt base link. 166 | geometry_msgs::msg::PoseStamped dock_pose_base_link; 167 | RCLCPP_INFO(this->get_logger(), "Finding Dock"); 168 | 169 | while (!perception_->getPose(dock_pose_base_link, "base_link")) { 170 | if (!continueDocking(result, goal_handle)) { 171 | RCLCPP_ERROR(this->get_logger(), 172 | "Docking failed: Initial dock not found."); 173 | break; 174 | } 175 | } 176 | 177 | RCLCPP_INFO(this->get_logger(), "Pre-orienting the bot!"); 178 | double dock_yaw = angles::normalize_angle( 179 | tf2::getYaw(dock_pose_base_link.pose.orientation)); 180 | if (!std::isfinite(dock_yaw)) { 181 | RCLCPP_ERROR(this->get_logger(), "Dock yaw is invalid."); 182 | std::cout << "The invalid yaw value is: " << dock_yaw << std::endl; 183 | cancel_docking_ = true; 184 | } else if (rclcpp::ok() && continueDocking(result, goal_handle)) { 185 | // Set backup limit to be the initial dock range. 186 | backup_limit_ = std::sqrt(std::pow(dock_pose_base_link.pose.position.x, 2) + 187 | std::pow(dock_pose_base_link.pose.position.y, 2)); 188 | // Shorten up the range a bit. 189 | backup_limit_ *= 0.9; 190 | 191 | // Preorient the robot towards the dock. 192 | while (!controller_->backup(0.0, dock_yaw) && 193 | continueDocking(result, goal_handle) && rclcpp::ok()) { 194 | loop_rate.sleep(); // Sleep the rate control object. 195 | } 196 | } 197 | // Make sure controller is ready 198 | controller_->stop(); 199 | 200 | // main control which brings the bot to the dock 201 | while (rclcpp::ok() && continueDocking(result, goal_handle)) { 202 | // Update perception 203 | if (perception_->getPose(feedback->dock_pose)) { 204 | if (aborting_) { 205 | // backup 206 | executeBackupSequence(loop_rate); 207 | // Reset abort flag. 208 | aborting_ = false; 209 | // Decrement the number of retries. 210 | num_of_retries_--; 211 | } else { 212 | if (isApproachBad(correction_angle_)) { 213 | // Not on target, abort, abort, abort! 214 | controller_->stop(); 215 | aborting_ = true; 216 | } else { 217 | // do stuff here 218 | geometry_msgs::msg::PoseStamped dock_x_distance; 219 | perception_->getPose(dock_x_distance, "base_link"); 220 | 221 | std::cout << "Distance to dock: " << dock_x_distance.pose.position.x 222 | << std::endl; 223 | // remember to replace 0.55 with a reconfigurable parameter 224 | if (dock_x_distance.pose.position.x <= DOCKED_DISTANCE_THRESHOLD_) { 225 | charging_ = true; 226 | } else { 227 | charging_ = false; 228 | } 229 | // Update control 230 | controller_->approach(feedback->dock_pose); 231 | // Are we on the dock? Check charging timeout. 232 | // TODO: port over checkDockChargingConditions(maybe) 233 | // checkDockChargingConditions(); 234 | } 235 | } 236 | // feedback 237 | controller_->getCommand(feedback->command); 238 | goal_handle->publish_feedback(feedback); 239 | } 240 | loop_rate.sleep(); 241 | } 242 | // stop all movements once we are done docking 243 | controller_->stop(); 244 | perception_->stop(); 245 | } 246 | 247 | void DockingServer::handle_accepted( 248 | const std::shared_ptr goal_handle) { 249 | using namespace std::placeholders; 250 | // this needs to return quickly to avoid blocking the executor, so spin up 251 | // a new thread 252 | std::thread{std::bind(&DockingServer::execute, this, _1), goal_handle} 253 | .detach(); 254 | } 255 | 256 | //////////////////////////////////Udocking Server/////////////////////// 257 | 258 | rclcpp_action::GoalResponse UndockingServer::handle_goal( 259 | const rclcpp_action::GoalUUID& uuid, 260 | std::shared_ptr goal) { 261 | RCLCPP_INFO(this->get_logger(), "Received goal request!"); 262 | (void)uuid; 263 | if (goal->rotate_in_place == false) { 264 | RCLCPP_INFO(this->get_logger(), "Not going to rotate!"); 265 | } 266 | return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; 267 | } 268 | 269 | rclcpp_action::CancelResponse UndockingServer::handle_cancel( 270 | const std::shared_ptr goal_handle) { 271 | RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); 272 | (void)goal_handle; 273 | return rclcpp_action::CancelResponse::ACCEPT; 274 | } 275 | 276 | void UndockingServer::handle_accepted( 277 | const std::shared_ptr goal_handle) { 278 | using namespace std::placeholders; 279 | // this needs to return quickly to avoid blocking the executor, so spin up 280 | // a new thread 281 | std::thread{std::bind(&UndockingServer::execute, this, _1), goal_handle} 282 | .detach(); 283 | } 284 | 285 | void UndockingServer::execute( 286 | const std::shared_ptr goal_handle) { 287 | RCLCPP_INFO(this->get_logger(), "Executing goal"); 288 | 289 | rclcpp::Rate loop_rate(50); 290 | const auto goal = goal_handle->get_goal(); 291 | auto feedback = std::make_shared(); 292 | auto result = std::make_shared(); 293 | result->undocked = false; 294 | 295 | // Distances to backup/turn 296 | double backup = DOCK_CONNECTOR_CLEARANCE_DISTANCE_; 297 | double turn = goal->rotate_in_place ? 3.1 : 0.0; 298 | // get current time and fill up the header 299 | rclcpp::Time time_now = rclcpp::Clock().now(); 300 | rclcpp::Time timeout = time_now + rclcpp::Duration(50s); 301 | // ensure that the robot is not moving 302 | controller_->stop(); 303 | while (rclcpp::ok()) { 304 | // TODO: add in pre-empt function 305 | 306 | // Update control 307 | if (controller_->backup(backup, turn)) { 308 | // Odom says we have undocked 309 | 310 | RCLCPP_INFO(this->get_logger(), "Undock Successful!"); 311 | result->undocked = true; 312 | controller_->stop(); 313 | goal_handle->succeed(result); 314 | RCLCPP_INFO(this->get_logger(), "undocked robot"); 315 | return; 316 | } 317 | 318 | // Timeout 319 | if (rclcpp::Clock().now() > timeout) { 320 | controller_->stop(); 321 | RCLCPP_INFO(this->get_logger(), "Undock Unsuccessful!"); 322 | return; 323 | } 324 | 325 | // feedback 326 | controller_->getCommand(feedback->command); 327 | goal_handle->publish_feedback(feedback); 328 | loop_rate.sleep(); 329 | } 330 | } 331 | 332 | void UndockingServer::init_objects() { 333 | std::shared_ptr new_ptr = shared_ptr_from_this(); 334 | controller_ = std::make_shared(new_ptr); 335 | } 336 | 337 | // shared_ptr_from_this would return a shared pointer of the current class 338 | std::shared_ptr UndockingServer::shared_ptr_from_this() { 339 | return shared_from_this(); 340 | } 341 | 342 | //////////////////////////////main function//////////////////// 343 | 344 | int main(int argc, char** argv) { 345 | rclcpp::init(argc, argv); 346 | rclcpp::executors::MultiThreadedExecutor executor; 347 | auto docking_server = std::make_shared(); 348 | docking_server->init_objects(); 349 | 350 | auto undocking_server = std::make_shared(); 351 | undocking_server->init_objects(); 352 | 353 | executor.add_node(docking_server); 354 | executor.add_node(undocking_server); 355 | executor.spin(); 356 | rclcpp::shutdown(); 357 | return 0; 358 | } 359 | -------------------------------------------------------------------------------- /src/controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fetch Robotics Inc 3 | * Author: Michael Ferguson 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU Lesser General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | BaseController::BaseController(std::shared_ptr node_ptr) 28 | : listener_(node_ptr->get_clock()) { 29 | // Create publishers 30 | path_pub_ = node_ptr->create_publisher("path", 10); 31 | cmd_vel_pub_ = 32 | node_ptr->create_publisher("cmd_vel", 10); 33 | 34 | // TODO(enhancement): these should be loaded from ROS params 35 | k1_ = 3; 36 | k2_ = 2; 37 | min_velocity_ = 0.06; 38 | max_velocity_ = 0.06; 39 | max_angular_velocity_ = 1.0; 40 | beta_ = 0.2; 41 | lambda_ = 2.0; 42 | } 43 | 44 | bool BaseController::approach(const geometry_msgs::msg::PoseStamped& target) { 45 | // Transform pose by -dist_ in the X direction of the dock 46 | geometry_msgs::msg::PoseStamped pose = target; 47 | { 48 | double theta = angles::normalize_angle(tf2::getYaw(pose.pose.orientation)); 49 | // If the target has an invalid orientation then don't approach it. 50 | if (!std::isfinite(theta)) { 51 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), 52 | "Invalid approach target for docking"); 53 | stop(); 54 | return true; 55 | } 56 | pose.pose.position.x += cos(theta) * -dist_; 57 | pose.pose.position.y += sin(theta) * -dist_; 58 | } 59 | 60 | // Transform target into base frame 61 | try { 62 | pose.header.stamp = rclcpp::Time(0); 63 | listener_.transformPose("base_link", pose, pose); 64 | } catch (const tf2::TransformException& ex) { 65 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), 66 | "Couldn't get transform from dock to base_link"); 67 | 68 | stop(); 69 | return false; 70 | } 71 | 72 | // Distance to goal 73 | double r = std::sqrt(pose.pose.position.x * pose.pose.position.x + 74 | pose.pose.position.y * pose.pose.position.y); 75 | 76 | // Once we get close, reduce d_ 77 | if (r < 0.3) { 78 | // This part is trying to bring the target point into the goal but 79 | // right now it just jumps from 80 | // some virtual pose to the actual pose. We want to bring it in 81 | // more gradually. 82 | dist_ = 0.0; 83 | 84 | // dist_ = 1.6*r - 0.08; 85 | // if (dist_ < 0.0) dist_ = 0.0; 86 | } 87 | 88 | // If within distance tolerance, return true 89 | if (r < 0.01) { 90 | // stop(); 91 | return true; 92 | } 93 | 94 | // Orientation base frame relative to r_ 95 | double delta = std::atan2(-pose.pose.position.y, pose.pose.position.x); 96 | 97 | // Determine orientation of goal frame relative to r_ 98 | double theta = 99 | angles::normalize_angle(tf2::getYaw(pose.pose.orientation) + delta); 100 | 101 | // Compute the virtual control 102 | double a = atan(-k1_ * theta); 103 | // Compute curvature (k) 104 | double k = -1.0 / r * 105 | (k2_ * (delta - a) + 106 | (1 + (k1_ / 1 + ((k1_ * theta) * (k1_ * theta)))) * sin(delta)); 107 | 108 | // Compute max_velocity based on curvature 109 | double v = max_velocity_ / (1 + beta_ * std::pow(fabs(k), lambda_)); 110 | // Limit max velocity based on approaching target (avoids 111 | // overshoot) 112 | 113 | if (r < 0.75) { 114 | v = std::max(min_velocity_, std::min(std::min(r, max_velocity_), v)); 115 | } else { 116 | v = std::min(max_velocity_, std::max(min_velocity_, v)); 117 | } 118 | 119 | // Compute angular velocity 120 | double w = k * v; 121 | // Bound angular velocity 122 | double bounded_w = 123 | std::min(max_angular_velocity_, std::max(-max_angular_velocity_, w)); 124 | // Make sure that if we reduce w, we reduce v so that kurvature is 125 | // still followed 126 | if (w != 0.0) { 127 | v *= (bounded_w / w); 128 | } 129 | 130 | // Send command to base 131 | command_.linear.x = v; 132 | command_.angular.z = bounded_w; 133 | cmd_vel_pub_->publish(command_); 134 | 135 | // Create debugging view of path 136 | nav_msgs::msg::Path plan; 137 | plan.header.stamp = rclcpp::Clock().now(); 138 | plan.header.frame_id = "base_link"; 139 | // Add origin 140 | geometry_msgs::msg::PoseStamped path_pose; 141 | path_pose.header.frame_id = "base_link"; 142 | path_pose.pose.orientation.w = 1.0; 143 | plan.poses.push_back(path_pose); 144 | double yaw = 0.0; 145 | for (int i = 0; i < 20; i++) // 2 sec 146 | { 147 | path_pose.pose.position.x += 0.1 * command_.linear.x * cos(yaw); 148 | path_pose.pose.position.y += 0.1 * command_.linear.x * sin(yaw); 149 | yaw += 0.1 * command_.angular.z; 150 | path_pose.pose.orientation.z = sin(theta / 2.0); 151 | path_pose.pose.orientation.w = cos(theta / 2.0); 152 | 153 | double dx = path_pose.pose.position.x - pose.pose.position.x; 154 | double dy = path_pose.pose.position.y - pose.pose.position.y; 155 | if ((dx * dx + dy * dy) < 0.005) { 156 | break; 157 | } 158 | 159 | plan.poses.push_back(path_pose); 160 | } 161 | // Push goal pose onto path 162 | plan.poses.push_back(pose); 163 | // Publish path 164 | path_pub_->publish(plan); 165 | 166 | return false; 167 | } 168 | 169 | bool BaseController::backup(double distance, double rotate_distance) { 170 | // If the inputs are invalid then don't backup. 171 | if (!std::isfinite(distance) || !std::isfinite(rotate_distance)) { 172 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), 173 | "Backup parameters are not valid."); 174 | stop(); 175 | return true; 176 | } 177 | 178 | // Get current base pose in odom 179 | geometry_msgs::msg::PoseStamped pose; 180 | pose.header.frame_id = "base_link"; 181 | pose.pose.orientation.w = 1.0; 182 | 183 | try { 184 | listener_.waitTransform("odom", pose.header.frame_id); 185 | listener_.transformPose("odom", pose, pose); 186 | } 187 | 188 | catch (const tf2::TransformException& ex) { 189 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), 190 | "Couldn't get transform from base_link to odom"); 191 | stop(); 192 | return false; 193 | } 194 | 195 | // If just getting started, stow starting pose 196 | if (!ready_) { 197 | start_ = pose; 198 | turning_ = false; 199 | ready_ = true; 200 | } 201 | 202 | if (turning_) { 203 | // Get yaw angles 204 | double theta = 205 | angles::normalize_angle(tf2::getYaw(pose.pose.orientation) - 206 | tf2::getYaw(start_.pose.orientation)); 207 | double error = angles::normalize_angle(rotate_distance - theta); 208 | 209 | if (fabs(error) < 0.05) { 210 | stop(); 211 | return true; 212 | } else if (rotate_distance > 0.0) { // for rotation, take the minimum 213 | // value as the angular turning 214 | command_.angular.z = std::min(0.6, fabs(error) * 1.3 + 0.1); 215 | } else { 216 | command_.angular.z = std::max(-0.6, -(fabs(error) * 1.3 + 0.1)); 217 | } 218 | } else { 219 | // Check if have backed up enough 220 | double dx = pose.pose.position.x - start_.pose.position.x; 221 | double dy = pose.pose.position.y - start_.pose.position.y; 222 | if ((dx * dx + dy * dy) > (distance * distance)) { 223 | if (rotate_distance == 0.0) { 224 | stop(); 225 | return true; 226 | } else { 227 | turning_ = true; 228 | command_.linear.x = 0.0; 229 | } 230 | } else { 231 | command_.linear.x = -0.1; 232 | } 233 | } 234 | 235 | cmd_vel_pub_->publish(command_); 236 | 237 | return false; 238 | } 239 | 240 | bool BaseController::getCommand(geometry_msgs::msg::Twist& command) { 241 | command = command_; 242 | return true; 243 | } 244 | 245 | void BaseController::stop() { 246 | command_ = geometry_msgs::msg::Twist(); 247 | cmd_vel_pub_->publish(command_); 248 | 249 | // Reset the backup controller 250 | ready_ = false; 251 | 252 | // Reset the approach controller 253 | dist_ = 0.4; 254 | } 255 | -------------------------------------------------------------------------------- /src/dock_coordinates.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "lidar_auto_docking_messages/msg/initdock.hpp" 11 | #include "lidar_auto_docking/tf2listener.h" 12 | #include "sensor_msgs/msg/joy.hpp" 13 | 14 | using namespace std::chrono_literals; 15 | 16 | class DockCoordinates : public rclcpp::Node 17 | { 18 | public: 19 | DockCoordinates() : Node("dock_coordinates"), tf2_listen(this->get_clock()) 20 | { 21 | tbr = std::make_shared(this); 22 | publisher_ = this->create_publisher( 23 | "init_dock", 10); 24 | this->declare_parameter("reset_goal_button", 3); 25 | this->get_parameter("reset_goal_button", reset_goal_button); 26 | joy_sub_ = this->create_subscription( 27 | "joy", 10, [this](const sensor_msgs::msg::Joy::SharedPtr msg) 28 | { 29 | std::vector pressed_buttons = msg->buttons; 30 | 31 | if (pressed_buttons[reset_goal_button]) { 32 | RCLCPP_INFO(this->get_logger(), "Resetting initial dock estimate"); 33 | this->found_dockk = false; 34 | this->perception_ptr->stop(); 35 | update_init_dock(init_dock_pose); 36 | perception_ptr->start(init_dock_pose); 37 | } }); 38 | } 39 | 40 | // the init_objects function would return a shared_ptr to this class. It will 41 | // be stored in new_ptr before being passed to 42 | // the init_node function of Sepclass to initialise its memeber functions. 43 | void init_objects() 44 | { 45 | std::shared_ptr new_ptr = shared_ptr_from_this(); 46 | perception_ptr = std::make_shared(new_ptr); 47 | } 48 | 49 | // shared_ptr_from_this would return a shared pointer of the current class 50 | std::shared_ptr shared_ptr_from_this() 51 | { 52 | return shared_from_this(); 53 | } 54 | // debugging function to print out the initial dock pose 55 | void print_idp(geometry_msgs::msg::PoseStamped idp) 56 | { 57 | std::cout << "init x: " << idp.pose.position.x << "\n"; 58 | std::cout << "init y: " << idp.pose.position.y << "\n"; 59 | std::cout << "init z: " << idp.pose.orientation.z << "\n"; 60 | std::cout << "init w: " << idp.pose.orientation.w << "\n"; 61 | } 62 | 63 | // this function would update the initial dock pose to be 1m from robot. 64 | // wrt map frame. 65 | void update_init_dock(geometry_msgs::msg::PoseStamped &idp) 66 | { 67 | tf2_listen.waitTransform("map", "base_link"); 68 | geometry_msgs::msg::PoseStamped fake_dock; 69 | // take it that the fake dock is 1m in front of the robot. 70 | fake_dock.header.frame_id = "base_link"; 71 | fake_dock.pose.position.x = 1; 72 | // we will transform fake_dock wrt map 73 | tf2_listen.transformPose("map", fake_dock, idp); 74 | } 75 | 76 | void main_test() 77 | { 78 | update_init_dock(init_dock_pose); 79 | perception_ptr->start(init_dock_pose); 80 | timer_ = this->create_wall_timer(20ms, [this]() 81 | { 82 | // if no dock is found yet, call start function with init_dock_pose to let 83 | // perception assume the dock is 1m ahead of the bot. 84 | if (this->perception_ptr->getPose(this->dock_pose, "map") == false && 85 | this->found_dockk == false) { 86 | std::cout << "still finding dock!\n"; 87 | this->update_init_dock(this->init_dock_pose); 88 | this->perception_ptr->start(this->init_dock_pose); 89 | 90 | } else { 91 | this->found_dockk = true; 92 | 93 | // publish the transformations of the dock 94 | auto dock_pose_msg = lidar_auto_docking_messages::msg::Initdock(); 95 | dock_pose_msg.x = this->dock_pose.pose.position.x; 96 | dock_pose_msg.y = this->dock_pose.pose.position.y; 97 | dock_pose_msg.z = this->dock_pose.pose.orientation.z; 98 | dock_pose_msg.w = this->dock_pose.pose.orientation.w; 99 | publisher_->publish(dock_pose_msg); 100 | // Also, send the transform for visualisation 101 | this->time_now = rclcpp::Clock().now(); 102 | 103 | this->transformStamped.header.stamp = this->time_now; 104 | this->transformStamped.header.frame_id = "map"; 105 | this->transformStamped.child_frame_id = "dock"; 106 | this->transformStamped.transform.translation.x = 107 | this->dock_pose.pose.position.x; 108 | this->transformStamped.transform.translation.y = 109 | this->dock_pose.pose.position.y; 110 | this->transformStamped.transform.translation.z = 0.0; 111 | this->transformStamped.transform.rotation.x = 0; 112 | this->transformStamped.transform.rotation.y = 0; 113 | this->transformStamped.transform.rotation.z = 114 | this->dock_pose.pose.orientation.z; 115 | this->transformStamped.transform.rotation.w = 116 | this->dock_pose.pose.orientation.w; 117 | 118 | this->tbr->sendTransform(this->transformStamped); 119 | } }); 120 | } 121 | 122 | private: 123 | int reset_goal_button; 124 | std::shared_ptr perception_ptr; 125 | std::shared_ptr tbr; 126 | rclcpp::Publisher::SharedPtr publisher_; 127 | rclcpp::Subscription::SharedPtr joy_sub_; 128 | tf2_listener tf2_listen; 129 | 130 | rclcpp::Time time_now; 131 | geometry_msgs::msg::TransformStamped transformStamped; 132 | geometry_msgs::msg::PoseStamped dock_pose; 133 | geometry_msgs::msg::PoseStamped init_dock_pose; 134 | bool found_dockk = false; 135 | rclcpp::TimerBase::SharedPtr timer_; 136 | }; 137 | 138 | int main(int argc, char *argv[]) 139 | { 140 | rclcpp::init(argc, argv); 141 | std::shared_ptr min_ptr = 142 | std::make_shared(); 143 | 144 | min_ptr->init_objects(); 145 | min_ptr->main_test(); 146 | rclcpp::spin(min_ptr); 147 | rclcpp::shutdown(); 148 | return 0; 149 | } 150 | -------------------------------------------------------------------------------- /src/icp_2d.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fetch Robotics Inc. 3 | * Author: Michael Ferguson 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU Lesser General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace icp_2d { 26 | 27 | double thetaFromQuaternion(const geometry_msgs::msg::Quaternion& q) { 28 | // If all zeros, is invalid, assume no rotation 29 | if (q.x == 0.0 && q.y == 0.0 && q.z == 0.0 && q.w == 0.0) { 30 | return 0.0; 31 | } 32 | 33 | // Compute theta (this only works if q is in 2d) 34 | return 2.0 * atan2(q.z, q.w); 35 | } 36 | 37 | std::vector transform( 38 | const std::vector& points, double x, double y, 39 | double theta) { 40 | std::vector points_t; 41 | 42 | double cos_th = cos(theta); 43 | double sin_th = sin(theta); 44 | 45 | for (size_t i = 0; i < points.size(); i++) { 46 | geometry_msgs::msg::Point pt = points[i]; 47 | geometry_msgs::msg::Point pt_t; 48 | 49 | // Rotate then Translate 50 | pt_t.x = cos_th * pt.x - sin_th * pt.y + x; 51 | pt_t.y = sin_th * pt.x + cos_th * pt.y + y; 52 | 53 | points_t.push_back(pt_t); 54 | } 55 | 56 | return points_t; 57 | } 58 | 59 | geometry_msgs::msg::Point getCentroid( 60 | const std::vector points) { 61 | geometry_msgs::msg::Point pt; 62 | for (size_t i = 0; i < points.size(); i++) { 63 | pt.x += points[i].x; 64 | pt.y += points[i].y; 65 | } 66 | pt.x /= points.size(); 67 | pt.y /= points.size(); 68 | return pt; 69 | } 70 | 71 | // Computes correspondences for alignment 72 | // correspondences will be equal in size to source, and contain the 73 | // closest point in target that corresponds to that in source. 74 | bool computeCorrespondences( 75 | const std::vector& source, 76 | const std::vector& target, 77 | std::vector& correspondences) { 78 | correspondences.clear(); 79 | std::vector c_num; 80 | 81 | for (size_t i = 0; i < source.size(); i++) { 82 | double d = 1.0; 83 | size_t best = 0; 84 | 85 | for (size_t j = 0; j < target.size(); j++) { 86 | double dx = source[i].x - target[j].x; 87 | double dy = source[i].y - target[j].y; 88 | double dist = dx * dx + dy * dy; 89 | if (dist < d) { 90 | best = j; 91 | d = dist; 92 | } 93 | } 94 | 95 | if (d >= 1.0) { 96 | // No correspondence found! 97 | return false; 98 | } 99 | correspondences.push_back(target[best]); 100 | c_num.push_back(best); 101 | } 102 | 103 | // All points have correspondence 104 | return true; 105 | } 106 | 107 | bool alignPCA(const std::vector source, 108 | const std::vector target, 109 | geometry_msgs::msg::Transform& t) { 110 | // Get initial rotation angle 111 | double theta = thetaFromQuaternion(t.rotation); 112 | 113 | // Transform source based on initial transfrom 114 | std::vector source_t = 115 | transform(source, t.translation.x, t.translation.y, theta); 116 | 117 | // Get centroid of source and target 118 | geometry_msgs::msg::Point cs = getCentroid(source_t); 119 | geometry_msgs::msg::Point ct = getCentroid(target); 120 | 121 | // Update translation 122 | t.translation.x += ct.x - cs.x; 123 | t.translation.y += ct.y - cs.y; 124 | 125 | // Compute P for source 126 | Eigen::MatrixXf Ps(2, source_t.size()); 127 | for (size_t i = 0; i < source_t.size(); i++) { 128 | Ps(0, i) = source_t[i].x - cs.x; 129 | Ps(1, i) = source_t[i].y - cs.y; 130 | } 131 | 132 | // Compute M for source (covariance matrix) 133 | Eigen::MatrixXf Ms = Ps * Ps.transpose(); 134 | 135 | // Get EigenVectors of Covariance Matrix 136 | Eigen::EigenSolver solver_s(Ms); 137 | Eigen::MatrixXf A = solver_s.eigenvectors().real(); 138 | Eigen::MatrixXf eig_of_Ms = solver_s.eigenvalues().real(); 139 | // Sort the eigen vectors for the ideal dock. 140 | // There are only two eigenvalues. If they come out not sorted, 141 | // swap the vectors, and make sure the axes are still right handed. 142 | if (eig_of_Ms(0, 0) < eig_of_Ms(1, 0)) { 143 | A.col(0).swap(A.col(1)); 144 | A.col(1) = -A.col(1); 145 | } 146 | // Compute P for target 147 | Eigen::MatrixXf Pt(2, target.size()); 148 | for (size_t i = 0; i < target.size(); i++) { 149 | Pt(0, i) = target[i].x - ct.x; 150 | Pt(1, i) = target[i].y - ct.y; 151 | } 152 | 153 | // Compute M for target (covariance matrix) 154 | Eigen::MatrixXf Mt = Pt * Pt.transpose(); 155 | 156 | // Get EigenVectors of Covariance Matrix 157 | Eigen::EigenSolver solver_t(Mt); 158 | Eigen::MatrixXf B = solver_t.eigenvectors().real(); 159 | Eigen::MatrixXf eig_of_Mt = solver_t.eigenvalues().real(); 160 | // Sort the eigen vectors for the candidate dock. 161 | // There are only two eigenvalues. If they come out not sorted, 162 | // swap the vectors. 163 | if (eig_of_Mt(0, 0) < eig_of_Mt(1, 0)) { 164 | B.col(0).swap(B.col(1)); 165 | B.col(1) = -B.col(1); 166 | } 167 | 168 | // Get rotation 169 | Eigen::MatrixXf R = B * A.transpose(); 170 | 171 | // Update rotation 172 | theta += atan2(R(1, 0), R(0, 0)); 173 | t.rotation.z = sin(theta / 2.0); 174 | t.rotation.w = cos(theta / 2.0); 175 | return true; 176 | } 177 | 178 | bool alignSVD(const std::vector source, 179 | const std::vector target, 180 | geometry_msgs::msg::Transform& t) { 181 | double theta = thetaFromQuaternion(t.rotation); 182 | 183 | // Transform source based on initial transform 184 | std::vector source_t = 185 | transform(source, t.translation.x, t.translation.y, theta); 186 | 187 | // Get Correspondences 188 | // Technically this should be source_t -> target, however 189 | // to keep the frame transforms simpler, we actually call this 190 | // function with the ideal cloud as source, and the sensor 191 | // data as target. The correspondences then need to be setup 192 | // so that we map each point in the target onto a point in the 193 | // source, since the laser may not see the whole dock. 194 | std::vector corr; 195 | if (!computeCorrespondences(target, source_t, corr)) { 196 | return false; 197 | } 198 | 199 | // Get centroid of source_t and corr 200 | geometry_msgs::msg::Point cs = getCentroid(corr); 201 | geometry_msgs::msg::Point ct = getCentroid(target); 202 | 203 | // Compute P 204 | Eigen::MatrixXf P(2, corr.size()); 205 | for (size_t i = 0; i < corr.size(); i++) { 206 | P(0, i) = corr[i].x - cs.x; 207 | P(1, i) = corr[i].y - cs.y; 208 | } 209 | 210 | // Compute Q 211 | Eigen::MatrixXf Q(2, target.size()); 212 | for (size_t i = 0; i < target.size(); i++) { 213 | Q(0, i) = target[i].x - ct.x; 214 | Q(1, i) = target[i].y - ct.y; 215 | } 216 | 217 | // Compute M 218 | Eigen::MatrixXf M = P * Q.transpose(); 219 | 220 | // Find SVD of M 221 | Eigen::JacobiSVD svd( 222 | M, Eigen::ComputeThinU | Eigen::ComputeThinV); 223 | 224 | // Get R (rotation matrix from source to target) 225 | Eigen::MatrixXf R = svd.matrixV() * svd.matrixU().transpose(); 226 | 227 | // Compute final transformation 228 | t.translation.x += (ct.x - cs.x); 229 | t.translation.y += (ct.y - cs.y); 230 | 231 | // Update rotation of transform 232 | theta += atan2(R(1, 0), R(0, 0)); 233 | t.rotation.z = sin(theta / 2.0); 234 | t.rotation.w = cos(theta / 2.0); 235 | return true; 236 | } 237 | 238 | double getRMSD(const std::vector source, 239 | const std::vector target) { 240 | // Compute correspondences 241 | std::vector corr; 242 | if (!computeCorrespondences(source, target, corr)) { 243 | // No correspondences, return massive number 244 | // (we considered using std::numeric_limits::max() here, but 245 | // that could lead to under/overflow later on when comparing RMSD) 246 | return 10e6; 247 | } 248 | 249 | // Get Root Mean Squared Distance (RMSD) 250 | double rmsd = 0.0; 251 | for (size_t i = 0; i < source.size(); i++) { 252 | double dx = source[i].x - corr[i].x; 253 | double dy = source[i].y - corr[i].y; 254 | rmsd += dx * dx + dy * dy; 255 | } 256 | rmsd /= corr.size(); 257 | rmsd = sqrt(rmsd); 258 | 259 | return rmsd; 260 | } 261 | 262 | double alignICP(const std::vector source, 263 | const std::vector target, 264 | geometry_msgs::msg::Transform& t, size_t max_iterations, 265 | double min_delta_rmsd) { 266 | // Initial alignment with PCA 267 | alignPCA(source, target, t); 268 | 269 | // Previous RMSD 270 | double prev_rmsd = -1.0; 271 | 272 | // Iteratively refine with SVD 273 | for (size_t iteration = 0; iteration < max_iterations; iteration++) { 274 | // Perform SVD 275 | if (!alignSVD(source, target, t)) { 276 | // SVD failed 277 | return -1.0; 278 | } 279 | 280 | // Transform source to target 281 | std::vector source_t = 282 | transform(source, t.translation.x, t.translation.y, 283 | thetaFromQuaternion(t.rotation)); 284 | 285 | // As above, this should technically be the other way around, 286 | // However, at this phase we want to check the 287 | double rmsd = getRMSD(target, source_t); 288 | 289 | // Check termination condition 290 | if (prev_rmsd > 0.0) { 291 | if (fabs(prev_rmsd - rmsd) < min_delta_rmsd) { 292 | // Terminate on RMSD, return based on ideal -> observed 293 | return getRMSD(source_t, target); 294 | } 295 | } 296 | prev_rmsd = rmsd; 297 | } 298 | 299 | // Transform source to target one last time 300 | std::vector source_t = 301 | transform(source, t.translation.x, t.translation.y, 302 | thetaFromQuaternion(t.rotation)); 303 | 304 | // Return RMSD based on ideal -> observed 305 | return getRMSD(source_t, target); 306 | } 307 | 308 | } // namespace icp_2d 309 | -------------------------------------------------------------------------------- /src/laser_processor.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace laser_processor { 43 | Sample* Sample::Extract(int ind, const sensor_msgs::msg::LaserScan& scan) { 44 | Sample* s = new Sample(); 45 | 46 | s->index = ind; 47 | s->range = scan.ranges[ind]; 48 | s->x = cos(scan.angle_min + ind * scan.angle_increment) * s->range; 49 | s->y = sin(scan.angle_min + ind * scan.angle_increment) * s->range; 50 | if (s->range > scan.range_min && s->range < scan.range_max) 51 | return s; 52 | else { 53 | delete s; 54 | return NULL; 55 | } 56 | } 57 | 58 | void SampleSet::clear() { 59 | for (SampleSet::iterator i = begin(); i != end(); i++) { 60 | delete (*i); 61 | } 62 | set::clear(); 63 | } 64 | 65 | void SampleSet::appendToCloud(sensor_msgs::msg::PointCloud& cloud, int r, int g, 66 | int b) { 67 | float color_val = 0; 68 | 69 | int rgb = (r << 16) | (g << 8) | b; 70 | color_val = *reinterpret_cast(&rgb); 71 | 72 | for (iterator sample_iter = begin(); sample_iter != end(); sample_iter++) { 73 | geometry_msgs::msg::Point32 point; 74 | point.x = (*sample_iter)->x; 75 | point.y = (*sample_iter)->y; 76 | point.z = 0; 77 | 78 | cloud.points.push_back(point); 79 | 80 | if (cloud.channels[0].name == "rgb") 81 | cloud.channels[0].values.push_back(color_val); 82 | } 83 | } 84 | 85 | tf2::Vector3 SampleSet::center() { 86 | float x_mean = 0.0; 87 | float y_mean = 0.0; 88 | for (iterator i = begin(); i != end(); i++) 89 | 90 | { 91 | x_mean += ((*i)->x) / size(); 92 | y_mean += ((*i)->y) / size(); 93 | } 94 | 95 | return tf2::Vector3(x_mean, y_mean, 0.0); 96 | } 97 | 98 | void ScanMask::addScan(sensor_msgs::msg::LaserScan& scan) { 99 | if (!filled) { 100 | angle_min = scan.angle_min; 101 | angle_max = scan.angle_max; 102 | size = scan.ranges.size(); 103 | filled = true; 104 | } else if (angle_min != scan.angle_min || angle_max != scan.angle_max || 105 | size != scan.ranges.size()) { 106 | throw std::runtime_error( 107 | "laser_scan::ScanMask::addScan: inconsistantly sized scans added to " 108 | "mask"); 109 | } 110 | 111 | for (uint32_t i = 0; i < scan.ranges.size(); i++) { 112 | Sample* s = Sample::Extract(i, scan); 113 | 114 | if (s != NULL) { 115 | SampleSet::iterator m = mask_.find(s); 116 | 117 | if (m != mask_.end()) { 118 | if ((*m)->range > s->range) { 119 | delete (*m); 120 | mask_.erase(m); 121 | mask_.insert(s); 122 | } else { 123 | delete s; 124 | } 125 | } else { 126 | mask_.insert(s); 127 | } 128 | } 129 | } 130 | } 131 | 132 | bool ScanMask::hasSample(Sample* s, float thresh) { 133 | if (s != NULL) { 134 | SampleSet::iterator m = mask_.find(s); 135 | if (m != mask_.end()) 136 | if (((*m)->range - thresh) < s->range) return true; 137 | } 138 | return false; 139 | } 140 | 141 | ScanProcessor::ScanProcessor(const sensor_msgs::msg::LaserScan& scan, 142 | ScanMask& mask_, float mask_threshold) { 143 | scan_ = scan; 144 | 145 | SampleSet* cluster = new SampleSet; 146 | cluster->header = scan.header; 147 | 148 | for (uint32_t i = 0; i < scan.ranges.size(); i++) { 149 | Sample* s = Sample::Extract(i, scan); 150 | 151 | if (s != NULL) { 152 | if (!mask_.hasSample(s, mask_threshold)) { 153 | cluster->insert(s); 154 | } else { 155 | delete s; 156 | } 157 | } 158 | } 159 | 160 | clusters_.push_back(cluster); 161 | } 162 | 163 | ScanProcessor::~ScanProcessor() { 164 | for (std::list::iterator c = clusters_.begin(); 165 | c != clusters_.end(); c++) 166 | delete (*c); 167 | } 168 | 169 | void ScanProcessor::removeLessThan(uint32_t num) { 170 | std::list::iterator c_iter = clusters_.begin(); 171 | while (c_iter != clusters_.end()) { 172 | if ((*c_iter)->size() < num) { 173 | delete (*c_iter); 174 | clusters_.erase(c_iter++); 175 | } else { 176 | ++c_iter; 177 | } 178 | } 179 | } 180 | 181 | void ScanProcessor::splitConnected(float thresh) { 182 | std::list tmp_clusters; 183 | 184 | std::list::iterator c_iter = clusters_.begin(); 185 | 186 | // For each cluster 187 | while (c_iter != clusters_.end()) { 188 | // Go through the entire list 189 | while ((*c_iter)->size() > 0) { 190 | // Take the first element 191 | SampleSet::iterator s_first = (*c_iter)->begin(); 192 | 193 | // Start a new queue 194 | std::list sample_queue; 195 | sample_queue.push_back(*s_first); 196 | 197 | (*c_iter)->erase(s_first); 198 | 199 | // Grow until we get to the end of the queue 200 | std::list::iterator s_q = sample_queue.begin(); 201 | while (s_q != sample_queue.end()) { 202 | int expand = static_cast(asin(thresh / (*s_q)->range) / 203 | scan_.angle_increment); 204 | 205 | SampleSet::iterator s_rest = (*c_iter)->begin(); 206 | 207 | while ((s_rest != (*c_iter)->end() && 208 | (*s_rest)->index < (*s_q)->index + expand)) { 209 | if ((*s_rest)->range - (*s_q)->range > thresh) { 210 | break; 211 | } else if (sqrt(pow((*s_q)->x - (*s_rest)->x, 2.0f) + 212 | pow((*s_q)->y - (*s_rest)->y, 2.0f)) < thresh) { 213 | sample_queue.push_back(*s_rest); 214 | (*c_iter)->erase(s_rest++); 215 | break; 216 | } else { 217 | ++s_rest; 218 | } 219 | } 220 | s_q++; 221 | } 222 | 223 | // Move all the samples into the new cluster 224 | SampleSet* c = new SampleSet; 225 | c->header = (*c_iter)->header; 226 | for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++) 227 | c->insert(*s_q); 228 | 229 | // Store the temporary clusters 230 | tmp_clusters.push_back(c); 231 | } 232 | 233 | // Now that c_iter is empty, we can delete 234 | delete (*c_iter); 235 | 236 | // And remove from the map 237 | clusters_.erase(c_iter++); 238 | } 239 | 240 | clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end()); 241 | } 242 | 243 | } // namespace laser_processor 244 | -------------------------------------------------------------------------------- /src/linear_pose_filter_2d.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fetch Robotics Inc. 3 | * Author: Griswald Brooks 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU Lesser General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public License 16 | * along with this program. If not, see . 17 | */ 18 | 19 | #include 20 | 21 | // STL includes. 22 | #include 23 | 24 | // ROS2 includes. 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | LinearPoseFilter2D::LinearPoseFilter2D(const std::vector& b, 32 | const std::vector& a) { 33 | setCoeff(b, a); 34 | } 35 | 36 | void LinearPoseFilter2D::setCoeff(const std::vector& b, 37 | const std::vector& a) { 38 | // Check for the smallest vector and use that as the number of elements to 39 | // push. 40 | size_t N = std::min(b.size(), a.size()); 41 | ; 42 | 43 | // Clear the existing coefficients. 44 | b_.resize(N); 45 | a_.resize(N); 46 | 47 | // Copy in the new coefficients. Only copy the N elements from the back. 48 | // Ideally this should be all of the elements. 49 | std::copy_backward(b.end() - N, b.end(), b_.end()); 50 | std::copy_backward(a.end() - N, a.end(), a_.end()); 51 | 52 | // Ensure that the sample vectors are appropriately sized. It is done in this 53 | // way so if we modify the coefficients but want to mainatain history we don't 54 | // corrupt the past samples. 55 | int K = in_.size() - b_.size(); 56 | if (K > 0) { 57 | // Remove the extra elements. 58 | for (int i = 0; i < K; i++) { 59 | in_.pop_front(); 60 | out_.pop_front(); 61 | yaw_in_.pop_front(); 62 | yaw_out_.pop_front(); 63 | } 64 | } else { 65 | // Add the extra elements. 66 | for (int i = 0; i > K; i--) { 67 | in_.push_front(originPose()); 68 | out_.push_front(originPose()); 69 | yaw_in_.push_front(tf2::getYaw(originPose().orientation)); 70 | yaw_out_.push_front(tf2::getYaw(originPose().orientation)); 71 | } 72 | } 73 | } 74 | 75 | geometry_msgs::msg::Pose LinearPoseFilter2D::filter( 76 | const geometry_msgs::msg::Pose& pose) { 77 | // Add the new input pose to the vector of inputs. 78 | in_.push_back(pose); 79 | yaw_in_.push_back(getUnnormalizedYaw(pose, getNewestOutputYaw())); 80 | // Store a new output. 81 | out_.push_back(originPose()); 82 | yaw_out_.push_back(0.0f); 83 | // Remove the oldest elements. 84 | in_.pop_front(); 85 | out_.pop_front(); 86 | yaw_in_.pop_front(); 87 | yaw_out_.pop_front(); 88 | 89 | // Grab the iterator to the new output. 90 | std::deque::iterator outn = --out_.end(); 91 | std::deque::iterator yaw_outn = --yaw_out_.end(); 92 | 93 | // Terminal index of vector. 94 | size_t N = b_.size() - 1; 95 | for (size_t i = 0; i < (N + 1); i++) { 96 | // Sweep the inputs. 97 | outn->position.x += b_[i] * in_[N - i].position.x; 98 | outn->position.y += b_[i] * in_[N - i].position.y; 99 | *yaw_outn += b_[i] * yaw_in_[N - i]; 100 | 101 | // Skip the first index for the outputs. 102 | if (i > 0) { 103 | // Sweep the outputs. 104 | outn->position.x -= a_[i] * out_[N - i].position.x; 105 | outn->position.y -= a_[i] * out_[N - i].position.y; 106 | *yaw_outn -= a_[i] * yaw_out_[N - i]; 107 | } 108 | } 109 | 110 | // Store orientation. 111 | outn->orientation.z = sin(*yaw_outn / 2.0); 112 | outn->orientation.w = cos(*yaw_outn / 2.0); 113 | 114 | // Return the output. 115 | return *outn; 116 | } 117 | 118 | void LinearPoseFilter2D::reset() { 119 | // Copy in the origins. 120 | setFilterState(originPose(), originPose()); 121 | } 122 | 123 | void LinearPoseFilter2D::setFilterState( 124 | const geometry_msgs::msg::Pose& input_pose, 125 | const geometry_msgs::msg::Pose& output_pose) { 126 | // Get output yaw. 127 | float yaw_out = tf2::getYaw(output_pose.orientation); 128 | // Fill the output buffer with poses 129 | std::fill(out_.begin(), out_.end(), output_pose); 130 | std::fill(yaw_out_.begin(), yaw_out_.end(), yaw_out); 131 | 132 | // Fill the input buffer with poses. 133 | std::fill(in_.begin(), in_.end(), input_pose); 134 | std::fill(yaw_in_.begin(), yaw_in_.end(), 135 | getUnnormalizedYaw(input_pose, yaw_out)); 136 | } 137 | 138 | void LinearPoseFilter2D::setFilterState( 139 | const std::vector& input_poses, 140 | const std::vector& output_poses) { 141 | // Check length of new output set. 142 | // If it is less than or equal to the output buffer size, then copy all of it 143 | // into the output buffer. 144 | std::vector::const_iterator earliest_pose_out; 145 | if (output_poses.size() <= out_.size()) { 146 | earliest_pose_out = output_poses.begin(); 147 | } else { 148 | // If the new set is bigger than the output buffer, just copy the the newest 149 | // elements of the new set. 150 | earliest_pose_out = output_poses.end() - out_.size(); 151 | } 152 | 153 | // Copy output poses. 154 | std::copy_backward(earliest_pose_out, output_poses.end(), out_.end()); 155 | 156 | // Copy unnormalized output yaw. 157 | std::deque::iterator yaw_out = yaw_out_.end(); 158 | std::deque::iterator yaw_previous = yaw_out_.end(); 159 | std::vector::const_iterator pose_out = 160 | output_poses.end(); 161 | while (pose_out != earliest_pose_out) { 162 | // // If this is the first time then initialize previous yaw. 163 | if (yaw_previous == yaw_out_.end()) { 164 | // Dereference the newest output pose, convert it to a yaw and save it to 165 | // the vector of unnormalized yaws. 166 | *(--yaw_previous) = tf2::getYaw((--output_poses.end())->orientation); 167 | } 168 | 169 | // Convert the output pose to unnormalized yaw wrt the previous yaw and 170 | // increment the previous yaw to the current yaw. 171 | *(--yaw_out) = getUnnormalizedYaw(*(--pose_out), *(yaw_previous--)); 172 | } 173 | 174 | // Check length of new input set. 175 | // If it is less than or equal to the input buffer size, then copy all of it 176 | // into the input buffer. 177 | std::vector::const_iterator earliest_pose_in; 178 | if (input_poses.size() <= in_.size()) { 179 | earliest_pose_in = input_poses.begin(); 180 | } else { 181 | // If the new set is bigger than the input buffer, just copy the the newest 182 | // elements of the new set. 183 | earliest_pose_in = input_poses.end() - in_.size(); 184 | } 185 | 186 | // Copy input poses. 187 | std::copy_backward(earliest_pose_in, input_poses.end(), in_.end()); 188 | 189 | // Copy unnormalized input yaw. 190 | yaw_previous = yaw_out_.end(); 191 | std::deque::iterator yaw_in = yaw_in_.end(); 192 | std::vector::const_iterator pose_in = 193 | input_poses.end(); 194 | while (pose_in != earliest_pose_in) { 195 | *(--yaw_in) = getUnnormalizedYaw(*(--pose_in), *(--yaw_previous)); 196 | } 197 | } 198 | 199 | geometry_msgs::msg::Pose LinearPoseFilter2D::originPose() { 200 | // geometry_msgs::msg::Pose origin; 201 | auto origin = geometry_msgs::msg::Pose(); 202 | origin.position.x = 0; 203 | origin.position.y = 0; 204 | origin.position.z = 0; 205 | origin.orientation.x = 0; 206 | origin.orientation.y = 0; 207 | origin.orientation.z = 0; 208 | origin.orientation.w = 1; 209 | 210 | return origin; 211 | } 212 | 213 | float LinearPoseFilter2D::getUnnormalizedYaw(geometry_msgs::msg::Pose pose, 214 | float reference_yaw) { 215 | // Get the normalized yaw. 216 | float yaw = tf2::getYaw(pose.orientation); 217 | 218 | // Add the normalized difference in angles to the reference. 219 | return reference_yaw + angles::normalize_angle(yaw - reference_yaw); 220 | } 221 | 222 | float LinearPoseFilter2D::getNewestOutputYaw() { 223 | // If there aren't any previous yaws, return zero. 224 | if (yaw_out_.empty()) { 225 | return 0.0f; 226 | } else { 227 | return *(--yaw_out_.end()); 228 | } 229 | } 230 | -------------------------------------------------------------------------------- /src/perception.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fetch Robotics Inc. 3 | * Author: Michael Ferguson, Sriramvarun Nidamarthy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU Lesser General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public License 16 | * along with this program. If not, see . 17 | */ 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | double getPoseDistance(const geometry_msgs::msg::Pose a, 32 | const geometry_msgs::msg::Pose b) { 33 | double dx = a.position.x - b.position.x; 34 | double dy = a.position.y - b.position.y; 35 | return sqrt(dx * dx + dy * dy); 36 | } 37 | 38 | DockPerception::DockPerception(std::shared_ptr node_ptr) 39 | : listener_(node_ptr->get_clock()), 40 | running_(false), 41 | tracking_frame_("odom"), 42 | found_dock_(false) { 43 | node_ptr_ = node_ptr; 44 | debug_ = false; 45 | 46 | // enable debugging for first time test 47 | // Should we publish the debugging cloud 48 | 49 | // if (!pnh.getParam("debug", debug_)) 50 | //{ 51 | // debug_ = false; 52 | // } 53 | 54 | // Create coefficient vectors for a second order butterworth filter 55 | // with a cutoff frequency of 10 Hz assuming the loop is updated at 50 Hz 56 | // [b, a] = butter(order, cutoff_frequ/half_sampling_freq) 57 | // [b, a] = butter(2, 10/25) 58 | 59 | float b_arr[] = {0.20657, 0.41314, 0.20657}; 60 | float a_arr[] = {1.00000, -0.36953, 0.19582}; 61 | std::vector b(b_arr, b_arr + sizeof(b_arr) / sizeof(float)); 62 | std::vector a(a_arr, a_arr + sizeof(a_arr) / sizeof(float)); 63 | dock_pose_filter_.reset(new LinearPoseFilter2D(b, a)); 64 | 65 | // Limit the average reprojection error of points onto 66 | // the ideal dock. This prevents the robot docking 67 | // with something that is very un-dock-like. 68 | // TODO: parameterize (maybe) 69 | max_alignment_error_ = 0.01; 70 | 71 | // Create ideal cloud 72 | // Front face is 300mm long 73 | for (double y = -0.15; y <= 0.15; y += 0.001) { 74 | geometry_msgs::msg::Point p; 75 | p.x = p.z = 0.0; 76 | p.y = y; 77 | ideal_cloud_.push_back(p); 78 | front_cloud_.push_back(p); 79 | } 80 | // Each side is 100mm long, at 45 degree angle 81 | for (double x = 0.0; x < 0.05; x += 0.001) { 82 | geometry_msgs::msg::Point p; 83 | p.x = x; 84 | p.y = 0.15 + x; 85 | p.z = 0.0; 86 | ideal_cloud_.push_back(p); 87 | p.y = -0.15 - x; 88 | ideal_cloud_.insert(ideal_cloud_.begin(), p); 89 | } 90 | /* 91 | // Debugging publishers first 92 | if (debug_) { 93 | debug_points_ = 94 | nh.advertise("dock_points", 10); 95 | } 96 | */ 97 | // Init base scan only after publishers are created 98 | scan_sub_ = node_ptr_->create_subscription( 99 | "scan", 10, std::bind(&DockPerception::callback, this, _1)); 100 | std::cout << "Dock perception initialized\n"; 101 | } 102 | 103 | bool DockPerception::start(const geometry_msgs::msg::PoseStamped& pose) { 104 | running_ = false; 105 | found_dock_ = false; 106 | dock_ = pose; 107 | running_ = true; 108 | return true; 109 | } 110 | 111 | bool DockPerception::stop() { 112 | running_ = false; 113 | return true; 114 | } 115 | 116 | bool DockPerception::getPose(geometry_msgs::msg::PoseStamped& pose, 117 | std::string frame) { 118 | // All of this requires a lock on the dock_ 119 | std::lock_guard lock(dock_mutex_); 120 | 121 | if (!found_dock_) { 122 | return false; 123 | } 124 | 125 | // TODO: add in timeout function. 126 | /* 127 | if (ros::Time::now() > dock_stamp_ + ros::Duration(0.35)) { 128 | ROS_DEBUG_NAMED("dock_perception", "Dock pose timed out"); 129 | return false; 130 | } 131 | */ 132 | // Check for a valid orientation. 133 | tf2::Quaternion q; 134 | // change here 135 | tf2::fromMsg(dock_.pose.orientation, q); 136 | if (!isValid(q)) { 137 | std::cout << "Dock orientation invalid.\n"; 138 | return false; 139 | } 140 | 141 | pose = dock_; 142 | 143 | if (frame != "") { 144 | // Transform to desired frame 145 | try { 146 | listener_.waitTransform(frame, pose.header.frame_id); 147 | listener_.transformPose(frame, pose, pose); 148 | } catch (const tf2::TransformException& ex) { 149 | std::cout << "Couldn't transform dock pose\n"; 150 | return false; 151 | } 152 | } 153 | 154 | return found_dock_; 155 | } 156 | 157 | void DockPerception::callback( 158 | const sensor_msgs::msg::LaserScan::SharedPtr scan) { 159 | // Be lazy about search 160 | if (!running_) { 161 | return; 162 | } 163 | // lock dock_ to prevent other functions from modifying dock_ 164 | std::lock_guard lock(dock_mutex_); 165 | // Make sure goal is valid (orientation != 0 0 0 0) 166 | if (dock_.header.frame_id == "" || 167 | (dock_.pose.orientation.z == 0.0 && dock_.pose.orientation.w == 0.0)) { 168 | // dock_ is of type geometry_msgs::msg::PoseStamped 169 | // If goal is invalid, set to a point directly ahead of robot 170 | dock_.header = scan->header; 171 | for (size_t i = scan->ranges.size() / 2; i < scan->ranges.size(); i++) { 172 | if (std::isfinite(scan->ranges[i])) { 173 | double angle = scan->angle_min + i * scan->angle_increment; 174 | dock_.pose.position.x = cos(angle) * scan->ranges[i]; 175 | dock_.pose.position.y = sin(angle) * scan->ranges[i]; 176 | dock_.pose.orientation.x = 1.0; 177 | dock_.pose.orientation.y = 0.0; 178 | dock_.pose.orientation.z = 0.0; 179 | dock_.pose.orientation.w = 0.0; 180 | break; 181 | } 182 | } 183 | } 184 | 185 | // Make sure goal is in the tracking frame 186 | if (dock_.header.frame_id != tracking_frame_) { 187 | try { 188 | // wait for the transform between the frame the dock is currently 189 | // referenced to and the main reference frame 190 | listener_.waitTransform(tracking_frame_, dock_.header.frame_id); 191 | 192 | listener_.transformPose(tracking_frame_, dock_, dock_); 193 | } catch (const tf2::TransformException& ex) { 194 | std::cout << "Couldn't transform dock pose to tracking frame"; 195 | return; 196 | } 197 | // TODO: Add in ROS2 debug info 198 | // ROS_DEBUG_NAMED("dock_perception", 199 | // "Transformed initial pose to (%f, %f, %f)", 200 | // dock_.pose.position.x, dock_.pose.position.y, 201 | // icp_2d::thetaFromQuaternion(dock_.pose.orientation)); 202 | } 203 | 204 | // Cluster the laser scan 205 | laser_processor::ScanMask mask; 206 | laser_processor::ScanProcessor processor(*scan, mask); 207 | processor.splitConnected(0.04); // TODO(enhancement) parameterize 208 | processor.removeLessThan(5); 209 | 210 | // Sort clusters based on distance to last dock 211 | std::priority_queue, 212 | CompareCandidates> 213 | candidates; 214 | for (std::list::iterator i = 215 | processor.getClusters().begin(); 216 | i != processor.getClusters().end(); i++) { 217 | DockCandidatePtr c = extract(*i); 218 | if (c && c->valid(found_dock_)) { 219 | candidates.push(c); 220 | } 221 | } 222 | 223 | // Extract ICP pose/fit on best clusters 224 | DockCandidatePtr best; 225 | geometry_msgs::msg::Pose best_pose; 226 | while (!candidates.empty()) { 227 | geometry_msgs::msg::Pose pose = dock_.pose; 228 | double score = fit(candidates.top(), pose); 229 | if (score >= 0) { 230 | best = candidates.top(); 231 | best_pose = pose; 232 | break; 233 | } 234 | /* TODO: Add in pointcloud visualisation feature in ros2 235 | else // Let's see what's wrong with this point cloud. 236 | { 237 | if (debug_) { 238 | DockCandidatePtr not_best = candidates.top(); 239 | 240 | // Create point cloud 241 | sensor_msgs::msg::PointCloud2 cloud; 242 | cloud.header.stamp = scan->header.stamp; 243 | cloud.header.frame_id = tracking_frame_; 244 | cloud.width = cloud.height = 0; 245 | 246 | // Allocate space for points 247 | sensor_msgs::PointCloud2Modifier cloud_mod(cloud); 248 | cloud_mod.setPointCloud2FieldsByString(1, "xyz"); 249 | cloud_mod.resize(not_best->points.size()); 250 | 251 | // Fill in points 252 | sensor_msgs::PointCloud2Iterator cloud_iter(cloud, "x"); 253 | for (size_t i = 0; i < not_best->points.size(); i++) { 254 | cloud_iter[0] = not_best->points[i].x; 255 | cloud_iter[1] = not_best->points[i].y; 256 | cloud_iter[2] = not_best->points[i].z; 257 | ++cloud_iter; 258 | } 259 | debug_points_.publish(cloud); 260 | } 261 | } 262 | */ 263 | candidates.pop(); 264 | } 265 | // Did we find dock? 266 | if (best.use_count() == 0) { 267 | std::cout << "DID NOT FIND THE DOCK!\n"; 268 | return; 269 | } 270 | 271 | /* 272 | // Update 273 | if (debug_) { 274 | // Create point cloud 275 | sensor_msgs::msg::PointCloud2 cloud; 276 | cloud.header.stamp = scan->header.stamp; 277 | cloud.header.frame_id = tracking_frame_; 278 | cloud.width = cloud.height = 0; 279 | 280 | // Allocate space for points 281 | sensor_msgs::PointCloud2Modifier cloud_mod(cloud); 282 | cloud_mod.setPointCloud2FieldsByString(1, "xyz"); 283 | cloud_mod.resize(best->points.size()); 284 | 285 | // Fill in points 286 | sensor_msgs::PointCloud2Iterator cloud_iter(cloud, "x"); 287 | for (size_t i = 0; i < best->points.size(); i++) { 288 | cloud_iter[0] = best->points[i].x; 289 | cloud_iter[1] = best->points[i].y; 290 | cloud_iter[2] = best->points[i].z; 291 | ++cloud_iter; 292 | } 293 | debug_points_.publish(cloud); 294 | } 295 | */ 296 | 297 | // Update stamp 298 | dock_.header.stamp = scan->header.stamp; 299 | dock_.header.frame_id = tracking_frame_; 300 | 301 | // If this is the first time we've found dock, take whole pose 302 | if (!found_dock_) { 303 | dock_.pose = best_pose; 304 | // Reset the dock pose filter. 305 | dock_pose_filter_->reset(); 306 | // Set the filter state to the current pose estimate. 307 | dock_pose_filter_->setFilterState(dock_.pose, dock_.pose); 308 | } else { 309 | // Check that pose is not too far from current pose 310 | double d = getPoseDistance(dock_.pose, best_pose); 311 | if (d > 0.05) { 312 | std::cout << "Dock pose jumped: " << d << "\n"; 313 | return; 314 | } 315 | } 316 | 317 | // Filter the pose esitmate. 318 | dock_.pose = dock_pose_filter_->filter(best_pose); 319 | dock_stamp_ = scan->header.stamp; 320 | found_dock_ = true; 321 | } 322 | 323 | DockCandidatePtr DockPerception::extract(laser_processor::SampleSet* cluster) { 324 | DockCandidatePtr candidate = std::make_shared(); 325 | 326 | tf2::Vector3 tf_point; 327 | geometry_msgs::msg::TransformStamped t_frame; 328 | tf2::Stamped tf2_t_frame; 329 | try { 330 | listener_.waitTransform(tracking_frame_, cluster->header.frame_id); 331 | 332 | t_frame = listener_.getTransform(tracking_frame_, cluster->header.frame_id); 333 | } catch (const tf2::TransformException& ex) { 334 | std::cout << "ERROR. COULD NOT TRANSFORM POINT\n"; 335 | return candidate; 336 | } 337 | // Transform each point into tracking frame 338 | size_t i = 0; 339 | for (laser_processor::SampleSet::iterator p = cluster->begin(); 340 | p != cluster->end(); p++, i++) { 341 | geometry_msgs::msg::PointStamped pt; 342 | pt.header = cluster->header; 343 | 344 | tf2::fromMsg(t_frame, tf2_t_frame); 345 | tf2::Vector3 tf2_point((*p)->x, (*p)->y, 0); 346 | // apply transform operation 347 | tf2_point = tf2_t_frame * tf2_point; 348 | pt.point.x = tf2_point.getX(); 349 | pt.point.y = tf2_point.getY(); 350 | pt.point.z = 0; 351 | candidate->points.push_back(pt.point); 352 | } 353 | 354 | // Get distance from cloud center to previous pose 355 | geometry_msgs::msg::Point centroid = icp_2d::getCentroid(candidate->points); 356 | double dx = centroid.x - dock_.pose.position.x; 357 | double dy = centroid.y - dock_.pose.position.y; 358 | candidate->dist = (dx * dx + dy * dy); 359 | 360 | return candidate; 361 | } 362 | 363 | double DockPerception::fit(const DockCandidatePtr& candidate, 364 | geometry_msgs::msg::Pose& pose) { 365 | // Setup initial pose 366 | geometry_msgs::msg::Transform transform; 367 | transform.translation.x = pose.position.x; 368 | transform.translation.y = pose.position.y; 369 | transform.rotation = pose.orientation; 370 | 371 | tf2::Quaternion yaw_converter; 372 | 373 | // Initial yaw. Presumably the initial goal orientation estimate. 374 | tf2::Quaternion init_pose, cand_pose; 375 | tf2::fromMsg(pose.orientation, init_pose); 376 | if (!isValid(init_pose)) { 377 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), 378 | "Initial dock orientation estimate is invalid."); 379 | return -1.0; 380 | } 381 | 382 | // ICP the dock 383 | double fitness = icp_2d::alignICP(ideal_cloud_, candidate->points, transform); 384 | tf2::fromMsg(transform.rotation, cand_pose); 385 | 386 | if (!isValid(cand_pose)) { 387 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), 388 | "Dock candidate orientation estimate is invalid."); 389 | } 390 | 391 | // If the dock orientation seems flipped, flip it. 392 | // Check it by finding the relative roation between the two quaternions. 393 | if (fabs(angles::normalize_angle(tf2::getYaw( 394 | cand_pose.inverse() * init_pose))) > 3.1415 * (2.0 / 3.0)) { 395 | double yaw_angle = 3.1415 + tf2::getYaw(transform.rotation); 396 | yaw_converter.setRPY(0, 0, yaw_angle); 397 | 398 | transform.rotation.x = yaw_converter.x(); 399 | transform.rotation.y = yaw_converter.y(); 400 | transform.rotation.z = yaw_converter.z(); 401 | transform.rotation.w = yaw_converter.w(); 402 | } 403 | 404 | if (fitness >= 0.0) { 405 | // Initialize the number of times we retry if the fitness is bad. 406 | double retry = 5; 407 | // If the fitness is hosed or the angle is borked, try again. 408 | tf2::fromMsg(transform.rotation, cand_pose); 409 | while (retry-- && (fitness > max_alignment_error_ || 410 | fabs(angles::normalize_angle(tf2::getYaw( 411 | cand_pose.inverse() * init_pose))) > 3.1415 / 4.0)) { 412 | // Try one more time. 413 | 414 | // Perturb the pose to try to get it out of the local minima. 415 | transform.translation.x += 416 | retry * (0.75 / 100.0) * static_cast((rand() % 200) - 100); 417 | transform.translation.y += 418 | retry * (0.75 / 100.0) * static_cast((rand() % 200) - 100); 419 | yaw_converter.setRPY( 420 | 0, 0, 421 | retry * (0.28 / 100.0) * double((rand() % 200) - 100) + 422 | tf2::getYaw(transform.rotation)); 423 | 424 | transform.rotation.x = yaw_converter.x(); 425 | transform.rotation.y = yaw_converter.y(); 426 | transform.rotation.z = yaw_converter.z(); 427 | transform.rotation.w = yaw_converter.w(); 428 | 429 | // Rerun ICP. 430 | fitness = icp_2d::alignICP(ideal_cloud_, candidate->points, transform); 431 | 432 | // If the dock orientation seems flipped, flip it. 433 | tf2::fromMsg(transform.rotation, cand_pose); 434 | if (fabs(angles::normalize_angle(tf2::getYaw( 435 | cand_pose.inverse() * init_pose))) > 3.1415 * (2.0 / 3.0)) { 436 | yaw_converter.setRPY(0, 0, (3.1415 + tf2::getYaw(transform.rotation))); 437 | transform.rotation.x = yaw_converter.x(); 438 | transform.rotation.y = yaw_converter.y(); 439 | transform.rotation.z = yaw_converter.z(); 440 | transform.rotation.w = yaw_converter.w(); 441 | } 442 | } 443 | // If the dock orientation is still really borked, fail. 444 | tf2::fromMsg(transform.rotation, cand_pose); 445 | if (!isValid(cand_pose)) { 446 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), 447 | "Dock candidate orientation estimate is invalid."); 448 | return -1.0; 449 | } 450 | if (fabs(angles::normalize_angle( 451 | tf2::getYaw(cand_pose.inverse() * init_pose))) > 3.1415 / 2.0) { 452 | fitness = -1.0; 453 | } 454 | 455 | // Check that fitness is good enough 456 | if (!found_dock_ && fabs(fitness) > max_alignment_error_) { 457 | // If not, signal no fit 458 | fitness = -1.0; 459 | } 460 | 461 | // If width of candidate is smaller than the width of dock 462 | // then the whole dock is not visible... 463 | if (candidate->width() < 0.375) { 464 | // ... and heading is unreliable when close to dock 465 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), 466 | "Dock candidate width is unreliable."); 467 | transform.rotation = pose.orientation; 468 | fitness = 0.001234; 469 | // Probably can use a different algorithm here, if necessary, which it 470 | // might not be. 471 | } 472 | 473 | // Transform ideal cloud, and store for visualization 474 | candidate->points = icp_2d::transform( 475 | ideal_cloud_, transform.translation.x, transform.translation.y, 476 | icp_2d::thetaFromQuaternion(transform.rotation)); 477 | 478 | // Get pose 479 | pose.position.x = transform.translation.x; 480 | pose.position.y = transform.translation.y; 481 | pose.position.z = transform.translation.z; 482 | pose.orientation = transform.rotation; 483 | 484 | return fitness; 485 | } 486 | 487 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Did not converge"); 488 | return -1.0; 489 | } 490 | 491 | bool DockPerception::isValid(const tf2::Quaternion& q) { 492 | return 1e-3 >= fabs(1.0 - q.length()); 493 | } 494 | -------------------------------------------------------------------------------- /src/tf2listener.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void tf2_listener::waitTransform(std::string origin, std::string destination) { 4 | std::string warning_msg; 5 | while (rclcpp::ok() && 6 | !buffer_.canTransform(origin, destination, tf2::TimePoint(), 7 | &warning_msg)) { 8 | std::cout << "waiting: " << warning_msg << "\n"; 9 | rate.sleep(); 10 | } 11 | } 12 | 13 | geometry_msgs::msg::TransformStamped tf2_listener::getTransform( 14 | std::string origin, std::string destination) { 15 | geometry_msgs::msg::TransformStamped echo_transform; 16 | echo_transform = 17 | buffer_.lookupTransform(origin, destination, tf2::TimePoint()); 18 | 19 | return echo_transform; 20 | } 21 | 22 | void tf2_listener::transformPose(std::string tracking_frame, 23 | geometry_msgs::msg::PoseStamped &input_pose, 24 | geometry_msgs::msg::PoseStamped &output_pose) { 25 | // firstly, get the correct desired transformation. 26 | geometry_msgs::msg::TransformStamped corrective_transform; 27 | corrective_transform = 28 | getTransform(tracking_frame, input_pose.header.frame_id); 29 | // transform the input pose to be referenced to the main tracking frame. 30 | tf2::doTransform(input_pose, output_pose, corrective_transform); 31 | } 32 | --------------------------------------------------------------------------------