├── doc └── .gitignore ├── tf2_route_planning_msgs ├── src │ └── tf2_route_planning_msgs │ │ ├── __init__.py │ │ └── tf2_route_planning_msgs.py ├── package.xml ├── CMakeLists.txt ├── test │ ├── test_tf2_route_planning_msgs.cpp │ └── test_tf2_route_planning_msgs.py └── include │ └── tf2_route_planning_msgs │ └── tf2_route_planning_msgs.hpp ├── assets ├── teaser.png ├── route_planning_msgs.png └── route_planning_msgs.pptx ├── route_planning_msgs_rviz_plugins ├── icons │ └── classes │ │ ├── Route.png │ │ ├── PlanRoute.png │ │ └── ReferencePath.png ├── route_planning_msgs_rviz_plugins-extras.cmake ├── plugins_description.xml ├── package.xml ├── include │ └── route_planning_msgs │ │ └── tools │ │ └── route │ │ ├── plan_route_tool.hpp │ │ └── ref_path_tool.hpp └── CMakeLists.txt ├── trajectory_planning_msgs_rviz_plugins ├── icons │ └── classes │ │ └── Trajectory.png ├── trajectory_planning_msgs_rviz_plugins-extras.cmake ├── plugins_description.xml ├── package.xml ├── include │ └── trajectory_planning_msgs │ │ └── displays │ │ └── trajectory_display.hpp └── CMakeLists.txt ├── route_planning_msgs ├── msg │ ├── LaneBoundary.msg │ ├── LaneElement.msg │ ├── Route.msg │ ├── RouteElement.msg │ └── RegulatoryElement.msg ├── action │ └── PlanRoute.action ├── CMakeLists.txt └── package.xml ├── trajectory_planning_msgs ├── msg │ ├── Trajectory.msg │ ├── REFERENCE.msg │ ├── DRIVABLE.msg │ └── DRIVABLERWS.msg ├── CMakeLists.txt └── package.xml ├── .github └── workflows │ ├── industrial_ci.yml │ ├── doc.yml │ └── downstream_projects.yml ├── LICENSE ├── route_planning_msgs_utils ├── package.xml ├── src │ └── route_planning_msgs_utils │ │ ├── __init__.py │ │ ├── utils.py │ │ ├── route_setters.py │ │ └── route_getters.py ├── include │ └── route_planning_msgs_utils │ │ ├── route_access.hpp │ │ ├── utils.h │ │ └── route_setters.h ├── CMakeLists.txt └── test │ ├── test_route_access.cpp │ └── test_route_access.py ├── tf2_trajectory_planning_msgs ├── src │ └── tf2_trajectory_planning_msgs │ │ ├── __init__.py │ │ └── tf2_trajectory_planning_msgs.py ├── test │ ├── test_tf2_trajectory_planning_msgs.cpp │ ├── test_tf2_trajectory_planning_msgs.ros2.cpp │ ├── test_tf2_trajectory_planning_msgs.py │ └── impl │ │ └── test_tf2_trajectory_planning_msgs.cpp ├── package.xml ├── include │ └── tf2_trajectory_planning_msgs │ │ ├── tf2_trajectory_planning_msgs.h │ │ ├── tf2_trajectory_planning_msgs.hpp │ │ └── impl │ │ └── tf2_trajectory_planning_msgs.h └── CMakeLists.txt └── trajectory_planning_msgs_utils ├── test ├── test_trajectory_access.cpp ├── test_trajectory_access.ros2.cpp ├── test_trajectory_access.py └── impl │ └── test_trajectory_access.cpp ├── package.xml ├── src └── trajectory_planning_msgs_utils │ ├── __init__.py │ ├── init.py │ ├── utils.py │ ├── checks.py │ └── state_index.py ├── include └── trajectory_planning_msgs_utils │ ├── trajectory_access.h │ ├── impl │ ├── trajectory_access.h │ ├── init.h │ ├── utils.h │ ├── checks.h │ └── state_index.h │ └── trajectory_access.hpp └── CMakeLists.txt /doc/.gitignore: -------------------------------------------------------------------------------- 1 | html/ -------------------------------------------------------------------------------- /tf2_route_planning_msgs/src/tf2_route_planning_msgs/__init__.py: -------------------------------------------------------------------------------- 1 | from .tf2_route_planning_msgs import * -------------------------------------------------------------------------------- /assets/teaser.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ika-rwth-aachen/planning_interfaces/HEAD/assets/teaser.png -------------------------------------------------------------------------------- /assets/route_planning_msgs.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ika-rwth-aachen/planning_interfaces/HEAD/assets/route_planning_msgs.png -------------------------------------------------------------------------------- /assets/route_planning_msgs.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ika-rwth-aachen/planning_interfaces/HEAD/assets/route_planning_msgs.pptx -------------------------------------------------------------------------------- /route_planning_msgs_rviz_plugins/icons/classes/Route.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ika-rwth-aachen/planning_interfaces/HEAD/route_planning_msgs_rviz_plugins/icons/classes/Route.png -------------------------------------------------------------------------------- /route_planning_msgs_rviz_plugins/icons/classes/PlanRoute.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ika-rwth-aachen/planning_interfaces/HEAD/route_planning_msgs_rviz_plugins/icons/classes/PlanRoute.png -------------------------------------------------------------------------------- /route_planning_msgs_rviz_plugins/icons/classes/ReferencePath.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ika-rwth-aachen/planning_interfaces/HEAD/route_planning_msgs_rviz_plugins/icons/classes/ReferencePath.png -------------------------------------------------------------------------------- /trajectory_planning_msgs_rviz_plugins/icons/classes/Trajectory.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ika-rwth-aachen/planning_interfaces/HEAD/trajectory_planning_msgs_rviz_plugins/icons/classes/Trajectory.png -------------------------------------------------------------------------------- /route_planning_msgs/msg/LaneBoundary.msg: -------------------------------------------------------------------------------- 1 | uint8 TYPE_UNKNOWN=0 2 | uint8 TYPE_CROSSING_RESTRICTED=1 3 | uint8 TYPE_CROSSING_ALLOWED=2 4 | uint8 TYPE_CROSSING_ALLOWED_FROM_LEFT=3 5 | uint8 TYPE_CROSSING_ALLOWED_FROM_RIGHT=4 6 | uint8 type 7 | 8 | geometry_msgs/Point point -------------------------------------------------------------------------------- /route_planning_msgs_rviz_plugins/route_planning_msgs_rviz_plugins-extras.cmake: -------------------------------------------------------------------------------- 1 | # find package Qt5 because otherwise using the rviz_default_plugins::rviz_default_plugins 2 | # exported target will complain that the Qt5::Widgets target does not exist 3 | find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets) -------------------------------------------------------------------------------- /trajectory_planning_msgs_rviz_plugins/trajectory_planning_msgs_rviz_plugins-extras.cmake: -------------------------------------------------------------------------------- 1 | # find package Qt5 because otherwise using the rviz_default_plugins::rviz_default_plugins 2 | # exported target will complain that the Qt5::Widgets target does not exist 3 | find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets) -------------------------------------------------------------------------------- /trajectory_planning_msgs/msg/Trajectory.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # to ensure that the type_id is unique, all existing trajectory types and their corresponding id's are listed here: 4 | # REFERENCE: 0 5 | # DRIVABLE: 1 6 | # DRIVABLERWS: 2 7 | uint8 type_id 8 | 9 | # flatten sample-point-state matrix according to type (sample points x states) 10 | float64[] states 11 | 12 | # quality score of trajectory 13 | uint8 score 14 | 15 | # is standstill trajectory 16 | bool standstill -------------------------------------------------------------------------------- /.github/workflows/industrial_ci.yml: -------------------------------------------------------------------------------- 1 | name: industrial_ci 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | name: ROS ${{ matrix.ROS_DISTRO }} 8 | runs-on: ubuntu-latest 9 | strategy: 10 | fail-fast: false 11 | matrix: 12 | ROS_DISTRO: [humble, jazzy, rolling] 13 | ROS_REPO: [testing, main] 14 | steps: 15 | - uses: actions/checkout@v4 16 | - uses: ros-industrial/industrial_ci@master 17 | with: 18 | config: ${{ toJSON(matrix) }} -------------------------------------------------------------------------------- /trajectory_planning_msgs_rviz_plugins/plugins_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 9 | 10 | Displays data from a trajectory_planning_msgs::Trajectory message. 11 | 12 | trajectory_planning_msgs/msg/Trajectory 13 | 14 | 15 | -------------------------------------------------------------------------------- /.github/workflows/doc.yml: -------------------------------------------------------------------------------- 1 | name: doc 2 | 3 | on: 4 | push: 5 | branches: 6 | - main 7 | 8 | jobs: 9 | build: 10 | runs-on: ubuntu-latest 11 | steps: 12 | - name: Checkout repository 13 | uses: actions/checkout@v4 14 | with: 15 | submodules: true 16 | - name: Run Doxygen 17 | uses: mattnotmitt/doxygen-action@v1.9.8 18 | with: 19 | working-directory: doc 20 | doxyfile-path: Doxyfile 21 | - name: Publish to GitHub Pages 22 | uses: peaceiris/actions-gh-pages@v4 23 | if: github.ref == 'refs/heads/main' 24 | with: 25 | github_token: ${{ secrets.GITHUB_TOKEN }} 26 | publish_dir: doc/html -------------------------------------------------------------------------------- /route_planning_msgs/msg/LaneElement.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose reference_pose # pose on reference path of lane 2 | 3 | LaneBoundary left_boundary # left boundary of lane (only if is_enriched) 4 | LaneBoundary right_boundary # right boundary of lane (only if is_enriched) 5 | 6 | uint8 speed_limit # speed limit [km/h] (0=unknown, 255=unlimited) 7 | uint8[] regulatory_element_idcs # indices of regulatory elements from corresponding RouteElement belonging to this LaneElement (only if is_enriched) 8 | 9 | uint8 following_lane_idx # index of following LaneElement in following RouteElement 10 | bool has_following_lane_idx # whether following_lane_idx is valid -------------------------------------------------------------------------------- /route_planning_msgs/msg/Route.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | geometry_msgs/Point destination # destination (potentially mapped to suggested lane reference line) 4 | geometry_msgs/Point[] intermediate_destinations # route should be planned over those points (could be empty) 5 | 6 | RouteElement[] route_elements # cross sections of drivable adjacent lanes in driving direction towards destination 7 | 8 | uint64 starting_route_element_idx # index of the route element right before the starting point 9 | uint64 current_route_element_idx # index of the first route element of the remaining route, right before the current position 10 | uint64 destination_route_element_idx # index of the route element right behind the destination 11 | -------------------------------------------------------------------------------- /route_planning_msgs/action/PlanRoute.action: -------------------------------------------------------------------------------- 1 | # goal 2 | geometry_msgs/PointStamped destination # route destination 3 | geometry_msgs/PointStamped[] intermediate_destinations # route should be planned over those points (could be empty) 4 | --- 5 | # result 6 | bool destination_reached # if destination was reached 7 | float64 distance_traveled # distance traveled since start 8 | builtin_interfaces/Duration time_traveled # time traveled since start 9 | --- 10 | # feedback 11 | float64 distance_remaining # distance remaining to destination 12 | float64 distance_traveled # distance traveled since start 13 | builtin_interfaces/Duration time_remaining # remaining time to destination 14 | builtin_interfaces/Duration time_traveled # time traveled since start 15 | -------------------------------------------------------------------------------- /route_planning_msgs/msg/RouteElement.msg: -------------------------------------------------------------------------------- 1 | LaneElement[] lane_elements # cross sections of lanes in driving direction (from left to right) 2 | 3 | uint8 suggested_lane_idx # index of suggested lane within lane_elements 4 | bool will_change_suggested_lane # whether suggested path will change lanes from this RouteElement to the next 5 | 6 | float64 s # accumulated distance from start of route [m] 7 | 8 | bool is_enriched # whether this RouteElement is enriched with lane boundaries, adjacent lanes, drivable space, regulatory elements 9 | 10 | geometry_msgs/Point left_boundary # left boundary of drivable space (only if is_enriched) 11 | geometry_msgs/Point right_boundary # right boundary of drivable space (only if is_enriched) 12 | 13 | RegulatoryElement[] regulatory_elements # regulatory elements belonging to this RouteElement (referring at least one LaneElement) (only if is_enriched) -------------------------------------------------------------------------------- /route_planning_msgs/msg/RegulatoryElement.msg: -------------------------------------------------------------------------------- 1 | uint8 TYPE_UNKNOWN=0 2 | uint8 TYPE_YIELD=1 3 | uint8 TYPE_STOP=2 4 | uint8 TYPE_TRAFFIC_LIGHT=3 5 | uint8 TYPE_RIGHT_OF_WAY=4 6 | uint8 TYPE_SPEED_LIMIT=5 7 | uint8 type 8 | 9 | uint8 META_VALUE_UNKNOWN=0 10 | # meta values for YIELD, STOP, TRAFFIC_LIGHT 11 | uint8 META_VALUE_MOVEMENT_ALLOWED=1 # movement is allowed 12 | uint8 META_VALUE_MOVEMENT_RESTRICTED=2 # movement is restricted 13 | # meta values for SPEED_LIMIT [km/h] 14 | uint8 META_VALUE_SPEED_UNKNOWN=0 # speed limit is unknown 15 | uint8 META_VALUE_SPEED_UNLIMITED=255 # no speed limit 16 | uint8 meta_value 17 | 18 | bool has_validity_stamp # whether validity_stamp is set 19 | builtin_interfaces/Time validity_stamp # regulatory element is valid until this time (e.g., signal chage of traffic light) 20 | 21 | geometry_msgs/Point[2] reference_line # reference/effect line of regulatory element (e.g., stop line) 22 | geometry_msgs/Point[] positions # position of signals/signs indicating the regulatory element (e.g., positions of traffic lights) -------------------------------------------------------------------------------- /trajectory_planning_msgs/msg/REFERENCE.msg: -------------------------------------------------------------------------------- 1 | ########################################################################################################################################################### 2 | # REFERENCE Trajectory (independent of vehicle model) 3 | ########################################################################################################################################################### 4 | uint8 TYPE_ID=0 5 | uint8 STATE_DIM=4 6 | 7 | # sample points of REFERENCE trajectories have the following entries 8 | # state information for each sample point: 9 | uint8 T=0 # time to reach sample point [s] 10 | uint8 X=1 # x position of sample point in trajectory header frame [m] 11 | uint8 Y=2 # y position of sample point in trajectory header frame [m] 12 | uint8 V=3 # absolute velocity in vehicle frame [m/s] 13 | ########################################################################################################################################################### 14 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /trajectory_planning_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(trajectory_planning_msgs) 3 | 4 | find_package(ros_environment REQUIRED QUIET) 5 | set(ROS_VERSION $ENV{ROS_VERSION}) 6 | 7 | # === ROS2 (AMENT) ============================================================= 8 | if(${ROS_VERSION} EQUAL 2) 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | # find dependencies 15 | find_package(ament_cmake REQUIRED) 16 | find_package(rosidl_default_generators REQUIRED) 17 | find_package(std_msgs REQUIRED) 18 | 19 | rosidl_generate_interfaces(${PROJECT_NAME} 20 | "msg/Trajectory.msg" 21 | "msg/DRIVABLE.msg" 22 | "msg/DRIVABLERWS.msg" 23 | "msg/REFERENCE.msg" 24 | DEPENDENCIES std_msgs 25 | ) 26 | 27 | ament_export_dependencies(rosidl_default_runtime) 28 | 29 | ament_package() 30 | 31 | # === ROS1 (CATKIN) ============================================================ 32 | elseif(${ROS_VERSION} EQUAL 1) 33 | 34 | find_package(catkin REQUIRED COMPONENTS 35 | message_generation 36 | std_msgs 37 | ) 38 | 39 | add_message_files(DIRECTORY msg) 40 | 41 | generate_messages( 42 | DEPENDENCIES 43 | std_msgs 44 | ) 45 | 46 | catkin_package( 47 | CATKIN_DEPENDS 48 | message_runtime 49 | std_msgs 50 | ) 51 | 52 | endif() -------------------------------------------------------------------------------- /route_planning_msgs_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | route_planning_msgs_utils 5 | 0.0.0 6 | Utility functions for route_planning_msgs 7 | Jean-Pierre Busch 8 | MIT 9 | 10 | ros_environment 11 | std_msgs 12 | route_planning_msgs 13 | 14 | 15 | ament_cmake 16 | ament_cmake_python 17 | 18 | ament_cmake_gtest 19 | ament_cmake_pytest 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | catkin 25 | 26 | 27 | ament_cmake 28 | catkin 29 | 30 | 31 | -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/src/tf2_trajectory_planning_msgs/__init__.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | # ============================================================================ 23 | 24 | from .tf2_trajectory_planning_msgs import * # noqa: F401,F403 25 | -------------------------------------------------------------------------------- /route_planning_msgs_utils/src/route_planning_msgs_utils/__init__.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | from .utils import * 26 | from .route_getters import * 27 | from .route_setters import * -------------------------------------------------------------------------------- /trajectory_planning_msgs/msg/DRIVABLE.msg: -------------------------------------------------------------------------------- 1 | ########################################################################################################################################################### 2 | # DRIVABLE Trajectory for bicycle model (Ackermann steering) 3 | ########################################################################################################################################################### 4 | uint8 TYPE_ID=1 5 | uint8 STATE_DIM=8 6 | 7 | # sample points of DRIVABLE trajectories have the following entries 8 | # state information for each sample point: 9 | uint8 T=0 # time to reach sample point [s] 10 | uint8 X=1 # x position of sample point in trajectory header frame [m] 11 | uint8 Y=2 # y position of sample point in trajectory header frame [m] 12 | uint8 V=3 # longitudinal (="absolute", according to Ackermann) velocity in vehicle frame [m/s] 13 | uint8 THETA=4 # heading at sample point in trajectory header frame [rad] from [-pi, pi] 14 | uint8 A=5 # longitudinal (="absolute", according to Ackermann) acceleration in vehicle frame [m/s^2] 15 | uint8 DELTA=6 # Ackermann steering angle [rad] from [-pi, pi] 16 | uint8 S=7 # driven distance until sample point is reached in [m] 17 | ########################################################################################################################################################### 18 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/test/test_trajectory_access.cpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #include 26 | 27 | using namespace trajectory_planning_msgs; 28 | 29 | #include 30 | -------------------------------------------------------------------------------- /tf2_route_planning_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | tf2_route_planning_msgs 5 | 0.0.0 6 | tf2-support for route_planning_msgs 7 | Guido Küppers 8 | Jean-Pierre Busch 9 | Guido Küppers 10 | MIT 11 | 12 | geometry_msgs 13 | ros_environment 14 | route_planning_msgs 15 | 16 | 17 | tf2_ros_py 18 | ament_cmake 19 | ament_cmake_python 20 | ament_cmake_gtest 21 | ament_cmake_pytest 22 | route_planning_msgs_utils 23 | 24 | tf2 25 | 26 | 27 | catkin 28 | 29 | 30 | ament_cmake 31 | catkin 32 | 33 | 34 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/test/test_trajectory_access.ros2.cpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #include 26 | 27 | using namespace trajectory_planning_msgs; 28 | using namespace trajectory_planning_msgs::msg; 29 | 30 | #include -------------------------------------------------------------------------------- /route_planning_msgs_rviz_plugins/plugins_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 9 | 10 | Displays data from a route_planning_msgs::Route message. 11 | 12 | route_planning_msgs/msg/Route 13 | 14 | 15 | 16 | 21 | 22 | Publishes a reference path as route_planning_msgs::Route created in rviz. 23 | Right click to add suggested lane points, middle click to remove the last suggested lane point and left click to publish the route. 24 | 25 | route_planning_msgs/msg/Route 26 | 27 | 32 | 33 | Plans a route based on the user input in rviz and calls the route_planning_msgs::action::PlanRoute action. 34 | Right click to add intermediate destinations, middle click to remove the last intermediate destination and left click to set the destination and call the action. 35 | 36 | route_planning_msgs/action/PlanRoute 37 | 38 | 39 | -------------------------------------------------------------------------------- /.github/workflows/downstream_projects.yml: -------------------------------------------------------------------------------- 1 | name: Trigger downstream projects ci 2 | 3 | on: 4 | push: 5 | branches: 6 | - main 7 | 8 | jobs: 9 | 10 | trigger-ci: 11 | runs-on: ubuntu-latest 12 | steps: 13 | - name: Trigger pipeline 14 | run: | 15 | curl --silent --fail --request POST --form "token=${{ secrets.PLANNING_INTERFACES_CI_TRIGGER_GITLAB_TOKEN }}" --form "ref=main" "https://gitlab.ika.rwth-aachen.de/api/v4/projects/3126/trigger/pipeline" | jq -r .id > id 16 | - name: Upload pipeline ID 17 | uses: actions/upload-artifact@v4 18 | with: 19 | name: id_ci 20 | path: id 21 | 22 | watch-ci: 23 | runs-on: ubuntu-latest 24 | needs: trigger-ci 25 | steps: 26 | - name: Get pipeline ID 27 | uses: actions/download-artifact@v4 28 | with: 29 | name: id_ci 30 | - name: Wait for pipeline completion 31 | run: | 32 | PIPELINE_ID=$(cat id) 33 | while true; do 34 | sleep 30 35 | PIPELINE_STATUS=$(curl --silent --header "PRIVATE-TOKEN: ${{ secrets.PLANNING_INTERFACES_CI_READ_PIPELINE_GITLAB_TOKEN }}" "https://gitlab.ika.rwth-aachen.de/api/v4/projects/3126/pipelines/$PIPELINE_ID" | jq -r .status) 36 | echo "Pipeline status: $PIPELINE_STATUS (https://gitlab.ika.rwth-aachen.de/fb-fi/ops/planning_interfaces-ci/-/pipelines/$PIPELINE_ID)" 37 | if [[ $PIPELINE_STATUS == "success" ]]; then 38 | break 39 | elif [[ $PIPELINE_STATUS == "failed" ]]; then 40 | exit 1 41 | elif [[ $PIPELINE_STATUS == "canceled" ]]; then 42 | exit 1 43 | fi 44 | done -------------------------------------------------------------------------------- /trajectory_planning_msgs/msg/DRIVABLERWS.msg: -------------------------------------------------------------------------------- 1 | ########################################################################################################################################################### 2 | # DRIVABLE Trajectory for bicycle model with rear wheel steering (DRIVABLERWS) 3 | ########################################################################################################################################################### 4 | uint8 TYPE_ID=2 5 | uint8 STATE_DIM=10 6 | 7 | # sample points of DRIVABLERWS trajectories have the following entries 8 | # state information for each sample point: 9 | uint8 T=0 # time to reach sample point [s] 10 | uint8 X=1 # x position of sample point in trajectory header frame [m] 11 | uint8 Y=2 # y position of sample point in trajectory header frame [m] 12 | uint8 V=3 # absolute velocity in vehicle frame [m/s] 13 | uint8 THETA=4 # heading at sample point in trajectory header frame [rad] from [-pi, pi] 14 | uint8 A=5 # absolute acceleration in vehicle frame [m/s^2] 15 | uint8 BETA=6 # slip angle of the vehicle in center of gravity [rad] from [-pi, pi] 16 | uint8 DELTAFRONT=7 # steering angle of virtual front wheel [rad] from [-pi, pi] 17 | uint8 DELTAREAR=8 # steering angle of virtual rear wheel [rad] from [-pi, pi] 18 | uint8 S=9 # driven distance until sample point is reached in [m] 19 | ########################################################################################################################################################### 20 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | trajectory_planning_msgs_utils 5 | 0.0.0 6 | Utility functions for trajectory_planning_msgs 7 | Jean-Pierre Busch 8 | Jean-Pierre Busch 9 | Guido Küppers 10 | MIT 11 | 12 | ros_environment 13 | std_msgs 14 | trajectory_planning_msgs 15 | 16 | 17 | ament_cmake 18 | ament_cmake_python 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | ament_cmake_gtest 23 | ament_cmake_pytest 24 | 25 | 26 | catkin 27 | message_generation 28 | message_runtime 29 | 30 | 31 | ament_cmake 32 | catkin 33 | 34 | 35 | -------------------------------------------------------------------------------- /route_planning_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(route_planning_msgs) 3 | 4 | find_package(ros_environment REQUIRED QUIET) 5 | set(ROS_VERSION $ENV{ROS_VERSION}) 6 | 7 | # === ROS2 (AMENT) ============================================================= 8 | if(${ROS_VERSION} EQUAL 2) 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | # find dependencies 15 | find_package(ament_cmake REQUIRED) 16 | find_package(builtin_interfaces REQUIRED) 17 | find_package(geometry_msgs REQUIRED) 18 | find_package(rosidl_default_generators REQUIRED) 19 | find_package(std_msgs REQUIRED) 20 | 21 | rosidl_generate_interfaces(${PROJECT_NAME} 22 | "action/PlanRoute.action" 23 | "msg/LaneBoundary.msg" 24 | "msg/LaneElement.msg" 25 | "msg/RegulatoryElement.msg" 26 | "msg/Route.msg" 27 | "msg/RouteElement.msg" 28 | DEPENDENCIES std_msgs builtin_interfaces geometry_msgs 29 | ) 30 | 31 | ament_export_dependencies( 32 | builtin_interfaces 33 | geometry_msgs 34 | rosidl_default_runtime 35 | std_msgs 36 | ) 37 | 38 | ament_package() 39 | 40 | # === ROS1 (CATKIN) ============================================================ 41 | elseif(${ROS_VERSION} EQUAL 1) 42 | 43 | find_package(catkin REQUIRED COMPONENTS 44 | geometry_msgs 45 | message_generation 46 | std_msgs 47 | ) 48 | 49 | add_message_files(DIRECTORY msg) 50 | 51 | generate_messages( 52 | DEPENDENCIES 53 | geometry_msgs 54 | std_msgs 55 | ) 56 | 57 | catkin_package( 58 | CATKIN_DEPENDS 59 | geometry_msgs 60 | message_runtime 61 | std_msgs 62 | ) 63 | 64 | endif() 65 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/src/trajectory_planning_msgs_utils/__init__.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | from .utils import * # noqa: F401,F403 26 | from .checks import * # noqa: F401,F403 27 | from .state_index import * # noqa: F401,F403 28 | from .state_getters import * # noqa: F401,F403 29 | from .state_setters import * # noqa: F401,F403 30 | from .init import * # noqa: F401,F403 31 | -------------------------------------------------------------------------------- /trajectory_planning_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | trajectory_planning_msgs 5 | 0.0.0 6 | Message definitions related to trajectory planning tasks in Intelligent Transportation Systems 7 | Jean-Pierre Busch 8 | Jean-Pierre Busch 9 | Guido Küppers 10 | MIT 11 | 12 | ros_environment 13 | std_msgs 14 | 15 | 16 | ament_cmake 17 | 18 | rosidl_default_generators 19 | 20 | rosidl_default_runtime 21 | 22 | rosidl_interface_packages 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | catkin 29 | message_generation 30 | message_runtime 31 | 32 | 33 | ament_cmake 34 | catkin 35 | 36 | 37 | -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/test/test_tf2_trajectory_planning_msgs.cpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | namespace gm = geometry_msgs; 31 | using namespace trajectory_planning_msgs; 32 | 33 | #include 34 | -------------------------------------------------------------------------------- /route_planning_msgs_utils/include/route_planning_msgs_utils/route_access.hpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | 29 | namespace route_planning_msgs { 30 | namespace gm = geometry_msgs::msg; 31 | using namespace msg; 32 | } 33 | 34 | #include 35 | #include 36 | #include -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/include/trajectory_planning_msgs_utils/trajectory_access.h: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | // Messages 28 | #include 29 | 30 | // Trajectory Types 31 | #include 32 | #include 33 | #include 34 | 35 | #include -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/test/test_tf2_trajectory_planning_msgs.ros2.cpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | namespace gm = geometry_msgs::msg; 31 | using namespace trajectory_planning_msgs::msg; 32 | 33 | #include 34 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/include/trajectory_planning_msgs_utils/impl/trajectory_access.h: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/include/trajectory_planning_msgs_utils/trajectory_access.hpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | // Messages 28 | #include 29 | 30 | // Trajectory Types 31 | #include 32 | #include 33 | #include 34 | 35 | namespace trajectory_planning_msgs { 36 | using namespace msg; 37 | } 38 | 39 | #include -------------------------------------------------------------------------------- /route_planning_msgs_utils/src/route_planning_msgs_utils/utils.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | import math 26 | 27 | def wrap_angle(angle: float) -> float: 28 | """ 29 | Wraps an angle to the range [-π, π]. 30 | 31 | :param angle: The input angle in radians. 32 | :return: The wrapped angle in radians. 33 | """ 34 | wrapped_angle = angle 35 | while wrapped_angle > math.pi: 36 | wrapped_angle -= 2 * math.pi 37 | while wrapped_angle < -math.pi: 38 | wrapped_angle += 2 * math.pi 39 | return wrapped_angle -------------------------------------------------------------------------------- /route_planning_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | route_planning_msgs 5 | 0.0.0 6 | Message definitions and actions related to route planning tasks in Intelligent Transportation Systems 7 | Guido Küppers 8 | Jean-Pierre Busch 9 | Guido Küppers 10 | Lennart Reiher 11 | MIT 12 | 13 | geometry_msgs 14 | ros_environment 15 | std_msgs 16 | 17 | 18 | builtin_interfaces 19 | 20 | ament_cmake 21 | 22 | rosidl_default_generators 23 | 24 | rosidl_default_runtime 25 | 26 | rosidl_interface_packages 27 | 28 | ament_lint_auto 29 | ament_lint_common 30 | 31 | 32 | catkin 33 | message_generation 34 | message_runtime 35 | 36 | 37 | ament_cmake 38 | catkin 39 | 40 | 41 | -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | tf2_trajectory_planning_msgs 6 | 0.0.0 7 | tf2-support for trajectory_planning_msgs 8 | Jean-Pierre Busch 9 | Lennart Reiher 10 | Lukas Zanger 11 | Jean-Pierre Busch 12 | Guido Küppers 13 | MIT 14 | 15 | geometry_msgs 16 | ros_environment 17 | tf2_geometry_msgs 18 | tf2_ros 19 | tf2 20 | trajectory_planning_msgs_utils 21 | trajectory_planning_msgs 22 | 23 | 24 | ament_cmake 25 | ament_cmake_python 26 | rclcpp 27 | 28 | ament_lint_auto 29 | ament_lint_common 30 | ament_cmake_gtest 31 | ament_cmake_pytest 32 | 33 | 34 | catkin 35 | roscpp 36 | 37 | 38 | catkin 39 | ament_cmake 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /route_planning_msgs_utils/include/route_planning_msgs_utils/utils.h: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | 29 | namespace route_planning_msgs { 30 | 31 | namespace route_access { 32 | 33 | /** 34 | * @brief Wraps an angle to the range [-π, π]. 35 | * 36 | * @param angle The input angle in radians. 37 | * @return The wrapped angle in radians. 38 | */ 39 | inline double wrapAngle(const double& angle) { 40 | double wrapped_angle = angle; 41 | while (wrapped_angle > M_PI) wrapped_angle -= 2 * M_PI; 42 | while (wrapped_angle < -M_PI) wrapped_angle += 2 * M_PI; 43 | return wrapped_angle; 44 | } 45 | 46 | } // namespace route_access 47 | 48 | } // namespace route_planning_msgs -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/src/trajectory_planning_msgs_utils/init.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | # ============================================================================ 23 | 24 | from trajectory_planning_msgs.msg import Trajectory 25 | 26 | from .state_setters import set_states 27 | from .utils import get_state_dim 28 | 29 | 30 | STATE_INIT = 0.0 31 | 32 | 33 | def initialize_states(states, type_id: int, sample_points: int) -> None: 34 | vector_size = sample_points * get_state_dim(type_id) 35 | set_states(states, type_id, [STATE_INIT] * vector_size) 36 | 37 | 38 | def initialize_trajectory(trajectory: Trajectory, type_id: int, sample_points: int) -> None: 39 | initialize_states(trajectory.states, type_id, sample_points) 40 | trajectory.type_id = type_id 41 | trajectory.standstill = True 42 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/include/trajectory_planning_msgs_utils/impl/init.h: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | #include 29 | 30 | namespace trajectory_planning_msgs { 31 | 32 | namespace trajectory_access { 33 | 34 | const double STATE_INIT = 0; 35 | 36 | inline void initializeStates(std::vector& states, const unsigned char& type_id, 37 | const unsigned int nSamplePoints) { 38 | int vector_size = nSamplePoints * getStateDim(type_id); 39 | setStates(states, type_id, std::vector(vector_size, STATE_INIT)); 40 | } 41 | 42 | inline void initializeTrajectory(Trajectory& trajectory, const unsigned char& type_id, 43 | const unsigned int nSamplePoints) { 44 | initializeStates(trajectory.states, type_id, nSamplePoints); 45 | trajectory.type_id = type_id; 46 | trajectory.standstill = true; 47 | } 48 | 49 | } // namespace trajectory_access 50 | 51 | } // namespace trajectory_planning_msgs -------------------------------------------------------------------------------- /trajectory_planning_msgs_rviz_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | trajectory_planning_msgs_rviz_plugins 5 | 0.0.0 6 | This package provides rviz displays for the trajectory_planning_msgs 7 | Jean-Pierre Busch 8 | Jean-Pierre Busch 9 | Guido Küppers 10 | MIT 11 | 12 | geometry_msgs 13 | ros_environment 14 | std_msgs 15 | trajectory_planning_msgs 16 | trajectory_planning_msgs_utils 17 | 18 | 19 | tf2_ros 20 | tf2 21 | 22 | qtbase5-dev 23 | rviz_ogre_vendor 24 | 25 | rviz_ogre_vendor 26 | 27 | libqt5-core 28 | libqt5-gui 29 | libqt5-opengl 30 | libqt5-widgets 31 | rviz_ogre_vendor 32 | 33 | pluginlib 34 | rclcpp 35 | rviz_common 36 | rviz_default_plugins 37 | rviz_rendering 38 | 39 | ament_cmake 40 | 41 | 42 | catkin 43 | message_generation 44 | message_runtime 45 | 46 | 47 | ament_cmake 48 | catkin 49 | 50 | 51 | -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/include/tf2_trajectory_planning_msgs/tf2_trajectory_planning_msgs.h: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace tf2 { 34 | namespace gm = geometry_msgs; 35 | using namespace trajectory_planning_msgs; 36 | using Time = ros::Time; 37 | #ifndef STAMP2TIME 38 | #define STAMP2TIME 39 | inline const Time& stampToTime(const ros::Time& t) { return t; } 40 | #endif 41 | 42 | #ifndef STAMPS2TIMEDELTA 43 | #define STAMPS2TIMEDELTA 44 | /** 45 | * @brief Computes the time difference between two ROS time stamps. 46 | * 47 | * This function calculates the difference in seconds between two given ROS time stamps. 48 | * 49 | * @param t1 The first ROS time stamp. 50 | * @param t2 The second ROS time stamp. 51 | * @return The time difference (t2-t1) in seconds as a double. 52 | */ 53 | inline double stampsToTimeDelta(const ros::Time& t1, const ros::Time& t2) { return (t2-t1).toSec(); } 54 | #endif 55 | } // namespace tf2 56 | 57 | #include 58 | -------------------------------------------------------------------------------- /route_planning_msgs_rviz_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | route_planning_msgs_rviz_plugins 5 | 0.0.0 6 | This package provides rviz displays and tools for the route_planning_msgs 7 | Guido Küppers 8 | Jean-Pierre Busch 9 | Guido Küppers 10 | MIT 11 | 12 | geometry_msgs 13 | ros_environment 14 | std_msgs 15 | route_planning_msgs 16 | route_planning_msgs_utils 17 | tf2_geometry_msgs 18 | 19 | 20 | tf2 21 | tf2_geometry_msgs 22 | tf2_ros 23 | 24 | qtbase5-dev 25 | rviz_ogre_vendor 26 | 27 | rviz_ogre_vendor 28 | 29 | libqt5-core 30 | libqt5-gui 31 | libqt5-opengl 32 | libqt5-widgets 33 | rviz_ogre_vendor 34 | 35 | pluginlib 36 | rclcpp 37 | rviz_common 38 | rviz_default_plugins 39 | rviz_rendering 40 | 41 | ament_cmake 42 | 43 | 44 | catkin 45 | message_generation 46 | message_runtime 47 | 48 | 49 | ament_cmake 50 | catkin 51 | 52 | 53 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/src/trajectory_planning_msgs_utils/utils.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | import math 26 | from typing import Sequence 27 | 28 | from trajectory_planning_msgs.msg import ( 29 | DRIVABLE, 30 | DRIVABLERWS, 31 | REFERENCE, 32 | Trajectory, 33 | ) 34 | 35 | 36 | def wrap_angle(angle: float) -> float: 37 | """ 38 | Wrap an angle to the range [-π, π]. 39 | """ 40 | wrapped_angle = angle 41 | while wrapped_angle > math.pi: 42 | wrapped_angle -= 2 * math.pi 43 | while wrapped_angle < -math.pi: 44 | wrapped_angle += 2 * math.pi 45 | return wrapped_angle 46 | 47 | 48 | kExceptionUnknownType = "Unknown type ID: " 49 | 50 | 51 | def get_state_dim(type_id: int) -> int: 52 | if type_id == DRIVABLE.TYPE_ID: 53 | return DRIVABLE.STATE_DIM 54 | if type_id == DRIVABLERWS.TYPE_ID: 55 | return DRIVABLERWS.STATE_DIM 56 | if type_id == REFERENCE.TYPE_ID: 57 | return REFERENCE.STATE_DIM 58 | raise ValueError(kExceptionUnknownType + str(type_id)) 59 | 60 | 61 | def get_state_dim_from_trajectory(trajectory: Trajectory) -> int: 62 | return get_state_dim(trajectory.type_id) 63 | 64 | 65 | def get_states_size(states: Sequence[float]) -> int: 66 | return len(states) 67 | 68 | 69 | def get_states_size_from_trajectory(trajectory: Trajectory) -> int: 70 | return get_states_size(trajectory.states) 71 | 72 | 73 | def is_floating_division(numerator: int, denominator: int) -> bool: 74 | return numerator % denominator != 0 75 | -------------------------------------------------------------------------------- /route_planning_msgs_utils/include/route_planning_msgs_utils/route_setters.h: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | 29 | #include 30 | 31 | namespace route_planning_msgs { 32 | 33 | namespace route_access { 34 | 35 | inline void setLaneBoundary(LaneBoundary& lane_boundary, const gm::Point& point, const uint8_t type=LaneBoundary::TYPE_UNKNOWN) { 36 | lane_boundary.point = point; 37 | if (type > 4) { 38 | throw std::invalid_argument("Invalid lane boundary type: " + std::to_string(type)); 39 | } 40 | lane_boundary.type = type; 41 | } 42 | 43 | inline void setLeftBoundaryOfLaneElement(LaneElement& lane_element, const LaneBoundary& left_boundary) { 44 | lane_element.left_boundary = left_boundary; 45 | } 46 | 47 | inline void setLeftBoundaryOfLaneElement(LaneElement& lane_element, const gm::Point& point, const uint8_t type=LaneBoundary::TYPE_UNKNOWN) { 48 | LaneBoundary left_boundary; 49 | setLaneBoundary(left_boundary, point, type); 50 | setLeftBoundaryOfLaneElement(lane_element, left_boundary); 51 | } 52 | 53 | inline void setRightBoundaryOfLaneElement(LaneElement& lane_element, const LaneBoundary& right_boundary) { 54 | lane_element.right_boundary = right_boundary; 55 | } 56 | 57 | inline void setRightBoundaryOfLaneElement(LaneElement& lane_element, const gm::Point& point, const uint8_t type=LaneBoundary::TYPE_UNKNOWN) { 58 | LaneBoundary right_boundary; 59 | setLaneBoundary(right_boundary, point, type); 60 | setRightBoundaryOfLaneElement(lane_element, right_boundary); 61 | } 62 | 63 | } // namespace route_access 64 | 65 | } // namespace route_planning_msgs -------------------------------------------------------------------------------- /route_planning_msgs_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(route_planning_msgs_utils) 3 | 4 | find_package(ros_environment REQUIRED QUIET) 5 | set(ROS_VERSION $ENV{ROS_VERSION}) 6 | set(ROS_DISTRO $ENV{ROS_DISTRO}) 7 | 8 | # === ROS2 (AMENT) ============================================================= 9 | if(${ROS_VERSION} EQUAL 2) 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-Wall -Wextra -Wpedantic) 13 | endif() 14 | 15 | # find dependencies 16 | find_package(ament_cmake REQUIRED) 17 | find_package(ament_cmake_python REQUIRED) 18 | find_package(route_planning_msgs REQUIRED) 19 | find_package(std_msgs REQUIRED) 20 | 21 | ament_python_install_package(${PROJECT_NAME} 22 | PACKAGE_DIR src/${PROJECT_NAME}) 23 | 24 | add_library(${PROJECT_NAME} INTERFACE) 25 | 26 | target_include_directories(${PROJECT_NAME} INTERFACE 27 | $ 28 | $ 29 | ) 30 | 31 | target_link_libraries(${PROJECT_NAME} INTERFACE 32 | ${std_msgs_TARGETS} 33 | ${route_planning_msgs_TARGETS} 34 | ) 35 | 36 | ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) 37 | 38 | ament_export_dependencies( 39 | route_planning_msgs 40 | std_msgs 41 | ) 42 | 43 | install( 44 | DIRECTORY include/ 45 | DESTINATION include/${PROJECT_NAME} 46 | ) 47 | 48 | install(TARGETS ${PROJECT_NAME} 49 | EXPORT ${PROJECT_NAME}Targets 50 | ARCHIVE DESTINATION lib 51 | LIBRARY DESTINATION lib 52 | RUNTIME DESTINATION lib 53 | INCLUDES DESTINATION include 54 | ) 55 | 56 | if(BUILD_TESTING) 57 | # C++ tests 58 | find_package(ament_cmake_gtest REQUIRED) 59 | ament_add_gtest(${PROJECT_NAME}-test test/test_route_access.cpp) 60 | target_include_directories(${PROJECT_NAME}-test PUBLIC 61 | $ 62 | $ 63 | ) 64 | target_include_directories(${PROJECT_NAME}-test PUBLIC test) 65 | ament_target_dependencies(${PROJECT_NAME}-test 66 | std_msgs 67 | route_planning_msgs 68 | ) 69 | # Python tests 70 | find_package(ament_cmake_pytest REQUIRED) 71 | set(_pytest_tests 72 | test/test_route_access.py 73 | ) 74 | foreach(_test_path ${_pytest_tests}) 75 | get_filename_component(_test_name ${_test_path} NAME_WE) 76 | ament_add_pytest_test(${_test_name} ${_test_path} 77 | APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} 78 | TIMEOUT 60 79 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} 80 | ) 81 | endforeach() 82 | endif() 83 | 84 | ament_package() 85 | 86 | # === ROS (CATKIN) ============================================================= 87 | elseif(${ROS_VERSION} EQUAL 1) 88 | # not supported 89 | find_package(catkin REQUIRED) 90 | 91 | catkin_package() 92 | 93 | endif() -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/include/trajectory_planning_msgs_utils/impl/utils.h: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | 29 | namespace trajectory_planning_msgs { 30 | 31 | namespace trajectory_access { 32 | 33 | /** 34 | * @brief Wraps an angle to the range [-π, π]. 35 | * 36 | * @param angle The input angle in radians. 37 | * @return The wrapped angle in radians. 38 | */ 39 | inline double wrapAngle(const double& angle) { 40 | double wrapped_angle = angle; 41 | while (wrapped_angle > M_PI) wrapped_angle -= 2 * M_PI; 42 | while (wrapped_angle < -M_PI) wrapped_angle += 2 * M_PI; 43 | return wrapped_angle; 44 | } 45 | 46 | // --- state size ------------------------------------------------------------ 47 | 48 | const std::string kExceptionUnknownType = "Unknown type ID: "; 49 | 50 | inline int getStateDim(const unsigned char& type_id) { 51 | switch (type_id) { 52 | case DRIVABLE::TYPE_ID: 53 | return DRIVABLE::STATE_DIM; 54 | case DRIVABLERWS::TYPE_ID: 55 | return DRIVABLERWS::STATE_DIM; 56 | case REFERENCE::TYPE_ID: 57 | return REFERENCE::STATE_DIM; 58 | default: 59 | throw std::invalid_argument(kExceptionUnknownType + std::to_string(type_id)); 60 | } 61 | } 62 | 63 | inline int getStateDim(const Trajectory& trajectory) { return getStateDim(trajectory.type_id); } 64 | 65 | inline unsigned int getStatesSize(const std::vector& states) { return states.size(); } 66 | 67 | inline unsigned int getStatesSize(const Trajectory& trajectory) { return getStatesSize(trajectory.states); } 68 | 69 | inline bool isFloatingDivision(int numerator, int denominator) { return numerator % denominator != 0; } 70 | 71 | } // namespace trajectory_access 72 | 73 | } // namespace trajectory_planning_msgs -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/include/tf2_trajectory_planning_msgs/tf2_trajectory_planning_msgs.hpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace tf2 { 34 | namespace gm = geometry_msgs::msg; 35 | using namespace trajectory_planning_msgs::msg; 36 | using Time = tf2::TimePoint; 37 | #ifndef STAMP2TIME 38 | #define STAMP2TIME 39 | /** 40 | * @brief Converts a ROS2 builtin_interfaces::msg::Time message to a tf2_ros::Time object. 41 | * 42 | * @param t The ROS2 builtin_interfaces::msg::Time message to be converted. 43 | * @return Time The corresponding tf2_ros::Time object. 44 | */ 45 | inline Time stampToTime(const builtin_interfaces::msg::Time& t) { return tf2_ros::fromMsg(t); } 46 | #endif 47 | 48 | #ifndef STAMPS2TIMEDELTA 49 | #define STAMPS2TIMEDELTA 50 | /** 51 | * @brief Computes the time difference in seconds between two ROS2 Time messages. 52 | * 53 | * This function takes two `builtin_interfaces::msg::Time` objects and calculates the 54 | * difference between them in seconds. It converts the `builtin_interfaces::msg::Time` 55 | * objects to `rclcpp::Time` objects and then computes the difference. 56 | * 57 | * @param t1 The first time stamp. 58 | * @param t2 The second time stamp. 59 | * @return The time difference (t2-t1) in seconds as a double. 60 | */ 61 | inline double stampsToTimeDelta(const builtin_interfaces::msg::Time& t1, const builtin_interfaces::msg::Time& t2) { return (rclcpp::Time(t2)-rclcpp::Time(t1)).seconds(); } 62 | #endif 63 | } // namespace tf2 64 | 65 | #include 66 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/src/trajectory_planning_msgs_utils/checks.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | import math 26 | from typing import Sequence 27 | 28 | from trajectory_planning_msgs.msg import Trajectory 29 | 30 | from .utils import get_state_dim, get_states_size, is_floating_division 31 | 32 | 33 | kExceptionInvalidStateSize = "Invalid state size for trajectory with type ID: " 34 | kExceptionInvalidStatesSize = "Invalid states (sample points) size for trajectory with type ID: " 35 | 36 | 37 | def sanity_check_state_size(state: Sequence[float], type_id: int) -> None: 38 | state_dim = get_state_dim(type_id) 39 | state_size = len(state) 40 | if state_size != state_dim: 41 | raise ValueError( 42 | f"{kExceptionInvalidStateSize}{type_id}, {state_size} != {state_dim}" 43 | ) 44 | 45 | 46 | def sanity_check_states_size(states: Sequence[float], type_id: int) -> None: 47 | state_dim = get_state_dim(type_id) 48 | states_size = get_states_size(states) 49 | if is_floating_division(states_size, state_dim): 50 | raise ValueError( 51 | f"{kExceptionInvalidStatesSize}{type_id}, states_size = {states_size}, state_dim = {state_dim}" 52 | ) 53 | 54 | 55 | def sanity_check_states_size_for_trajectory(trajectory: Trajectory) -> None: 56 | sanity_check_states_size(trajectory.states, trajectory.type_id) 57 | 58 | 59 | def sanity_check_angle(angle: float, val_min: float = -math.pi, val_max: float = math.pi) -> None: 60 | if angle < val_min or angle > val_max: 61 | raise ValueError( 62 | f"Angle value: {angle} out of range [{val_min}, {val_max}]" 63 | ) 64 | 65 | 66 | def sanity_check_trajectory(trajectory: Trajectory) -> None: 67 | sanity_check_states_size_for_trajectory(trajectory) 68 | # Additional checks can be added here in the future. 69 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/include/trajectory_planning_msgs_utils/impl/checks.h: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | namespace trajectory_planning_msgs { 33 | 34 | namespace trajectory_access { 35 | 36 | const std::string kExceptionInvalidStateSize = "Invalid state size for trajectory with type ID: "; 37 | const std::string kExceptionInvalidStatesSize = "Invalid states (sample points) size for trajectory with type ID: "; 38 | 39 | inline void sanityCheckStateSize(const std::vector& state, const unsigned char& type_id) { 40 | int state_dim = getStateDim(type_id); 41 | int state_size = state.size(); 42 | if (state_size != state_dim) 43 | throw std::invalid_argument(kExceptionInvalidStateSize + std::to_string(type_id) + ", " + 44 | std::to_string(state_size) + " != " + std::to_string(state_dim)); 45 | } 46 | 47 | inline void sanityCheckStatesSize(const std::vector& states, const unsigned char& type_id) { 48 | int state_dim = getStateDim(type_id); 49 | int states_size = getStatesSize(states); 50 | if (isFloatingDivision(states_size, state_dim)) 51 | throw std::invalid_argument(kExceptionInvalidStatesSize + std::to_string(type_id) + ", states_size = " + 52 | std::to_string(states_size) + ", state_dim = " + std::to_string(state_dim)); 53 | } 54 | 55 | inline void sanityCheckStatesSize(const Trajectory& trajectory) { 56 | sanityCheckStatesSize(trajectory.states, trajectory.type_id); 57 | } 58 | 59 | inline void sanityCheckAngle(const double& angle, const double val_min = -M_PI, const double val_max = M_PI) { 60 | if (angle < val_min || angle > val_max) { 61 | throw std::invalid_argument("Angle value: " + std::to_string(angle) + " out of range [" + std::to_string(val_min) + 62 | ", " + std::to_string(val_max) + "]"); 63 | } 64 | } 65 | 66 | inline void sanityCheckTrajectory(const Trajectory& trajectory) { 67 | sanityCheckStatesSize(trajectory); 68 | // ToDo: more checks 69 | } 70 | 71 | } // namespace trajectory_access 72 | 73 | } // namespace trajectory_planning_msgs -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(trajectory_planning_msgs_utils) 3 | 4 | find_package(ros_environment REQUIRED QUIET) 5 | set(ROS_VERSION $ENV{ROS_VERSION}) 6 | set(ROS_DISTRO $ENV{ROS_DISTRO}) 7 | 8 | # === ROS2 (AMENT) ============================================================= 9 | if(${ROS_VERSION} EQUAL 2) 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-Wall -Wextra -Wpedantic) 13 | endif() 14 | 15 | # find dependencies 16 | find_package(ament_cmake REQUIRED) 17 | find_package(ament_cmake_python REQUIRED) 18 | find_package(trajectory_planning_msgs REQUIRED) 19 | find_package(std_msgs REQUIRED) 20 | 21 | ament_python_install_package(${PROJECT_NAME} 22 | PACKAGE_DIR src/${PROJECT_NAME} 23 | ) 24 | 25 | add_library(${PROJECT_NAME} INTERFACE) 26 | 27 | target_include_directories(${PROJECT_NAME} INTERFACE 28 | $ 29 | $ 30 | ) 31 | 32 | target_link_libraries(${PROJECT_NAME} INTERFACE 33 | ${std_msgs_TARGETS} 34 | ${trajectory_planning_msgs_TARGETS} 35 | ) 36 | 37 | ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) 38 | 39 | ament_export_dependencies( 40 | trajectory_planning_msgs 41 | std_msgs 42 | ) 43 | 44 | install( 45 | DIRECTORY include/ 46 | DESTINATION include/${PROJECT_NAME} 47 | ) 48 | 49 | install(TARGETS ${PROJECT_NAME} 50 | EXPORT ${PROJECT_NAME}Targets 51 | ARCHIVE DESTINATION lib 52 | LIBRARY DESTINATION lib 53 | RUNTIME DESTINATION lib 54 | INCLUDES DESTINATION include 55 | ) 56 | 57 | if(BUILD_TESTING) 58 | find_package(ament_cmake_gtest REQUIRED) 59 | ament_add_gtest(${PROJECT_NAME}-test test/test_trajectory_access.ros2.cpp) 60 | target_include_directories(${PROJECT_NAME}-test PUBLIC 61 | $ 62 | $ 63 | ) 64 | target_include_directories(${PROJECT_NAME}-test PUBLIC test) 65 | ament_target_dependencies(${PROJECT_NAME}-test 66 | std_msgs 67 | trajectory_planning_msgs 68 | ) 69 | 70 | find_package(ament_cmake_pytest REQUIRED) 71 | set(_pytest_tests 72 | test/test_trajectory_access.py 73 | ) 74 | foreach(_test_path ${_pytest_tests}) 75 | get_filename_component(_test_name ${_test_path} NAME_WE) 76 | ament_add_pytest_test(${_test_name} ${_test_path} 77 | APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} 78 | TIMEOUT 60 79 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} 80 | ) 81 | endforeach() 82 | endif() 83 | 84 | ament_package() 85 | 86 | # === ROS1 (CATKIN) ============================================================ 87 | elseif(${ROS_VERSION} EQUAL 1) 88 | 89 | find_package(catkin REQUIRED COMPONENTS 90 | std_msgs 91 | trajectory_planning_msgs 92 | ) 93 | 94 | catkin_package( 95 | INCLUDE_DIRS include 96 | CATKIN_DEPENDS 97 | std_msgs 98 | trajectory_planning_msgs 99 | ) 100 | 101 | include_directories( 102 | include 103 | ${catkin_INCLUDE_DIRS} 104 | ) 105 | 106 | install(DIRECTORY include/${PROJECT_NAME}/ 107 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 108 | ) 109 | 110 | catkin_add_gtest(${PROJECT_NAME}-test test/test_trajectory_access.cpp) 111 | if(TARGET ${PROJECT_NAME}-test) 112 | target_include_directories(${PROJECT_NAME}-test PUBLIC test) 113 | endif() 114 | 115 | endif() 116 | -------------------------------------------------------------------------------- /route_planning_msgs_utils/test/test_route_access.cpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | using namespace route_planning_msgs::msg; 31 | using namespace route_planning_msgs::route_access; 32 | 33 | std::uniform_real_distribution uniform_distribution(-1, 1); 34 | std::default_random_engine random_engine; 35 | 36 | double randomValue() { return uniform_distribution(random_engine); } 37 | static const double EPS = 1e-12; 38 | 39 | TEST(route_planning_msgs, test_setters) { 40 | 41 | LaneElement lane_element; 42 | 43 | geometry_msgs::msg::Point left_boundary; 44 | left_boundary.x = 1.0; 45 | left_boundary.y = 2.0; 46 | geometry_msgs::msg::Point right_boundary; 47 | right_boundary.x = 3.0; 48 | right_boundary.y = 4.0; 49 | setLeftBoundaryOfLaneElement(lane_element, left_boundary); 50 | setRightBoundaryOfLaneElement(lane_element, right_boundary); 51 | 52 | EXPECT_EQ(lane_element.left_boundary.point.x, 1.0); 53 | EXPECT_EQ(lane_element.left_boundary.point.y, 2.0); 54 | EXPECT_EQ(lane_element.left_boundary.type, LaneBoundary::TYPE_UNKNOWN); 55 | EXPECT_EQ(lane_element.right_boundary.point.x, 3.0); 56 | EXPECT_EQ(lane_element.right_boundary.point.y, 4.0); 57 | EXPECT_EQ(lane_element.right_boundary.type, LaneBoundary::TYPE_UNKNOWN); 58 | 59 | } 60 | 61 | TEST(route_planning_msgs, test_getters) { 62 | Route route; 63 | RouteElement route_element; 64 | LaneElement lane_element; 65 | 66 | geometry_msgs::msg::Point left_boundary; 67 | left_boundary.x = 1.0; 68 | left_boundary.y = 2.0; 69 | geometry_msgs::msg::Point right_boundary; 70 | right_boundary.x = 3.0; 71 | right_boundary.y = 4.0; 72 | setLeftBoundaryOfLaneElement(lane_element, left_boundary); 73 | setRightBoundaryOfLaneElement(lane_element, right_boundary); 74 | route_element.suggested_lane_idx = 0; 75 | route_element.lane_elements.push_back(lane_element); 76 | route_element.is_enriched = true; 77 | route.route_elements.push_back(route_element); 78 | route.starting_route_element_idx = 0; 79 | route.current_route_element_idx = 0; 80 | route.destination_route_element_idx = 0; 81 | 82 | EXPECT_NEAR(getWidthOfLaneElement(lane_element), sqrt(8), EPS); 83 | EXPECT_NEAR(getWidthOfSuggestedLaneElement(route_element), 2.8284271247461903, EPS); 84 | EXPECT_NEAR(getWidthOfCurrentSuggestedLaneElement(route), 2.8284271247461903, EPS); 85 | } 86 | 87 | int main(int argc, char *argv[]) { 88 | testing::InitGoogleTest(&argc, argv); 89 | return RUN_ALL_TESTS(); 90 | } -------------------------------------------------------------------------------- /tf2_route_planning_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(tf2_route_planning_msgs) 3 | 4 | find_package(ros_environment REQUIRED QUIET) 5 | set(ROS_VERSION $ENV{ROS_VERSION}) 6 | 7 | # === ROS2 (AMENT) ============================================================= 8 | if(${ROS_VERSION} EQUAL 2) 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | # find dependencies 15 | find_package(ament_cmake REQUIRED) 16 | find_package(ament_cmake_python REQUIRED) 17 | find_package(geometry_msgs REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(route_planning_msgs REQUIRED) 20 | find_package(route_planning_msgs_utils REQUIRED) 21 | find_package(tf2 REQUIRED) 22 | find_package(tf2_geometry_msgs REQUIRED) 23 | find_package(tf2_ros REQUIRED) 24 | 25 | ament_python_install_package(${PROJECT_NAME} 26 | PACKAGE_DIR src/${PROJECT_NAME}) 27 | 28 | add_library(${PROJECT_NAME} INTERFACE) 29 | 30 | target_include_directories(${PROJECT_NAME} INTERFACE 31 | $ 32 | $ 33 | ) 34 | 35 | target_link_libraries(${PROJECT_NAME} INTERFACE 36 | ${geometry_msgs_TARGETS} 37 | ${route_planning_msgs_TARGETS} 38 | ${tf2_TARGETS} 39 | ${tf2_ros_TARGETS} 40 | ) 41 | 42 | ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) 43 | ament_export_dependencies( 44 | geometry_msgs 45 | route_planning_msgs 46 | tf2 47 | tf2_ros 48 | ) 49 | 50 | install( 51 | DIRECTORY include/ 52 | DESTINATION include/${PROJECT_NAME} 53 | ) 54 | 55 | install(TARGETS ${PROJECT_NAME} 56 | EXPORT ${PROJECT_NAME}Targets 57 | ARCHIVE DESTINATION lib 58 | LIBRARY DESTINATION lib 59 | RUNTIME DESTINATION lib 60 | INCLUDES DESTINATION include 61 | ) 62 | 63 | if(BUILD_TESTING) 64 | # C++ tests 65 | find_package(ament_cmake_gtest REQUIRED) 66 | ament_add_gtest(${PROJECT_NAME}-test test/test_tf2_route_planning_msgs.cpp) 67 | target_include_directories(${PROJECT_NAME}-test PUBLIC 68 | $ 69 | $ 70 | ) 71 | target_include_directories(${PROJECT_NAME}-test PUBLIC test) 72 | ament_target_dependencies(${PROJECT_NAME}-test 73 | geometry_msgs 74 | rclcpp 75 | route_planning_msgs 76 | route_planning_msgs_utils 77 | tf2 78 | tf2_geometry_msgs 79 | tf2_ros 80 | ) 81 | # Python tests 82 | find_package(ament_cmake_pytest REQUIRED) 83 | set(_pytest_tests 84 | test/test_tf2_route_planning_msgs.py 85 | ) 86 | foreach(_test_path ${_pytest_tests}) 87 | get_filename_component(_test_name ${_test_path} NAME_WE) 88 | ament_add_pytest_test(${_test_name} ${_test_path} 89 | APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} 90 | TIMEOUT 60 91 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} 92 | ) 93 | endforeach() 94 | endif() 95 | 96 | ament_package() 97 | 98 | # === ROS1 (CATKIN) ============================================================ 99 | elseif(${ROS_VERSION} EQUAL 1) 100 | 101 | # Currently no full support for ROS1! 102 | 103 | find_package(catkin REQUIRED COMPONENTS 104 | geometry_msgs 105 | route_planning_msgs 106 | ) 107 | 108 | catkin_package( 109 | INCLUDE_DIRS include 110 | CATKIN_DEPENDS 111 | geometry_msgs 112 | route_planning_msgs 113 | ) 114 | 115 | include_directories( 116 | include 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | install(DIRECTORY include/${PROJECT_NAME}/ 121 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 122 | ) 123 | 124 | endif() -------------------------------------------------------------------------------- /trajectory_planning_msgs_rviz_plugins/include/trajectory_planning_msgs/displays/trajectory_display.hpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include "rviz_common/message_filter_display.hpp" 32 | #include "rviz_default_plugins/visibility_control.hpp" 33 | #include "rviz_rendering/objects/arrow.hpp" 34 | #include "rviz_rendering/objects/shape.hpp" 35 | #include "trajectory_planning_msgs/msg/trajectory.hpp" 36 | #include "trajectory_planning_msgs_utils/trajectory_access.hpp" 37 | 38 | namespace Ogre { 39 | class ManualObject; 40 | } 41 | 42 | namespace rviz_common { 43 | namespace properties { 44 | class ColorProperty; 45 | class FloatProperty; 46 | } // namespace properties 47 | } // namespace rviz_common 48 | 49 | namespace trajectory_planning_msgs { 50 | namespace displays { 51 | /** 52 | * \class TrajectoryDisplay 53 | * \brief Displays a trajectory_planning_msgs::Trajectory message 54 | */ 55 | 56 | class TrajectoryDisplay : public rviz_common::MessageFilterDisplay { 57 | Q_OBJECT 58 | 59 | public: 60 | TrajectoryDisplay(); 61 | ~TrajectoryDisplay() override; 62 | 63 | void onInitialize() override; 64 | 65 | void reset() override; 66 | 67 | void timeoutTimerCallback(); 68 | 69 | protected: 70 | void processMessage(trajectory_planning_msgs::msg::Trajectory::ConstSharedPtr msg) override; 71 | 72 | Ogre::ManualObject *vel_trj_, *time_trj_, *acc_trj_, *s_trj_; 73 | Ogre::MaterialPtr material_vel_, material_time_, material_acc_, material_s_; 74 | std::vector> vel_point_spheres_, time_point_spheres_, acc_point_spheres_, 75 | s_point_spheres_; 76 | std::vector> yaw_arrows_; 77 | 78 | rviz_common::properties::BoolProperty *viz_vel_, *viz_time_, *viz_acc_, *viz_s_; 79 | rviz_common::properties::ColorProperty *color_property_vel_, *color_property_time_, *color_property_acc_, 80 | *color_property_s_; 81 | rviz_common::properties::BoolProperty *viz_yaw_arrows_; 82 | rviz_common::properties::ColorProperty *color_property_yaw_; 83 | rviz_common::properties::BoolProperty *viz_vel_points_, *viz_time_points_, *viz_acc_points_, *viz_s_points_; 84 | rviz_common::properties::FloatProperty *alpha_property_, *size_property_points_, *scale_property_yaw_arrows_; 85 | 86 | // timeout 87 | rviz_common::properties::BoolProperty *enable_timeout_property_; 88 | rviz_common::properties::FloatProperty *timeout_property_; 89 | rclcpp::TimerBase::SharedPtr timeout_timer_; 90 | }; 91 | 92 | } // namespace displays 93 | } // namespace trajectory_planning_msgs 94 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_rviz_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(trajectory_planning_msgs_rviz_plugins) 3 | 4 | find_package(ros_environment REQUIRED QUIET) 5 | set(ROS_VERSION $ENV{ROS_VERSION}) 6 | 7 | # === ROS2 (AMENT) ============================================================= 8 | if(${ROS_VERSION} EQUAL 2) 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | # find dependencies 15 | find_package(ament_cmake REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(pluginlib REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(trajectory_planning_msgs REQUIRED) 20 | find_package(trajectory_planning_msgs_utils REQUIRED) 21 | find_package(rviz_common REQUIRED) 22 | find_package(rviz_default_plugins REQUIRED) 23 | find_package(rviz_ogre_vendor REQUIRED) 24 | find_package(rviz_rendering REQUIRED) 25 | find_package(std_msgs REQUIRED) 26 | find_package(tf2 REQUIRED) 27 | find_package(tf2_ros REQUIRED) 28 | 29 | set(display_headers_to_moc 30 | include/trajectory_planning_msgs/displays/trajectory_display.hpp 31 | ) 32 | 33 | foreach(header "${display_headers_to_moc}") 34 | qt5_wrap_cpp(display_moc_files "${header}") 35 | endforeach() 36 | 37 | set(display_source_files 38 | src/displays/trajectory_display.cpp 39 | ) 40 | 41 | add_library(${PROJECT_NAME} SHARED 42 | ${display_moc_files} 43 | ${display_source_files} 44 | ) 45 | 46 | target_include_directories(${PROJECT_NAME} PUBLIC 47 | $ 48 | $ 49 | ${Qt5Widgets_INCLUDE_DIRS} 50 | ) 51 | 52 | target_link_libraries(${PROJECT_NAME} PUBLIC 53 | rviz_ogre_vendor::OgreMain 54 | rviz_ogre_vendor::OgreOverlay 55 | ) 56 | 57 | pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) 58 | 59 | ament_target_dependencies(${PROJECT_NAME} 60 | PUBLIC 61 | geometry_msgs 62 | rclcpp 63 | trajectory_planning_msgs 64 | trajectory_planning_msgs_utils 65 | rviz_common 66 | rviz_default_plugins 67 | rviz_rendering 68 | tf2 69 | tf2_ros 70 | ) 71 | 72 | # Export old-style CMake variables 73 | ament_export_include_directories("include/${PROJECT_NAME}") 74 | 75 | # Export modern CMake targets 76 | ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) 77 | 78 | ament_export_dependencies( 79 | geometry_msgs 80 | rclcpp 81 | rviz_common 82 | rviz_default_plugins 83 | rviz_ogre_vendor 84 | tf2 85 | tf2_ros 86 | ) 87 | 88 | install( 89 | TARGETS ${PROJECT_NAME} 90 | EXPORT ${PROJECT_NAME} 91 | ARCHIVE DESTINATION lib 92 | LIBRARY DESTINATION lib 93 | RUNTIME DESTINATION bin 94 | ) 95 | 96 | install( 97 | DIRECTORY icons/classes/ 98 | DESTINATION share/${PROJECT_NAME}/icons/classes 99 | ) 100 | # RViz Display End 101 | 102 | if(BUILD_TESTING) 103 | #find_package(ament_lint_auto REQUIRED) 104 | # the following line skips the linter which checks for copyrights 105 | # comment the line when a copyright and license is added to all source files 106 | #set(ament_cmake_copyright_FOUND TRUE) 107 | # the following line skips cpplint (only works in a git repo) 108 | # comment the line when this package is in a git repo and when 109 | # a copyright and license is added to all source files 110 | #set(ament_cmake_cpplint_FOUND TRUE) 111 | #ament_lint_auto_find_test_dependencies() 112 | endif() 113 | 114 | ament_package( 115 | CONFIG_EXTRAS "${PROJECT_NAME}-extras.cmake" 116 | ) 117 | 118 | # === ROS1 (CATKIN) ============================================================ 119 | elseif(${ROS_VERSION} EQUAL 1) 120 | 121 | # Currently no support for ROS1! 122 | # Specify that the directory should be excluded from the install target 123 | install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 124 | DESTINATION . 125 | EXCLUDE_FROM_ALL) 126 | 127 | endif() -------------------------------------------------------------------------------- /route_planning_msgs_rviz_plugins/include/route_planning_msgs/tools/route/plan_route_tool.hpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include "geometry_msgs/msg/pose_array.hpp" 34 | #include "geometry_msgs/msg/pose_stamped.hpp" 35 | #include "rclcpp/node.hpp" 36 | #include "rclcpp/qos.hpp" 37 | 38 | #include "rviz_default_plugins/tools/pose/pose_tool.hpp" 39 | #include "rviz_default_plugins/visibility_control.hpp" 40 | #include "rviz_rendering/objects/shape.hpp" 41 | 42 | #include 43 | #include 44 | 45 | namespace rviz_common { 46 | class DisplayContext; 47 | namespace properties { 48 | class StringProperty; 49 | class QosProfileProperty; 50 | class BoolProperty; 51 | class FloatProperty; 52 | class VectorProperty; 53 | } // namespace properties 54 | } // namespace rviz_common 55 | 56 | namespace route_planning_msgs { 57 | namespace tools { 58 | class PlanRouteTool : public rviz_default_plugins::tools::PoseTool { 59 | Q_OBJECT 60 | 61 | public: 62 | PlanRouteTool(); 63 | 64 | ~PlanRouteTool() override; 65 | 66 | void onInitialize() override; 67 | 68 | void activate() override; 69 | void deactivate() override; 70 | 71 | protected: 72 | void onPoseSet(double x, double y, double theta) override; // needs to be overridden because of base class 73 | int processMouseEvent(rviz_common::ViewportMouseEvent& event) override; 74 | 75 | private Q_SLOTS: 76 | void updateActionServer(); 77 | int processMouseLeftButtonPressed(std::pair xy_plane_intersection); 78 | int processMouseRightButtonPressed(std::pair xy_plane_intersection); 79 | int processMouseMiddleButtonPressed(); 80 | void planRoute(); 81 | void drawIntermediates(std::vector& points); 82 | 83 | private: 84 | 85 | rclcpp_action::Client::SharedPtr action_client_; 86 | 87 | rclcpp::Clock::SharedPtr clock_; 88 | rclcpp::QoS qos_profile_; 89 | 90 | rviz_common::properties::StringProperty* action_server_property_; 91 | rviz_common::properties::QosProfileProperty* qos_profile_property_; 92 | 93 | geometry_msgs::msg::PointStamped destination_; 94 | std::vector intermediate_destinations_; 95 | std::vector> intermediate_shapes_; 96 | 97 | std::unique_ptr tf2_buffer_; 98 | std::shared_ptr tf2_listener_; 99 | }; 100 | 101 | } // namespace tools 102 | } // namespace route_planning_msgs -------------------------------------------------------------------------------- /route_planning_msgs_utils/src/route_planning_msgs_utils/route_setters.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | from route_planning_msgs.msg import LaneBoundary, LaneElement 26 | from geometry_msgs.msg import Point 27 | 28 | 29 | def set_lane_boundary(boundary: LaneBoundary, point: Point, type: int = LaneBoundary.TYPE_UNKNOWN) -> None: 30 | """ 31 | Set the position of a LaneBoundary to the given point. 32 | 33 | :param boundary: The LaneBoundary to set. 34 | :param point: The Point to set the LaneBoundary to. 35 | :param type: The type of the LaneBoundary. Defaults to TYPE_UNKNOWN. 36 | """ 37 | boundary.point = point 38 | if type > 4: 39 | raise ValueError(f"Invalid lane boundary type: {type}") 40 | boundary.type = type 41 | 42 | def set_left_boundary_of_lane_element(lane_element: LaneElement, lane_boundary: LaneBoundary): 43 | """ 44 | Set the left boundary of a LaneElement to the given LaneBoundary. 45 | 46 | :param lane_element: The LaneElement to set the left boundary for. 47 | :param lane_boundary: The LaneBoundary to set as the left boundary. 48 | :return: The updated LaneElement. 49 | """ 50 | lane_element.left_boundary = lane_boundary 51 | 52 | def set_left_boundary_of_lane_element_from_point(lane_element: LaneElement, point: Point, type: int=LaneBoundary.TYPE_UNKNOWN): 53 | """ 54 | Set the left boundary of a LaneElement to the given point. 55 | 56 | :param lane_element: The LaneElement to set the left boundary for. 57 | :param point: The Point to set the left boundary to. 58 | :param type: The type of the LaneBoundary. Default is TYPE_UNKNOWN. 59 | :return: The updated LaneElement. 60 | """ 61 | boundary = LaneBoundary() 62 | set_lane_boundary(boundary, point, type) 63 | set_left_boundary_of_lane_element(lane_element, boundary) 64 | 65 | def set_right_boundary_of_lane_element(lane_element: LaneElement, lane_boundary: LaneBoundary): 66 | """ 67 | Set the right boundary of a LaneElement to the given LaneBoundary. 68 | 69 | :param lane_element: The LaneElement to set the right boundary for. 70 | :param lane_boundary: The LaneBoundary to set as the right boundary. 71 | :return: The updated LaneElement. 72 | """ 73 | lane_element.right_boundary = lane_boundary 74 | 75 | def set_right_boundary_of_lane_element_from_point(lane_element: LaneElement, point: Point, type: int=LaneBoundary.TYPE_UNKNOWN): 76 | """ 77 | Set the right boundary of a LaneElement to the given point. 78 | 79 | :param lane_element: The LaneElement to set the right boundary for. 80 | :param point: The Point to set the right boundary to. 81 | :param type: The type of the LaneBoundary. Default is TYPE_UNKNOWN. 82 | :return: The updated LaneElement. 83 | """ 84 | boundary = LaneBoundary() 85 | set_lane_boundary(boundary, point, type) 86 | set_right_boundary_of_lane_element(lane_element, boundary) 87 | -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(tf2_trajectory_planning_msgs) 3 | 4 | find_package(ros_environment REQUIRED QUIET) 5 | set(ROS_VERSION $ENV{ROS_VERSION}) 6 | 7 | # === ROS2 (AMENT) ============================================================= 8 | if(${ROS_VERSION} EQUAL 2) 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | # find dependencies 15 | find_package(ament_cmake REQUIRED) 16 | find_package(ament_cmake_python REQUIRED) 17 | find_package(geometry_msgs REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(tf2 REQUIRED) 20 | find_package(tf2_geometry_msgs REQUIRED) 21 | find_package(tf2_ros REQUIRED) 22 | find_package(trajectory_planning_msgs REQUIRED) 23 | find_package(trajectory_planning_msgs_utils REQUIRED) 24 | 25 | ament_python_install_package(${PROJECT_NAME} 26 | PACKAGE_DIR src/${PROJECT_NAME} 27 | ) 28 | 29 | add_library(${PROJECT_NAME} INTERFACE) 30 | 31 | target_include_directories(${PROJECT_NAME} INTERFACE 32 | $ 33 | $ 34 | ) 35 | 36 | ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) 37 | ament_export_dependencies( 38 | geometry_msgs 39 | rclcpp 40 | tf2 41 | tf2_geometry_msgs 42 | tf2_ros 43 | trajectory_planning_msgs 44 | trajectory_planning_msgs_utils 45 | ) 46 | 47 | install( 48 | DIRECTORY include/ 49 | DESTINATION include/${PROJECT_NAME} 50 | ) 51 | 52 | install(TARGETS ${PROJECT_NAME} 53 | EXPORT ${PROJECT_NAME}Targets 54 | ARCHIVE DESTINATION lib 55 | LIBRARY DESTINATION lib 56 | RUNTIME DESTINATION lib 57 | INCLUDES DESTINATION include 58 | ) 59 | 60 | if(BUILD_TESTING) 61 | find_package(ament_cmake_gtest REQUIRED) 62 | ament_add_gtest(${PROJECT_NAME}-test test/test_tf2_trajectory_planning_msgs.ros2.cpp) 63 | target_include_directories(${PROJECT_NAME}-test PUBLIC 64 | $ 65 | $ 66 | ) 67 | target_include_directories(${PROJECT_NAME}-test PUBLIC test) 68 | ament_target_dependencies(${PROJECT_NAME}-test 69 | geometry_msgs 70 | rclcpp 71 | tf2 72 | tf2_geometry_msgs 73 | tf2_ros 74 | trajectory_planning_msgs 75 | trajectory_planning_msgs_utils 76 | ) 77 | 78 | find_package(ament_cmake_pytest REQUIRED) 79 | set(_pytest_tests 80 | test/test_tf2_trajectory_planning_msgs.py 81 | ) 82 | foreach(_test_path ${_pytest_tests}) 83 | get_filename_component(_test_name ${_test_path} NAME_WE) 84 | ament_add_pytest_test(${_test_name} ${_test_path} 85 | APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} 86 | TIMEOUT 60 87 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} 88 | ) 89 | endforeach() 90 | endif() 91 | 92 | ament_package() 93 | 94 | # === ROS1 (CATKIN) ============================================================ 95 | elseif(${ROS_VERSION} EQUAL 1) 96 | 97 | add_compile_definitions(ROS1) 98 | 99 | find_package(catkin REQUIRED COMPONENTS 100 | geometry_msgs 101 | roscpp 102 | tf2 103 | tf2_geometry_msgs 104 | trajectory_planning_msgs 105 | trajectory_planning_msgs_utils 106 | ) 107 | 108 | catkin_package( 109 | INCLUDE_DIRS include 110 | CATKIN_DEPENDS 111 | geometry_msgs 112 | roscpp 113 | tf2 114 | tf2_geometry_msgs 115 | trajectory_planning_msgs 116 | trajectory_planning_msgs_utils 117 | ) 118 | 119 | include_directories( 120 | include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | install(DIRECTORY include/${PROJECT_NAME}/ 125 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 126 | ) 127 | 128 | catkin_add_gtest(${PROJECT_NAME}-test test/test_tf2_trajectory_planning_msgs.cpp) 129 | if(TARGET ${PROJECT_NAME}-test) 130 | target_include_directories(${PROJECT_NAME}-test PUBLIC test) 131 | target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES}) 132 | endif() 133 | 134 | endif() 135 | -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/include/tf2_trajectory_planning_msgs/impl/tf2_trajectory_planning_msgs.h: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | #include 29 | 30 | namespace tf2 { 31 | 32 | using namespace trajectory_planning_msgs::trajectory_access; 33 | 34 | inline void doTransform(const std::vector& state_in, std::vector& state_out, 35 | const unsigned char& type_id, const gm::TransformStamped& transform, const double time_offset) { 36 | state_out = state_in; 37 | 38 | double x = getX(state_in, type_id); 39 | double y = getY(state_in, type_id); 40 | double theta = 0; 41 | double t = getT(state_in, type_id); 42 | if (type_id == DRIVABLE::TYPE_ID || type_id == DRIVABLERWS::TYPE_ID) theta = getTheta(state_in, type_id); 43 | 44 | gm::PoseStamped pose_in; 45 | tf2::Quaternion q; 46 | q.setRPY(0, 0, theta); 47 | 48 | pose_in.pose.position.x = x; 49 | pose_in.pose.position.y = y; 50 | pose_in.pose.position.z = 0; 51 | pose_in.pose.orientation = tf2::toMsg(q); 52 | 53 | gm::PoseStamped pose_out; 54 | doTransform(pose_in, pose_out, transform); 55 | 56 | setX(state_out, type_id, pose_out.pose.position.x); 57 | setY(state_out, type_id, pose_out.pose.position.y); 58 | setT(state_out, type_id, t + time_offset); 59 | if (type_id == DRIVABLE::TYPE_ID || type_id == DRIVABLERWS::TYPE_ID) setTheta(state_out, type_id, tf2::getYaw(pose_out.pose.orientation)); 60 | } 61 | 62 | template <> 63 | inline void doTransform(const Trajectory& trajectory_in, Trajectory& trajectory_out, 64 | const gm::TransformStamped& transform) { 65 | trajectory_out = trajectory_in; 66 | trajectory_out.header.stamp = transform.header.stamp; 67 | trajectory_out.header.frame_id = transform.header.frame_id; 68 | double time_offset = stampsToTimeDelta(transform.header.stamp, trajectory_in.header.stamp); 69 | 70 | for (int i = 0; i < getSamplePointSize(trajectory_in); i++) { 71 | std::vector state_in = getState(trajectory_in, i); 72 | std::vector state_out; 73 | unsigned char type_id = trajectory_in.type_id; 74 | doTransform(state_in, state_out, type_id, transform, time_offset); 75 | setState(trajectory_out, state_out, i); 76 | } 77 | } 78 | 79 | #ifdef ROS1 80 | #define TF2_TRAJECTORY_PLANNING_MSGS_TIME_TYPE const Time& 81 | #else 82 | #define TF2_TRAJECTORY_PLANNING_MSGS_TIME_TYPE Time 83 | #endif 84 | template <> 85 | inline TF2_TRAJECTORY_PLANNING_MSGS_TIME_TYPE getTimestamp(const Trajectory& trajectory) { 86 | const Time& t = stampToTime(trajectory.header.stamp); 87 | return t; 88 | } 89 | 90 | #ifdef ROS1 91 | #define TF2_TRAJECTORY_PLANNING_MSGS_FRAME_TYPE const std::string& 92 | #else 93 | #define TF2_TRAJECTORY_PLANNING_MSGS_FRAME_TYPE std::string 94 | #endif 95 | template <> 96 | inline TF2_TRAJECTORY_PLANNING_MSGS_FRAME_TYPE getFrameId(const Trajectory& trajectory) { 97 | return trajectory.header.frame_id; 98 | } 99 | 100 | } // namespace tf2 101 | -------------------------------------------------------------------------------- /route_planning_msgs_rviz_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(route_planning_msgs_rviz_plugins) 3 | 4 | find_package(ros_environment REQUIRED QUIET) 5 | set(ROS_VERSION $ENV{ROS_VERSION}) 6 | 7 | # === ROS2 (AMENT) ============================================================= 8 | if(${ROS_VERSION} EQUAL 2) 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | # find dependencies 15 | find_package(ament_cmake REQUIRED) 16 | # uncomment the following section in order to fill in 17 | # further dependencies manually. 18 | # find_package( REQUIRED) 19 | find_package(geometry_msgs REQUIRED) 20 | find_package(pluginlib REQUIRED) 21 | find_package(rclcpp REQUIRED) 22 | find_package(route_planning_msgs REQUIRED) 23 | find_package(route_planning_msgs_utils REQUIRED) 24 | find_package(rviz_common REQUIRED) 25 | find_package(rviz_default_plugins REQUIRED) 26 | find_package(rviz_ogre_vendor REQUIRED) 27 | find_package(rviz_rendering REQUIRED) 28 | find_package(std_msgs REQUIRED) 29 | find_package(tf2 REQUIRED) 30 | find_package(tf2_geometry_msgs REQUIRED) 31 | find_package(tf2_ros REQUIRED) 32 | 33 | set(display_headers_to_moc 34 | include/route_planning_msgs/displays/route/route_display.hpp 35 | ) 36 | 37 | set(tool_headers_to_moc 38 | include/route_planning_msgs/tools/route/ref_path_tool.hpp 39 | include/route_planning_msgs/tools/route/plan_route_tool.hpp 40 | ) 41 | 42 | foreach(header "${display_headers_to_moc}") 43 | qt5_wrap_cpp(display_moc_files "${header}") 44 | endforeach() 45 | 46 | foreach(header "${tool_headers_to_moc}") 47 | qt5_wrap_cpp(tool_moc_files "${header}") 48 | endforeach() 49 | 50 | set(display_source_files 51 | src/displays/route/route_display.cpp 52 | ) 53 | 54 | set(tool_source_files 55 | src/tools/route/ref_path_tool.cpp 56 | src/tools/route/plan_route_tool.cpp 57 | ) 58 | 59 | add_library(${PROJECT_NAME} SHARED 60 | ${display_moc_files} 61 | ${display_source_files} 62 | ${tool_moc_files} 63 | ${tool_source_files} 64 | ) 65 | 66 | target_include_directories(${PROJECT_NAME} PUBLIC 67 | $ 68 | $ 69 | ${Qt5Widgets_INCLUDE_DIRS} 70 | ) 71 | 72 | target_link_libraries(${PROJECT_NAME} PUBLIC 73 | rviz_ogre_vendor::OgreMain 74 | rviz_ogre_vendor::OgreOverlay 75 | rviz_common::rviz_common 76 | ) 77 | 78 | pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) 79 | 80 | ament_target_dependencies(${PROJECT_NAME} 81 | PUBLIC 82 | geometry_msgs 83 | rclcpp 84 | route_planning_msgs 85 | route_planning_msgs_utils 86 | rviz_common 87 | rviz_default_plugins 88 | rviz_rendering 89 | tf2 90 | tf2_geometry_msgs 91 | tf2_ros 92 | ) 93 | 94 | # Export old-style CMake variables 95 | ament_export_include_directories("include/${PROJECT_NAME}") 96 | 97 | # Export modern CMake targets 98 | ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) 99 | 100 | ament_export_dependencies( 101 | geometry_msgs 102 | rclcpp 103 | rviz_common 104 | rviz_default_plugins 105 | rviz_ogre_vendor 106 | tf2 107 | tf2_geometry_msgs 108 | tf2_ros 109 | ) 110 | 111 | install( 112 | TARGETS ${PROJECT_NAME} 113 | EXPORT ${PROJECT_NAME} 114 | ARCHIVE DESTINATION lib 115 | LIBRARY DESTINATION lib 116 | RUNTIME DESTINATION bin 117 | ) 118 | 119 | install( 120 | DIRECTORY icons/classes/ 121 | DESTINATION share/${PROJECT_NAME}/icons/classes 122 | ) 123 | # RViz Display End 124 | 125 | if(BUILD_TESTING) 126 | #find_package(ament_lint_auto REQUIRED) 127 | # the following line skips the linter which checks for copyrights 128 | # comment the line when a copyright and license is added to all source files 129 | #set(ament_cmake_copyright_FOUND TRUE) 130 | # the following line skips cpplint (only works in a git repo) 131 | # comment the line when this package is in a git repo and when 132 | # a copyright and license is added to all source files 133 | #set(ament_cmake_cpplint_FOUND TRUE) 134 | #ament_lint_auto_find_test_dependencies() 135 | endif() 136 | 137 | ament_package( 138 | CONFIG_EXTRAS "${PROJECT_NAME}-extras.cmake" 139 | ) 140 | 141 | # === ROS1 (CATKIN) ============================================================ 142 | elseif(${ROS_VERSION} EQUAL 1) 143 | 144 | # Currently no support for ROS1! 145 | # Specify that the directory should be excluded from the install target 146 | install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 147 | DESTINATION . 148 | EXCLUDE_FROM_ALL) 149 | 150 | endif() -------------------------------------------------------------------------------- /tf2_route_planning_msgs/test/test_tf2_route_planning_msgs.cpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | 31 | namespace gm = geometry_msgs::msg; 32 | using namespace route_planning_msgs::msg; 33 | using namespace route_planning_msgs::route_access; 34 | 35 | #include 36 | 37 | #include 38 | 39 | TEST(tf2_route_planning_msgs, test_doTransform_Route) { 40 | 41 | gm::Point point_in, point_out; 42 | point_in.x = 1.0; 43 | point_in.y = 2.0; 44 | point_in.z = 3.0; 45 | 46 | point_out.x = 9.0; 47 | point_out.y = 18.0; 48 | point_out.z = 33.0; 49 | 50 | gm::Pose pose_in, pose_out; 51 | pose_in.position = point_in; 52 | pose_in.orientation.x = 0.0; 53 | pose_in.orientation.y = 0.0; 54 | pose_in.orientation.z = 1.0; 55 | pose_in.orientation.w = 0.0; 56 | 57 | pose_out.position = point_out; 58 | pose_out.orientation.x = 0.0; 59 | pose_out.orientation.y = 0.0; 60 | pose_out.orientation.z = 0.0; 61 | pose_out.orientation.w = 1.0; 62 | 63 | // Define a LaneBoundary 64 | LaneBoundary lane_boundary; 65 | lane_boundary.point = point_in; 66 | 67 | // Define a LaneElement 68 | LaneElement lane_element; 69 | lane_element.reference_pose = pose_in; 70 | setLeftBoundaryOfLaneElement(lane_element, lane_boundary); 71 | setRightBoundaryOfLaneElement(lane_element, lane_boundary); 72 | 73 | // Define a RegulatoryElement 74 | RegulatoryElement regulatory_element; 75 | regulatory_element.reference_line[0] = point_in; 76 | regulatory_element.reference_line[1] = point_in; 77 | regulatory_element.positions.push_back(point_in); 78 | 79 | // Define a route element 80 | RouteElement route_element; 81 | route_element.lane_elements.push_back(lane_element); 82 | route_element.left_boundary = point_in; 83 | route_element.right_boundary = point_in; 84 | route_element.regulatory_elements.push_back(regulatory_element); 85 | 86 | // Define a route 87 | Route route, route_tf; 88 | route.destination = point_in; 89 | route.intermediate_destinations.push_back(point_in); 90 | route.intermediate_destinations.push_back(point_in); 91 | route.route_elements.push_back(route_element); 92 | 93 | gm::TransformStamped tf; 94 | tf.transform.translation.x = 10.0; 95 | tf.transform.translation.y = 20.0; 96 | tf.transform.translation.z = 30.0; 97 | tf.transform.rotation.x = 0.0; 98 | tf.transform.rotation.y = 0.0; 99 | tf.transform.rotation.z = 1.0; 100 | tf.transform.rotation.w = 0.0; 101 | 102 | tf2::doTransform(route, route_tf, tf); 103 | 104 | // transformed route 105 | EXPECT_EQ(route_tf.destination, point_out); 106 | EXPECT_EQ(route_tf.intermediate_destinations.size(), 2); 107 | EXPECT_EQ(route_tf.intermediate_destinations[0], point_out); 108 | EXPECT_EQ(route_tf.intermediate_destinations[1], point_out); 109 | EXPECT_EQ(route_tf.route_elements.size(), 1); 110 | EXPECT_EQ(route_tf.route_elements[0].lane_elements.size(), 1); 111 | EXPECT_EQ(route_tf.route_elements[0].left_boundary, point_out); 112 | EXPECT_EQ(route_tf.route_elements[0].right_boundary, point_out); 113 | EXPECT_EQ(route_tf.route_elements[0].lane_elements[0].reference_pose, pose_out); 114 | EXPECT_EQ(route_tf.route_elements[0].lane_elements[0].left_boundary.point, point_out); 115 | EXPECT_EQ(route_tf.route_elements[0].lane_elements[0].right_boundary.point, point_out); 116 | EXPECT_EQ(route_tf.route_elements[0].regulatory_elements.size(), 1); 117 | EXPECT_EQ(route_tf.route_elements[0].regulatory_elements[0].reference_line.size(), 2); 118 | EXPECT_EQ(route_tf.route_elements[0].regulatory_elements[0].reference_line[0], point_out); 119 | EXPECT_EQ(route_tf.route_elements[0].regulatory_elements[0].reference_line[1], point_out); 120 | EXPECT_EQ(route_tf.route_elements[0].regulatory_elements[0].positions[0], point_out); 121 | 122 | } 123 | 124 | int main(int argc, char *argv[]) { 125 | testing::InitGoogleTest(&argc, argv); 126 | return RUN_ALL_TESTS(); 127 | } 128 | -------------------------------------------------------------------------------- /tf2_route_planning_msgs/test/test_tf2_route_planning_msgs.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | import pytest 26 | from geometry_msgs.msg import TransformStamped, Point, Pose 27 | from route_planning_msgs.msg import Route, LaneBoundary, LaneElement, RegulatoryElement, RouteElement 28 | from tf2_route_planning_msgs import do_transform_route 29 | 30 | from route_planning_msgs_utils.route_setters import set_left_boundary_of_lane_element, set_right_boundary_of_lane_element 31 | 32 | from copy import deepcopy 33 | 34 | def test_do_transform_route(): 35 | point_in = Point(x=1.0, y=2.0, z=3.0) 36 | point_out = Point(x=9.0, y=18.0, z=33.0) 37 | 38 | pose_in = Pose() 39 | pose_in.position = deepcopy(point_in) 40 | pose_in.orientation.x = 0.0 41 | pose_in.orientation.y = 0.0 42 | pose_in.orientation.z = 1.0 43 | pose_in.orientation.w = 0.0 44 | 45 | pose_out = Pose() 46 | pose_out.position = deepcopy(point_out) 47 | pose_out.orientation.x = 0.0 48 | pose_out.orientation.y = 0.0 49 | pose_out.orientation.z = 0.0 50 | pose_out.orientation.w = 1.0 51 | 52 | # Define a LaneBoundary 53 | lane_boundary = LaneBoundary() 54 | lane_boundary.point = deepcopy(point_in) 55 | 56 | # Define a LaneElement 57 | lane_element = LaneElement() 58 | lane_element.reference_pose = deepcopy(pose_in) 59 | set_left_boundary_of_lane_element(lane_element, deepcopy(lane_boundary)) 60 | set_right_boundary_of_lane_element(lane_element, deepcopy(lane_boundary)) 61 | 62 | # Define a RegulatoryElement 63 | regulatory_element = RegulatoryElement() 64 | regulatory_element.reference_line = [deepcopy(point_in), deepcopy(point_in)] 65 | regulatory_element.positions.append(deepcopy(point_in)) 66 | 67 | # Define a RouteElement 68 | route_element = RouteElement() 69 | route_element.lane_elements.append(lane_element) 70 | route_element.left_boundary = deepcopy(point_in) 71 | route_element.right_boundary = deepcopy(point_in) 72 | route_element.regulatory_elements.append(regulatory_element) 73 | 74 | # Define a Route 75 | route = Route() 76 | route.destination = deepcopy(point_in) 77 | route.intermediate_destinations.append(deepcopy(point_in)) 78 | route.intermediate_destinations.append(deepcopy(point_in)) 79 | route.route_elements.append(deepcopy(route_element)) 80 | 81 | # Create a TransformStamped object 82 | tf = TransformStamped() 83 | tf.transform.translation.x = 10.0 84 | tf.transform.translation.y = 20.0 85 | tf.transform.translation.z = 30.0 86 | tf.transform.rotation.x = 0.0 87 | tf.transform.rotation.y = 0.0 88 | tf.transform.rotation.z = 1.0 89 | tf.transform.rotation.w = 0.0 90 | 91 | # Perform the transformation 92 | route_tf = do_transform_route(route, tf) 93 | 94 | # Assert the transformed route 95 | assert route_tf.destination == point_out 96 | assert len(route_tf.intermediate_destinations) == 2 97 | assert route_tf.intermediate_destinations[0] == point_out 98 | assert route_tf.intermediate_destinations[1] == point_out 99 | assert len(route_tf.route_elements) == 1 100 | assert len(route_tf.route_elements[0].lane_elements) == 1 101 | assert route_tf.route_elements[0].left_boundary == point_out 102 | assert route_tf.route_elements[0].right_boundary == point_out 103 | assert route_tf.route_elements[0].lane_elements[0].reference_pose == pose_out 104 | assert route_tf.route_elements[0].lane_elements[0].left_boundary.point == point_out 105 | assert route_tf.route_elements[0].lane_elements[0].right_boundary.point == point_out 106 | assert len(route_tf.route_elements[0].regulatory_elements) == 1 107 | assert len(route_tf.route_elements[0].regulatory_elements[0].reference_line) == 2 108 | assert route_tf.route_elements[0].regulatory_elements[0].reference_line[0] == point_out 109 | assert route_tf.route_elements[0].regulatory_elements[0].reference_line[1] == point_out 110 | assert route_tf.route_elements[0].regulatory_elements[0].positions[0] == point_out 111 | 112 | if __name__ == "__main__": 113 | pytest.main() -------------------------------------------------------------------------------- /route_planning_msgs_rviz_plugins/include/route_planning_msgs/tools/route/ref_path_tool.hpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include "geometry_msgs/msg/pose_array.hpp" 34 | #include "geometry_msgs/msg/pose_stamped.hpp" 35 | #include "rclcpp/node.hpp" 36 | #include "rclcpp/qos.hpp" 37 | 38 | #include "rviz_default_plugins/tools/pose/pose_tool.hpp" 39 | #include "rviz_default_plugins/visibility_control.hpp" 40 | #include "rviz_rendering/objects/line.hpp" 41 | 42 | #include "route_planning_msgs/msg/route.hpp" 43 | 44 | namespace rviz_common { 45 | class DisplayContext; 46 | namespace properties { 47 | class StringProperty; 48 | class QosProfileProperty; 49 | class BoolProperty; 50 | class FloatProperty; 51 | class VectorProperty; 52 | } // namespace properties 53 | } // namespace rviz_common 54 | 55 | namespace route_planning_msgs { 56 | namespace tools { 57 | class ReferencePathTool : public rviz_default_plugins::tools::PoseTool { 58 | Q_OBJECT 59 | 60 | public: 61 | ReferencePathTool(); 62 | 63 | ~ReferencePathTool() override; 64 | 65 | void onInitialize() override; 66 | 67 | void activate() override; 68 | void deactivate() override; 69 | 70 | protected: 71 | void onPoseSet(double x, double y, double theta) override; 72 | int processMouseEvent(rviz_common::ViewportMouseEvent& event) override; 73 | 74 | private Q_SLOTS: 75 | void drawLinesBetweenPoints(std::vector points); 76 | void updateTopic(); 77 | void updateInitPoint(); 78 | bool setInitPoint(); 79 | void updateFrame(); 80 | int processMouseLeftButtonPressed(); 81 | int processMouseRightButtonPressed(std::pair xy_plane_intersection); 82 | int processMouseMiddleButtonPressed(); 83 | bool fillRoute(std::vector ref_path_points); 84 | void initRoute(); 85 | void updateGeoFence(); 86 | void updateShowGeoFence(); 87 | void updateFirstPoint(); 88 | void updateSecondPoint(); 89 | void updateThirdPoint(); 90 | void updateFourthPoint(); 91 | void updateSampling(); 92 | void updateSamplingDistance(); 93 | double calculateArea(double x1, double y1, double x2, double y2, double x3, double y3); 94 | bool isPointInTriangle(double px, double py, double x1, double y1, double x2, double y2, double x3, double y3); 95 | bool checkIfPointIsInsideGeoFence(double px, double py); 96 | void drawGeoFence(std::vector points); 97 | 98 | private: 99 | rclcpp::Publisher::SharedPtr route_publisher_; 100 | rclcpp::Clock::SharedPtr clock_; 101 | rclcpp::QoS qos_profile_; 102 | 103 | rviz_common::properties::StringProperty* topic_property_; 104 | rviz_common::properties::QosProfileProperty* qos_profile_property_; 105 | rviz_common::properties::BoolProperty* init_point_property_; 106 | rviz_common::properties::StringProperty* frame_property_; 107 | rviz_common::properties::BoolProperty* geo_fence_property_; 108 | rviz_common::properties::BoolProperty* geo_fence_visible_property_; 109 | rviz_common::properties::VectorProperty* first_point_property_; 110 | rviz_common::properties::VectorProperty* second_point_property_; 111 | rviz_common::properties::VectorProperty* third_point_property_; 112 | rviz_common::properties::VectorProperty* fourth_point_property_; 113 | rviz_common::properties::BoolProperty* sampling_property_; 114 | rviz_common::properties::FloatProperty* sampling_distance_property_; 115 | 116 | std::vector ref_path_points_; 117 | std::vector points_quadrilateral_; 118 | std::vector> lines_; 119 | std::vector> lines_geo_fence_; 120 | 121 | std::unique_ptr tf2_buffer_; 122 | std::shared_ptr tf2_listener_; 123 | 124 | route_planning_msgs::msg::Route route_; 125 | }; 126 | 127 | } // namespace tools 128 | } // namespace route_planning_msgs -------------------------------------------------------------------------------- /tf2_route_planning_msgs/include/tf2_route_planning_msgs/tf2_route_planning_msgs.hpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | namespace tf2 { 36 | 37 | using namespace route_planning_msgs::msg; 38 | 39 | // LaneBoundary 40 | template <> 41 | inline void doTransform(const LaneBoundary& lane_boundary_in, LaneBoundary& lane_boundary_out, 42 | const geometry_msgs::msg::TransformStamped& transform) { 43 | lane_boundary_out = lane_boundary_in; 44 | 45 | // point 46 | doTransform(lane_boundary_in.point, lane_boundary_out.point, transform); 47 | } 48 | 49 | // LaneElement 50 | template <> 51 | inline void doTransform(const LaneElement& lane_element_in, LaneElement& lane_element_out, 52 | const geometry_msgs::msg::TransformStamped& transform) { 53 | lane_element_out = lane_element_in; 54 | 55 | doTransform(lane_element_in.reference_pose, lane_element_out.reference_pose, transform); 56 | doTransform(lane_element_in.left_boundary, lane_element_out.left_boundary, transform); 57 | doTransform(lane_element_in.right_boundary, lane_element_out.right_boundary, transform); 58 | } 59 | 60 | // RegulatoryElement 61 | template <> 62 | inline void doTransform(const RegulatoryElement& regulatory_element_in, RegulatoryElement& regulatory_element_out, 63 | const geometry_msgs::msg::TransformStamped& transform) { 64 | regulatory_element_out = regulatory_element_in; 65 | 66 | // effect line 67 | for (size_t i = 0; i < regulatory_element_in.reference_line.size(); i++) { 68 | doTransform(regulatory_element_in.reference_line[i], regulatory_element_out.reference_line[i], transform); 69 | } 70 | 71 | // sign positions 72 | for (size_t i = 0; i < regulatory_element_in.positions.size(); i++) { 73 | doTransform(regulatory_element_in.positions[i], regulatory_element_out.positions[i], transform); 74 | } 75 | } 76 | 77 | // RouteElement 78 | template <> 79 | inline void doTransform(const RouteElement& route_element_in, RouteElement& route_element_out, 80 | const geometry_msgs::msg::TransformStamped& transform) { 81 | route_element_out = route_element_in; 82 | 83 | // lane elements 84 | for (size_t i = 0; i < route_element_in.lane_elements.size(); i++) { 85 | doTransform(route_element_in.lane_elements[i], route_element_out.lane_elements[i], transform); 86 | } 87 | 88 | // boundaries (drivable space) 89 | doTransform(route_element_in.left_boundary, route_element_out.left_boundary, transform); 90 | doTransform(route_element_in.right_boundary, route_element_out.right_boundary, transform); 91 | 92 | // regulatory elements 93 | for (size_t i = 0; i < route_element_in.regulatory_elements.size(); i++) { 94 | doTransform(route_element_in.regulatory_elements[i], route_element_out.regulatory_elements[i], transform); 95 | } 96 | } 97 | 98 | // Route 99 | template <> 100 | inline void doTransform(const Route& route_in, Route& route_out, const geometry_msgs::msg::TransformStamped& transform) { 101 | route_out = route_in; 102 | route_out.header.stamp = transform.header.stamp; 103 | route_out.header.frame_id = transform.header.frame_id; 104 | 105 | // destination 106 | doTransform(route_in.destination, route_out.destination, transform); 107 | 108 | // intermediate destinations 109 | for (size_t i = 0; i < route_in.intermediate_destinations.size(); i++) { 110 | doTransform(route_in.intermediate_destinations[i], route_out.intermediate_destinations[i], transform); 111 | } 112 | 113 | // route elements 114 | for (size_t i = 0; i < route_in.route_elements.size(); i++) { 115 | doTransform(route_in.route_elements[i], route_out.route_elements[i], transform); 116 | } 117 | } 118 | 119 | template <> 120 | inline tf2::TimePoint getTimestamp(const Route& route) { return tf2_ros::fromMsg(route.header.stamp); } 121 | 122 | template <> 123 | inline std::string getFrameId(const Route& route) { return route.header.frame_id; } 124 | 125 | } // namespace tf2 126 | -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/test/test_tf2_trajectory_planning_msgs.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | import math 26 | 27 | import pytest 28 | import tf2_geometry_msgs 29 | from geometry_msgs.msg import Pose, TransformStamped 30 | from trajectory_planning_msgs.msg import DRIVABLE, Trajectory 31 | 32 | from tf2_trajectory_planning_msgs import do_transform_trajectory 33 | from trajectory_planning_msgs_utils.init import initialize_trajectory 34 | from trajectory_planning_msgs_utils.state_getters import ( 35 | get_theta_from_trajectory, 36 | get_t_from_trajectory, 37 | get_x_from_trajectory, 38 | get_y_from_trajectory, 39 | ) 40 | from trajectory_planning_msgs_utils.state_setters import ( 41 | set_theta_in_trajectory, 42 | set_t_in_trajectory, 43 | set_x_in_trajectory, 44 | set_y_in_trajectory, 45 | ) 46 | 47 | EPS = 1e-9 48 | 49 | 50 | def _stamp_to_sec(stamp): 51 | return float(stamp.sec) + float(stamp.nanosec) * 1e-9 52 | 53 | 54 | def _quaternion_from_yaw(yaw: float): 55 | half = yaw / 2.0 56 | return 0.0, 0.0, math.sin(half), math.cos(half) 57 | 58 | 59 | def _yaw_from_quaternion(quaternion) -> float: 60 | x, y, z, w = quaternion 61 | siny_cosp = 2.0 * (w * z + x * y) 62 | cosy_cosp = 1.0 - 2.0 * (y * y + z * z) 63 | return math.atan2(siny_cosp, cosy_cosp) 64 | 65 | 66 | def test_do_transform_trajectory_drivable(): 67 | trajectory = Trajectory() 68 | trajectory.header.frame_id = "source" 69 | trajectory.header.stamp.sec = 1 70 | trajectory.header.stamp.nanosec = 200_000_000 71 | trajectory.type_id = DRIVABLE.TYPE_ID 72 | 73 | initialize_trajectory(trajectory, DRIVABLE.TYPE_ID, 1) 74 | 75 | set_t_in_trajectory(trajectory, 0.5, 0) 76 | set_x_in_trajectory(trajectory, 1.0, 0) 77 | set_y_in_trajectory(trajectory, -2.0, 0) 78 | set_theta_in_trajectory(trajectory, 0.25, 0) 79 | 80 | transform = TransformStamped() 81 | transform.header.frame_id = "target" 82 | transform.child_frame_id = "source" 83 | transform.header.stamp.sec = 3 84 | transform.header.stamp.nanosec = 700_000_000 85 | transform.transform.translation.x = 4.0 86 | transform.transform.translation.y = -1.0 87 | transform.transform.translation.z = 0.0 88 | qx, qy, qz, qw = _quaternion_from_yaw(math.pi / 3.0) 89 | transform.transform.rotation.x = qx 90 | transform.transform.rotation.y = qy 91 | transform.transform.rotation.z = qz 92 | transform.transform.rotation.w = qw 93 | 94 | transformed = do_transform_trajectory(trajectory, transform) 95 | 96 | assert transformed.header.frame_id == transform.header.frame_id 97 | assert transformed.header.stamp == transform.header.stamp 98 | 99 | pose_in = Pose() 100 | pose_in.position.x = 1.0 101 | pose_in.position.y = -2.0 102 | pose_in.position.z = 0.0 103 | qx_in, qy_in, qz_in, qw_in = _quaternion_from_yaw(0.25) 104 | pose_in.orientation.x = qx_in 105 | pose_in.orientation.y = qy_in 106 | pose_in.orientation.z = qz_in 107 | pose_in.orientation.w = qw_in 108 | pose_expected = tf2_geometry_msgs.do_transform_pose(pose_in, transform) 109 | 110 | assert math.isclose( 111 | get_x_from_trajectory(transformed, 0), 112 | pose_expected.position.x, 113 | rel_tol=EPS, 114 | abs_tol=EPS, 115 | ) 116 | assert math.isclose( 117 | get_y_from_trajectory(transformed, 0), 118 | pose_expected.position.y, 119 | rel_tol=EPS, 120 | abs_tol=EPS, 121 | ) 122 | 123 | expected_yaw = _yaw_from_quaternion( 124 | ( 125 | pose_expected.orientation.x, 126 | pose_expected.orientation.y, 127 | pose_expected.orientation.z, 128 | pose_expected.orientation.w, 129 | ) 130 | ) 131 | assert math.isclose( 132 | get_theta_from_trajectory(transformed, 0), 133 | expected_yaw, 134 | rel_tol=EPS, 135 | abs_tol=EPS, 136 | ) 137 | 138 | time_offset = _stamp_to_sec(transform.header.stamp) - _stamp_to_sec(trajectory.header.stamp) 139 | assert math.isclose( 140 | get_t_from_trajectory(transformed, 0), 141 | 0.5 + time_offset, 142 | rel_tol=EPS, 143 | abs_tol=EPS, 144 | ) 145 | 146 | 147 | if __name__ == "__main__": 148 | pytest.main() 149 | -------------------------------------------------------------------------------- /tf2_route_planning_msgs/src/tf2_route_planning_msgs/tf2_route_planning_msgs.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | from typing import Iterable 3 | 4 | from geometry_msgs.msg import Point, PointStamped, TransformStamped 5 | from route_planning_msgs.msg import ( 6 | LaneBoundary, 7 | LaneElement, 8 | RegulatoryElement, 9 | Route, 10 | RouteElement, 11 | ) 12 | 13 | import tf2_geometry_msgs 14 | import tf2_ros 15 | 16 | 17 | def _register_passthrough_conversions(message_types: Iterable[type]) -> None: 18 | def _to_msg(msg): 19 | return msg 20 | 21 | def _from_msg(msg): 22 | return msg 23 | 24 | for msg_type in message_types: 25 | tf2_ros.ConvertRegistration().add_to_msg(msg_type, _to_msg) 26 | tf2_ros.ConvertRegistration().add_from_msg(msg_type, _from_msg) 27 | 28 | 29 | _register_passthrough_conversions( 30 | (Route, RouteElement, LaneElement, RegulatoryElement, LaneBoundary) 31 | ) 32 | 33 | 34 | def _transform_point(point: Point, transform: TransformStamped) -> Point: 35 | stamped = PointStamped() 36 | stamped.point = deepcopy(point) 37 | transformed = tf2_geometry_msgs.do_transform_point(stamped, transform) 38 | return transformed.point 39 | 40 | 41 | 42 | def do_transform_lane_boundary(msg: LaneBoundary, transform: TransformStamped) -> LaneBoundary: 43 | """ 44 | Apply a `Transform` or `TransformStamped` to `LaneBoundary`. 45 | :param msg: The msg that should be transformed 46 | :param transform: The transform which will applied to the msg 47 | :returns: The transformed msg 48 | """ 49 | msg_out = deepcopy(msg) 50 | msg_out.point = _transform_point(msg.point, transform) 51 | return msg_out 52 | 53 | tf2_ros.TransformRegistration().add(LaneBoundary, do_transform_lane_boundary) 54 | 55 | def do_transform_lane_element(msg: LaneElement, transform: TransformStamped) -> LaneElement: 56 | """ 57 | Apply a `Transform` or `TransformStamped` to `LaneElement`. 58 | :param msg: The msg that should be transformed 59 | :param transform: The transform which will applied to the msg 60 | :returns: The transformed msg 61 | """ 62 | msg_out = deepcopy(msg) 63 | 64 | # reference_pose 65 | msg_out.reference_pose = tf2_geometry_msgs.do_transform_pose(msg.reference_pose, transform) 66 | 67 | # lane_boundaries 68 | msg_out.left_boundary = do_transform_lane_boundary(msg.left_boundary, transform) 69 | msg_out.right_boundary = do_transform_lane_boundary(msg.right_boundary, transform) 70 | 71 | return msg_out 72 | 73 | tf2_ros.TransformRegistration().add(LaneElement, do_transform_lane_element) 74 | 75 | def do_transform_regulatory_element(msg: RegulatoryElement, transform: TransformStamped) -> RegulatoryElement: 76 | """ 77 | Apply a `Transform` or `TransformStamped` to `RegulatoryElement`. 78 | :param msg: The msg that should be transformed 79 | :param transform: The transform which will applied to the msg 80 | :returns: The transformed msg 81 | """ 82 | msg_out = deepcopy(msg) 83 | 84 | # reference_line 85 | for i in range(len(msg.reference_line)): 86 | msg_out.reference_line[i] = _transform_point(msg.reference_line[i], transform) 87 | 88 | # positions 89 | for i in range(len(msg.positions)): 90 | msg_out.positions[i] = _transform_point(msg.positions[i], transform) 91 | 92 | return msg_out 93 | 94 | tf2_ros.TransformRegistration().add(RegulatoryElement, do_transform_regulatory_element) 95 | 96 | def do_transform_route_element(msg: RouteElement, transform: TransformStamped) -> RouteElement: 97 | """ 98 | Apply a `Transform` or `TransformStamped` to `RouteElement`. 99 | :param msg: The msg that should be transformed 100 | :param transform: The transform which will applied to the msg 101 | :returns: The transformed msg 102 | """ 103 | msg_out = deepcopy(msg) 104 | 105 | # lane_elements 106 | for i in range(len(msg.lane_elements)): 107 | msg_out.lane_elements[i] = do_transform_lane_element(msg.lane_elements[i], transform) 108 | 109 | # boundaries (drivable space) 110 | msg_out.left_boundary = _transform_point(msg.left_boundary, transform) 111 | msg_out.right_boundary = _transform_point(msg.right_boundary, transform) 112 | 113 | # regulatory_elements 114 | for i in range(len(msg.regulatory_elements)): 115 | msg_out.regulatory_elements[i] = do_transform_regulatory_element(msg.regulatory_elements[i], transform) 116 | 117 | return msg_out 118 | 119 | tf2_ros.TransformRegistration().add(RouteElement, do_transform_route_element) 120 | 121 | def do_transform_route(msg: Route, transform: TransformStamped) -> Route: 122 | """ 123 | Apply a `Transform` or `TransformStamped` to `Route`. 124 | :param msg: The msg that should be transformed 125 | :param transform: The transform which will applied to the msg 126 | :returns: The transformed msg 127 | """ 128 | msg_out = deepcopy(msg) 129 | msg_out.header.stamp = transform.header.stamp 130 | msg_out.header.frame_id = transform.header.frame_id 131 | 132 | # destination 133 | msg_out.destination = _transform_point(msg.destination, transform) 134 | 135 | # intermediate destinations 136 | for i in range(len(msg.intermediate_destinations)): 137 | msg_out.intermediate_destinations[i] = _transform_point(msg.intermediate_destinations[i], transform) 138 | 139 | # route_elements 140 | for i in range(len(msg.route_elements)): 141 | msg_out.route_elements[i] = do_transform_route_element(msg.route_elements[i], transform) 142 | 143 | return msg_out 144 | 145 | tf2_ros.TransformRegistration().add(Route, do_transform_route) 146 | -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/src/tf2_trajectory_planning_msgs/tf2_trajectory_planning_msgs.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | import math 26 | from copy import deepcopy 27 | from typing import Iterable, Sequence 28 | 29 | from geometry_msgs.msg import Pose, TransformStamped 30 | from trajectory_planning_msgs.msg import DRIVABLE, DRIVABLERWS, REFERENCE, Trajectory 31 | from trajectory_planning_msgs_utils.state_getters import ( 32 | get_sample_point_size_from_trajectory, 33 | get_state_from_trajectory, 34 | get_t, 35 | get_theta, 36 | get_x, 37 | get_y, 38 | ) 39 | from trajectory_planning_msgs_utils.state_setters import ( 40 | set_state_in_trajectory, 41 | set_t, 42 | set_theta, 43 | set_x, 44 | set_y, 45 | ) 46 | 47 | import tf2_geometry_msgs 48 | import tf2_ros 49 | 50 | 51 | def _register_passthrough_conversions(message_types: Iterable[type]) -> None: 52 | def _to_msg(msg): 53 | return msg 54 | 55 | def _from_msg(msg): 56 | return msg 57 | 58 | for msg_type in message_types: 59 | tf2_ros.ConvertRegistration().add_to_msg(msg_type, _to_msg) 60 | tf2_ros.ConvertRegistration().add_from_msg(msg_type, _from_msg) 61 | 62 | 63 | _register_passthrough_conversions((Trajectory,)) 64 | 65 | 66 | def _stamp_to_seconds(stamp) -> float: 67 | return float(stamp.sec) + float(stamp.nanosec) * 1e-9 68 | 69 | 70 | def _stamps_to_time_delta(stamp_a, stamp_b) -> float: 71 | """ 72 | Compute (stamp_a - stamp_b) in seconds. 73 | """ 74 | return _stamp_to_seconds(stamp_a) - _stamp_to_seconds(stamp_b) 75 | 76 | 77 | def _state_has_theta(type_id: int) -> bool: 78 | return type_id in (DRIVABLE.TYPE_ID, DRIVABLERWS.TYPE_ID) 79 | 80 | 81 | def _quaternion_from_yaw(yaw: float): 82 | half = yaw / 2.0 83 | return 0.0, 0.0, math.sin(half), math.cos(half) 84 | 85 | 86 | def _yaw_from_quaternion(quaternion) -> float: 87 | x, y, z, w = quaternion 88 | siny_cosp = 2.0 * (w * z + x * y) 89 | cosy_cosp = 1.0 - 2.0 * (y * y + z * z) 90 | return math.atan2(siny_cosp, cosy_cosp) 91 | 92 | 93 | def _do_transform_state( 94 | state: Sequence[float], 95 | type_id: int, 96 | transform: TransformStamped, 97 | time_offset: float, 98 | ) -> Sequence[float]: 99 | x = get_x(state, type_id) 100 | y = get_y(state, type_id) 101 | t = get_t(state, type_id) 102 | theta = get_theta(state, type_id) if _state_has_theta(type_id) else 0.0 103 | 104 | pose_in = Pose() 105 | pose_in.position.x = x 106 | pose_in.position.y = y 107 | pose_in.position.z = 0.0 108 | qx, qy, qz, qw = _quaternion_from_yaw(theta) 109 | pose_in.orientation.x = qx 110 | pose_in.orientation.y = qy 111 | pose_in.orientation.z = qz 112 | pose_in.orientation.w = qw 113 | 114 | pose_out = tf2_geometry_msgs.do_transform_pose(pose_in, transform) 115 | 116 | state_out = list(state) 117 | set_x(state_out, type_id, pose_out.position.x) 118 | set_y(state_out, type_id, pose_out.position.y) 119 | set_t(state_out, type_id, t + time_offset) 120 | 121 | if _state_has_theta(type_id): 122 | yaw = _yaw_from_quaternion( 123 | ( 124 | pose_out.orientation.x, 125 | pose_out.orientation.y, 126 | pose_out.orientation.z, 127 | pose_out.orientation.w, 128 | ) 129 | ) 130 | set_theta(state_out, type_id, yaw) 131 | 132 | return state_out 133 | 134 | 135 | def do_transform_trajectory(msg: Trajectory, transform: TransformStamped) -> Trajectory: 136 | """ 137 | Apply a `Transform` or `TransformStamped` to a `Trajectory`. 138 | """ 139 | msg_out = deepcopy(msg) 140 | msg_out.header.stamp = transform.header.stamp 141 | msg_out.header.frame_id = transform.header.frame_id 142 | 143 | time_offset = _stamps_to_time_delta(transform.header.stamp, msg.header.stamp) 144 | 145 | sample_points = get_sample_point_size_from_trajectory(msg) 146 | for i in range(sample_points): 147 | state_in = get_state_from_trajectory(msg, i) 148 | state_out = _do_transform_state(state_in, msg.type_id, transform, time_offset) 149 | set_state_in_trajectory(msg_out, state_out, i) 150 | 151 | return msg_out 152 | 153 | 154 | tf2_ros.TransformRegistration().add(Trajectory, do_transform_trajectory) 155 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/src/trajectory_planning_msgs_utils/state_index.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | from trajectory_planning_msgs.msg import DRIVABLE, DRIVABLERWS, REFERENCE 26 | 27 | 28 | kExceptionUnknownStateEntry = ( 29 | "Trajectory type with the following ID does not support requested entry: " 30 | ) 31 | 32 | 33 | def index_t(type_id: int) -> int: 34 | if type_id == DRIVABLE.TYPE_ID: 35 | return DRIVABLE.T 36 | if type_id == DRIVABLERWS.TYPE_ID: 37 | return DRIVABLERWS.T 38 | if type_id == REFERENCE.TYPE_ID: 39 | return REFERENCE.T 40 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, t") 41 | 42 | 43 | def index_x(type_id: int) -> int: 44 | if type_id == DRIVABLE.TYPE_ID: 45 | return DRIVABLE.X 46 | if type_id == DRIVABLERWS.TYPE_ID: 47 | return DRIVABLERWS.X 48 | if type_id == REFERENCE.TYPE_ID: 49 | return REFERENCE.X 50 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, x") 51 | 52 | 53 | def index_y(type_id: int) -> int: 54 | if type_id == DRIVABLE.TYPE_ID: 55 | return DRIVABLE.Y 56 | if type_id == DRIVABLERWS.TYPE_ID: 57 | return DRIVABLERWS.Y 58 | if type_id == REFERENCE.TYPE_ID: 59 | return REFERENCE.Y 60 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, y") 61 | 62 | 63 | def index_v(type_id: int) -> int: 64 | if type_id == DRIVABLE.TYPE_ID: 65 | return DRIVABLE.V 66 | if type_id == DRIVABLERWS.TYPE_ID: 67 | return DRIVABLERWS.V 68 | if type_id == REFERENCE.TYPE_ID: 69 | return REFERENCE.V 70 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, v") 71 | 72 | 73 | def index_theta(type_id: int) -> int: 74 | if type_id == DRIVABLE.TYPE_ID: 75 | return DRIVABLE.THETA 76 | if type_id == DRIVABLERWS.TYPE_ID: 77 | return DRIVABLERWS.THETA 78 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, theta") 79 | 80 | 81 | def index_a(type_id: int) -> int: 82 | if type_id == DRIVABLE.TYPE_ID: 83 | return DRIVABLE.A 84 | if type_id == DRIVABLERWS.TYPE_ID: 85 | return DRIVABLERWS.A 86 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, a") 87 | 88 | 89 | def index_beta(type_id: int) -> int: 90 | if type_id == DRIVABLERWS.TYPE_ID: 91 | return DRIVABLERWS.BETA 92 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, beta") 93 | 94 | 95 | def index_delta_front(type_id: int) -> int: 96 | if type_id == DRIVABLERWS.TYPE_ID: 97 | return DRIVABLERWS.DELTAFRONT 98 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, delta_front") 99 | 100 | 101 | def index_delta_rear(type_id: int) -> int: 102 | if type_id == DRIVABLERWS.TYPE_ID: 103 | return DRIVABLERWS.DELTAREAR 104 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, delta_rear") 105 | 106 | 107 | def index_delta_ack(type_id: int) -> int: 108 | if type_id == DRIVABLE.TYPE_ID: 109 | return DRIVABLE.DELTA 110 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, delta") 111 | 112 | 113 | def index_s(type_id: int) -> int: 114 | if type_id == DRIVABLE.TYPE_ID: 115 | return DRIVABLE.S 116 | if type_id == DRIVABLERWS.TYPE_ID: 117 | return DRIVABLERWS.S 118 | raise ValueError(kExceptionUnknownStateEntry + f"{type_id}, s") 119 | 120 | 121 | def has_t(type_id: int) -> bool: 122 | return type_id in {DRIVABLE.TYPE_ID, DRIVABLERWS.TYPE_ID, REFERENCE.TYPE_ID} 123 | 124 | 125 | def has_x(type_id: int) -> bool: 126 | return type_id in {DRIVABLE.TYPE_ID, DRIVABLERWS.TYPE_ID, REFERENCE.TYPE_ID} 127 | 128 | 129 | def has_y(type_id: int) -> bool: 130 | return type_id in {DRIVABLE.TYPE_ID, DRIVABLERWS.TYPE_ID, REFERENCE.TYPE_ID} 131 | 132 | 133 | def has_v(type_id: int) -> bool: 134 | return type_id in {DRIVABLE.TYPE_ID, DRIVABLERWS.TYPE_ID, REFERENCE.TYPE_ID} 135 | 136 | 137 | def has_theta(type_id: int) -> bool: 138 | return type_id in {DRIVABLE.TYPE_ID, DRIVABLERWS.TYPE_ID} 139 | 140 | 141 | def has_a(type_id: int) -> bool: 142 | return type_id in {DRIVABLE.TYPE_ID, DRIVABLERWS.TYPE_ID} 143 | 144 | 145 | def has_beta(type_id: int) -> bool: 146 | return type_id == DRIVABLERWS.TYPE_ID 147 | 148 | 149 | def has_delta_front(type_id: int) -> bool: 150 | return type_id == DRIVABLERWS.TYPE_ID 151 | 152 | 153 | def has_delta_rear(type_id: int) -> bool: 154 | return type_id == DRIVABLERWS.TYPE_ID 155 | 156 | 157 | def has_delta_ack(type_id: int) -> bool: 158 | return type_id == DRIVABLE.TYPE_ID 159 | 160 | 161 | def has_s(type_id: int) -> bool: 162 | return type_id in {DRIVABLE.TYPE_ID, DRIVABLERWS.TYPE_ID} 163 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/test/test_trajectory_access.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | import math 26 | 27 | import pytest 28 | 29 | from trajectory_planning_msgs.msg import DRIVABLE, DRIVABLERWS, REFERENCE, Trajectory 30 | 31 | from trajectory_planning_msgs_utils.init import initialize_states, initialize_trajectory 32 | from trajectory_planning_msgs_utils.state_getters import ( 33 | get_beta_from_trajectory, 34 | get_delta_ack_from_trajectory, 35 | get_delta_front_from_trajectory, 36 | get_sample_point_size_from_trajectory, 37 | get_standstill, 38 | get_t_from_trajectory, 39 | get_theta_from_trajectory, 40 | get_x_from_trajectory, 41 | get_y_from_trajectory, 42 | ) 43 | from trajectory_planning_msgs_utils.state_setters import ( 44 | compute_beta, 45 | set_beta_from_axles_in_trajectory, 46 | set_delta_ack_in_trajectory, 47 | set_delta_front_in_trajectory, 48 | set_s_in_trajectory, 49 | set_standstill, 50 | set_t_in_trajectory, 51 | set_theta_in_trajectory, 52 | set_x_in_trajectory, 53 | set_y_in_trajectory, 54 | ) 55 | from trajectory_planning_msgs_utils.utils import get_state_dim, wrap_angle 56 | 57 | EPS = 1e-12 58 | 59 | 60 | def test_wrap_angle(): 61 | assert math.isclose(wrap_angle(4 * math.pi), 0.0, rel_tol=EPS, abs_tol=EPS) 62 | assert math.isclose(wrap_angle(-3 * math.pi / 2), math.pi / 2, rel_tol=EPS, abs_tol=EPS) 63 | 64 | 65 | def test_initialize_states(): 66 | states = [] 67 | initialize_states(states, DRIVABLE.TYPE_ID, 2) 68 | assert len(states) == 2 * get_state_dim(DRIVABLE.TYPE_ID) 69 | assert all(value == 0.0 for value in states) 70 | 71 | 72 | def test_initialize_trajectory_and_basic_setters(): 73 | trajectory = Trajectory() 74 | initialize_trajectory(trajectory, DRIVABLE.TYPE_ID, 2) 75 | 76 | assert trajectory.type_id == DRIVABLE.TYPE_ID 77 | assert get_sample_point_size_from_trajectory(trajectory) == 2 78 | assert get_standstill(trajectory) 79 | 80 | set_t_in_trajectory(trajectory, 1.0, 0) 81 | set_x_in_trajectory(trajectory, 2.0, 0) 82 | set_y_in_trajectory(trajectory, -1.0, 0) 83 | set_s_in_trajectory(trajectory, 5.0, 0) 84 | 85 | assert math.isclose(get_t_from_trajectory(trajectory, 0), 1.0, rel_tol=EPS) 86 | assert math.isclose(get_x_from_trajectory(trajectory, 0), 2.0, rel_tol=EPS) 87 | assert math.isclose(get_y_from_trajectory(trajectory, 0), -1.0, rel_tol=EPS) 88 | 89 | # Angles are wrapped into [-pi, pi] 90 | set_theta_in_trajectory(trajectory, math.pi + 0.3, 0) 91 | theta = get_theta_from_trajectory(trajectory, 0) 92 | assert -math.pi <= theta <= math.pi 93 | assert math.isclose(theta, wrap_angle(math.pi + 0.3), rel_tol=EPS, abs_tol=EPS) 94 | 95 | set_delta_ack_in_trajectory(trajectory, -4.2, 0) 96 | delta = get_delta_ack_from_trajectory(trajectory, 0) 97 | assert -math.pi <= delta <= math.pi 98 | 99 | set_standstill(trajectory, False) 100 | assert not get_standstill(trajectory) 101 | 102 | 103 | def test_reference_theta_approximation(): 104 | trajectory = Trajectory() 105 | initialize_trajectory(trajectory, REFERENCE.TYPE_ID, 3) 106 | 107 | set_x_in_trajectory(trajectory, 0.0, 0) 108 | set_y_in_trajectory(trajectory, 0.0, 0) 109 | 110 | set_x_in_trajectory(trajectory, 1.0, 1) 111 | set_y_in_trajectory(trajectory, 0.0, 1) 112 | 113 | set_x_in_trajectory(trajectory, 1.0, 2) 114 | set_y_in_trajectory(trajectory, 1.0, 2) 115 | 116 | theta_mid = get_theta_from_trajectory(trajectory, 1) 117 | assert math.isclose(theta_mid, math.pi / 4, rel_tol=1e-9) 118 | 119 | theta_start = get_theta_from_trajectory(trajectory, 0) 120 | assert math.isclose(theta_start, 0.0, rel_tol=1e-9) 121 | 122 | theta_end = get_theta_from_trajectory(trajectory, 2) 123 | assert math.isclose(theta_end, math.pi / 2, rel_tol=1e-9) 124 | 125 | 126 | def test_beta_computation_for_drivablerws(): 127 | trajectory = Trajectory() 128 | initialize_trajectory(trajectory, DRIVABLERWS.TYPE_ID, 1) 129 | 130 | delta_front = 0.1 131 | delta_rear = -0.05 132 | distance_front_axle = 1.2 133 | distance_rear_axle = 1.3 134 | 135 | set_beta_from_axles_in_trajectory( 136 | trajectory, 137 | delta_front, 138 | delta_rear, 139 | distance_front_axle, 140 | distance_rear_axle, 141 | 0, 142 | ) 143 | beta = get_beta_from_trajectory(trajectory, 0) 144 | expected = compute_beta(delta_front, delta_rear, distance_front_axle, distance_rear_axle) 145 | assert math.isclose(beta, expected, rel_tol=1e-9) 146 | 147 | set_delta_front_in_trajectory(trajectory, math.pi / 2, 0) 148 | delta_front_read = get_delta_front_from_trajectory(trajectory, 0) 149 | assert -math.pi <= delta_front_read <= math.pi 150 | -------------------------------------------------------------------------------- /tf2_trajectory_planning_msgs/test/impl/test_tf2_trajectory_planning_msgs.cpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #include 26 | 27 | #include 28 | 29 | using namespace trajectory_planning_msgs::trajectory_access; 30 | 31 | static const double EPS = 1e-12; 32 | 33 | TEST(tf2_trajectory_planning_msgs, test_doTransform_REFERENCE) { 34 | 35 | Trajectory trajectory, trajectory_tf; 36 | initializeTrajectory(trajectory, REFERENCE::TYPE_ID, 2); 37 | 38 | for (int i = 0; i < getSamplePointSize(trajectory); i++) { 39 | setT(trajectory, i*1.0, i); 40 | setX(trajectory, i*1.0, i); 41 | setY(trajectory, 1.0, i); 42 | setV(trajectory, 1.0, i); 43 | } 44 | 45 | gm::TransformStamped tf; 46 | tf.transform.translation.x = 10.0; 47 | tf.transform.translation.y = 20.0; 48 | tf.transform.translation.z = 0.0; 49 | tf.transform.rotation.x = 0.0; 50 | tf.transform.rotation.y = 0.0; 51 | tf.transform.rotation.z = sin(M_PI / 4); 52 | tf.transform.rotation.w = cos(M_PI / 4); 53 | 54 | tf2::doTransform(trajectory, trajectory_tf, tf); 55 | 56 | for (int i = 0; i < getSamplePointSize(trajectory); i++) { 57 | 58 | // transformed state 59 | EXPECT_NEAR(getX(trajectory_tf, i), 9, EPS); 60 | EXPECT_NEAR(getY(trajectory_tf, i), i + 20, EPS); 61 | EXPECT_NEAR(getTheta(trajectory_tf, i), M_PI / 2, EPS); 62 | 63 | // transform-invariant state 64 | EXPECT_NEAR(getV(trajectory_tf, i), 1.0, EPS); 65 | } 66 | } 67 | 68 | 69 | TEST(tf2_trajectory_planning_msgs, test_doTransform_DRIVABLE) { 70 | 71 | Trajectory trajectory, trajectory_tf; 72 | initializeTrajectory(trajectory, DRIVABLE::TYPE_ID, 2); 73 | 74 | for (int i = 0; i < getSamplePointSize(trajectory); i++) { 75 | setT(trajectory, 1.0, i); 76 | setX(trajectory, 1.0, i); 77 | setY(trajectory, 2.0, i); 78 | setV(trajectory, 1.0, i); 79 | setTheta(trajectory, M_PI, i); 80 | setA(trajectory, 1.0, i); 81 | setDeltaAck(trajectory, 1.0, i); 82 | setS(trajectory, 1.0, i); 83 | } 84 | 85 | gm::TransformStamped tf; 86 | tf.transform.translation.x = 10.0; 87 | tf.transform.translation.y = 20.0; 88 | tf.transform.translation.z = 30.0; 89 | tf.transform.rotation.x = 0.0; 90 | tf.transform.rotation.y = 0.0; 91 | tf.transform.rotation.z = 1.0; 92 | tf.transform.rotation.w = 0.0; 93 | 94 | tf2::doTransform(trajectory, trajectory_tf, tf); 95 | 96 | for (int i = 0; i < getSamplePointSize(trajectory); i++) { 97 | 98 | // transformed state 99 | EXPECT_NEAR(getX(trajectory_tf, i), 9.0, EPS); 100 | EXPECT_NEAR(getY(trajectory_tf, i), 18.0, EPS); 101 | EXPECT_NEAR(getTheta(trajectory_tf, i), 0.0, EPS); 102 | 103 | // transform-invariant state 104 | EXPECT_NEAR(getV(trajectory_tf, i), 1.0, EPS); 105 | EXPECT_NEAR(getA(trajectory_tf, i), 1.0, EPS); 106 | EXPECT_NEAR(getDeltaAck(trajectory_tf, i), 1.0, EPS); 107 | EXPECT_NEAR(getS(trajectory_tf, i), 1.0, EPS); 108 | } 109 | } 110 | 111 | TEST(tf2_trajectory_planning_msgs, test_doTransform_DRIVABLERWS) { 112 | 113 | Trajectory trajectory, trajectory_tf; 114 | initializeTrajectory(trajectory, DRIVABLERWS::TYPE_ID, 2); 115 | 116 | double delta_front = 1.2; 117 | double delta_rear = 0.2; 118 | double distance_front_axle = 1.5; 119 | double distance_rear_axle = 1.9; 120 | double beta = atan((distance_rear_axle / (distance_front_axle + distance_rear_axle)) * tan(delta_front) 121 | + (distance_front_axle / (distance_front_axle + distance_rear_axle)) * tan(delta_rear)); 122 | 123 | for (int i = 0; i < getSamplePointSize(trajectory); i++) { 124 | setT(trajectory, 1.0, i); 125 | setX(trajectory, 1.0, i); 126 | setY(trajectory, 2.0, i); 127 | setV(trajectory, 1.0, i); 128 | setTheta(trajectory, M_PI, i); 129 | setA(trajectory, 1.0, i); 130 | setBeta(trajectory, delta_front, delta_rear, distance_front_axle, distance_rear_axle, i); 131 | setDeltaFront(trajectory, M_PI, i); 132 | setDeltaRear(trajectory, M_PI, i); 133 | setS(trajectory, 1.0, i); 134 | } 135 | 136 | gm::TransformStamped tf; 137 | tf.transform.translation.x = 10.0; 138 | tf.transform.translation.y = 20.0; 139 | tf.transform.translation.z = 30.0; 140 | tf.transform.rotation.x = 0.0; 141 | tf.transform.rotation.y = 0.0; 142 | tf.transform.rotation.z = 1.0; 143 | tf.transform.rotation.w = 0.0; 144 | 145 | tf2::doTransform(trajectory, trajectory_tf, tf); 146 | 147 | for (int i = 0; i < getSamplePointSize(trajectory); i++) { 148 | 149 | // transformed state 150 | EXPECT_NEAR(getX(trajectory_tf, i), 9.0, EPS); 151 | EXPECT_NEAR(getY(trajectory_tf, i), 18.0, EPS); 152 | EXPECT_NEAR(getTheta(trajectory_tf, i), 0.0, EPS); 153 | 154 | // transform-invariant state 155 | EXPECT_NEAR(getV(trajectory_tf, i), 1.0, EPS); 156 | EXPECT_NEAR(getA(trajectory_tf, i), 1.0, EPS); 157 | EXPECT_NEAR(getBeta(trajectory_tf, i), beta, EPS); 158 | EXPECT_NEAR(getDeltaFront(trajectory_tf, i), M_PI, EPS); 159 | EXPECT_NEAR(getDeltaRear(trajectory_tf, i), M_PI, EPS); 160 | EXPECT_NEAR(getS(trajectory_tf, i), 1.0, EPS); 161 | } 162 | } 163 | 164 | 165 | int main(int argc, char *argv[]) { 166 | testing::InitGoogleTest(&argc, argv); 167 | return RUN_ALL_TESTS(); 168 | } 169 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/test/impl/test_trajectory_access.cpp: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #include 26 | #include 27 | 28 | using namespace trajectory_planning_msgs; 29 | using namespace trajectory_planning_msgs::trajectory_access; 30 | 31 | std::uniform_real_distribution uniform_distribution(-1, 1); 32 | std::default_random_engine random_engine; 33 | 34 | double randomValue() { return uniform_distribution(random_engine); } 35 | static const double EPS = 1e-12; 36 | 37 | TEST(trajectory_planning_msgs, test_set_get_REFERENCE) { 38 | Trajectory tra; 39 | int nSamplePoints = 8; 40 | initializeTrajectory(tra, REFERENCE::TYPE_ID, nSamplePoints); 41 | 42 | double val; 43 | 44 | for (int i = 0; i < getSamplePointSize(tra); i++) { 45 | // set/getT 46 | val = randomValue(); 47 | setT(tra, val, i); 48 | EXPECT_DOUBLE_EQ(val, getT(tra, i)); 49 | 50 | // set/getX 51 | val = randomValue(); 52 | setX(tra, val, i); 53 | EXPECT_DOUBLE_EQ(val, getX(tra, i)); 54 | 55 | // set/getY 56 | val = randomValue(); 57 | setY(tra, val, i); 58 | EXPECT_DOUBLE_EQ(val, getY(tra, i)); 59 | 60 | // set/getV 61 | val = randomValue(); 62 | setV(tra, val, i); 63 | EXPECT_DOUBLE_EQ(val, getV(tra, i)); 64 | } 65 | 66 | Trajectory theta_tra; 67 | nSamplePoints = 3; 68 | initializeTrajectory(theta_tra, REFERENCE::TYPE_ID, nSamplePoints); 69 | // setX_0/Y_0 70 | setX(theta_tra, 0.0, 0); 71 | setY(theta_tra, 0.0, 0); 72 | // setX_1/Y_1 73 | setX(theta_tra, 1.0, 1); 74 | setY(theta_tra, 0.0, 1); 75 | // setX_2/Y_2 76 | setX(theta_tra, 2.0, 2); 77 | setY(theta_tra, 1.0, 2); 78 | 79 | EXPECT_NEAR(getTheta(theta_tra, 0), 0.0, EPS); 80 | EXPECT_NEAR(getTheta(theta_tra, 1), std::atan(1.0/2.0), EPS); 81 | EXPECT_NEAR(getTheta(theta_tra, 2), std::atan(1.0), EPS); 82 | } 83 | 84 | TEST(trajectory_planning_msgs, test_set_get_DRIVABLE) { 85 | Trajectory tra; 86 | int nSamplePoints = 8; 87 | initializeTrajectory(tra, DRIVABLE::TYPE_ID, nSamplePoints); 88 | 89 | double val; 90 | 91 | for (int i = 0; i < getSamplePointSize(tra); i++) { 92 | // set/getT 93 | val = randomValue(); 94 | setT(tra, val, i); 95 | EXPECT_DOUBLE_EQ(val, getT(tra, i)); 96 | 97 | // set/getX 98 | val = randomValue(); 99 | setX(tra, val, i); 100 | EXPECT_DOUBLE_EQ(val, getX(tra, i)); 101 | 102 | // set/getY 103 | val = randomValue(); 104 | setY(tra, val, i); 105 | EXPECT_DOUBLE_EQ(val, getY(tra, i)); 106 | 107 | // set/getTheta 108 | val = randomValue(); 109 | setTheta(tra, val, i); 110 | EXPECT_DOUBLE_EQ(val, getTheta(tra, i)); 111 | 112 | // set/getV 113 | val = randomValue(); 114 | setV(tra, val, i); 115 | EXPECT_DOUBLE_EQ(val, getV(tra, i)); 116 | 117 | // set/getA 118 | val = randomValue(); 119 | setA(tra, val, i); 120 | EXPECT_DOUBLE_EQ(val, getA(tra, i)); 121 | 122 | // set/getDeltaAck 123 | val = randomValue(); 124 | setDeltaAck(tra, val, i); 125 | EXPECT_DOUBLE_EQ(val, getDeltaAck(tra, i)); 126 | 127 | // set/getS 128 | val = randomValue(); 129 | setS(tra, val, i); 130 | EXPECT_DOUBLE_EQ(val, getS(tra, i)); 131 | } 132 | } 133 | 134 | TEST(trajectory_planning_msgs, test_set_get_DRIVABLERWS) { 135 | Trajectory tra; 136 | int nSamplePoints = 8; 137 | initializeTrajectory(tra, DRIVABLERWS::TYPE_ID, nSamplePoints); 138 | 139 | double val; 140 | double delta_front = 1.2; 141 | double delta_rear = 0.2; 142 | double distance_front_axle = 1.5; 143 | double distance_rear_axle = 1.9; 144 | double beta = atan((distance_rear_axle / (distance_front_axle + distance_rear_axle)) * tan(delta_front) 145 | + (distance_front_axle / (distance_front_axle + distance_rear_axle)) * tan(delta_rear)); 146 | 147 | for (int i = 0; i < getSamplePointSize(tra); i++) { 148 | // set/getT 149 | val = randomValue(); 150 | setT(tra, val, i); 151 | EXPECT_DOUBLE_EQ(val, getT(tra, i)); 152 | 153 | // set/getX 154 | val = randomValue(); 155 | setX(tra, val, i); 156 | EXPECT_DOUBLE_EQ(val, getX(tra, i)); 157 | 158 | // set/getY 159 | val = randomValue(); 160 | setY(tra, val, i); 161 | EXPECT_DOUBLE_EQ(val, getY(tra, i)); 162 | 163 | // set/getV 164 | val = randomValue(); 165 | setV(tra, val, i); 166 | EXPECT_DOUBLE_EQ(val, getV(tra, i)); 167 | 168 | // set/getTheta 169 | val = randomValue(); 170 | setTheta(tra, val, i); 171 | EXPECT_DOUBLE_EQ(val, getTheta(tra, i)); 172 | 173 | // set/getA 174 | val = randomValue(); 175 | setA(tra, val, i); 176 | EXPECT_DOUBLE_EQ(val, getA(tra, i)); 177 | 178 | // set/getBeta(direct and indirect) 179 | setBeta(tra, beta, i); 180 | EXPECT_DOUBLE_EQ(beta, getBeta(tra, i)); 181 | setBeta(tra, delta_front, delta_rear, distance_front_axle, distance_rear_axle, i); 182 | EXPECT_NEAR(getBeta(tra, i), beta, EPS); 183 | 184 | // set/getDeltaFront 185 | val = randomValue(); 186 | setDeltaFront(tra, val, i); 187 | EXPECT_DOUBLE_EQ(val, getDeltaFront(tra, i)); 188 | 189 | // set/getDeltaRear 190 | val = randomValue(); 191 | setDeltaRear(tra, val, i); 192 | EXPECT_DOUBLE_EQ(val, getDeltaRear(tra, i)); 193 | 194 | // set/getS 195 | val = randomValue(); 196 | setS(tra, val, i); 197 | EXPECT_DOUBLE_EQ(val, getS(tra, i)); 198 | } 199 | } 200 | 201 | int main(int argc, char *argv[]) { 202 | testing::InitGoogleTest(&argc, argv); 203 | return RUN_ALL_TESTS(); 204 | } -------------------------------------------------------------------------------- /route_planning_msgs_utils/test/test_route_access.py: -------------------------------------------------------------------------------- 1 | import math 2 | import pytest 3 | 4 | from geometry_msgs.msg import Point 5 | from route_planning_msgs.msg import ( 6 | LaneBoundary, 7 | LaneElement, 8 | RegulatoryElement, 9 | Route, 10 | RouteElement, 11 | ) 12 | from geometry_msgs.msg import Point 13 | 14 | from route_planning_msgs_utils.route_getters import ( 15 | get_adjacent_lane, 16 | get_current_suggested_lane_element, 17 | get_following_lane_element, 18 | get_following_lane_element_idx, 19 | get_idx_of_lane_in_route_element, 20 | get_lane_change_direction, 21 | get_preceding_lane_element_idx, 22 | get_regulatory_elements, 23 | get_regulatory_elements_of_lane_element, 24 | get_regulatory_elements_of_suggested_lane, 25 | get_regulatory_element_of_lane_element, 26 | get_remaining_route_elements, 27 | get_route_element_closest_to_s, 28 | get_route_element_idx_closest_to_s, 29 | get_traveled_route_elements, 30 | get_width_of_current_suggested_lane_element, 31 | get_width_of_lane_element, 32 | get_width_of_suggested_lane_element, 33 | has_adjacent_lane, 34 | has_left_adjacent_lane, 35 | has_right_adjacent_lane, 36 | ) 37 | from route_planning_msgs_utils.route_setters import ( 38 | set_left_boundary_of_lane_element_from_point, 39 | set_right_boundary_of_lane_element_from_point, 40 | ) 41 | 42 | EPS = 1e-12 43 | 44 | 45 | def test_setters(): 46 | lane_element = LaneElement() 47 | 48 | left_boundary = Point() 49 | left_boundary.x = 1.0 50 | left_boundary.y = 2.0 51 | right_boundary = Point() 52 | right_boundary.x = 3.0 53 | right_boundary.y = 4.0 54 | set_left_boundary_of_lane_element_from_point(lane_element, left_boundary) 55 | set_right_boundary_of_lane_element_from_point(lane_element, right_boundary) 56 | 57 | assert lane_element.left_boundary.point.x == 1.0 58 | assert lane_element.left_boundary.point.y == 2.0 59 | assert lane_element.left_boundary.type == LaneBoundary.TYPE_UNKNOWN 60 | assert lane_element.right_boundary.point.x == 3.0 61 | assert lane_element.right_boundary.point.y == 4.0 62 | assert lane_element.right_boundary.type == LaneBoundary.TYPE_UNKNOWN 63 | 64 | 65 | def test_getters(): 66 | route = Route() 67 | route_element = RouteElement() 68 | lane_element = LaneElement() 69 | left_boundary = LaneBoundary() 70 | right_boundary = LaneBoundary() 71 | 72 | left_boundary.point.x = 1.0 73 | left_boundary.point.y = 2.0 74 | lane_element.left_boundary = left_boundary 75 | 76 | right_boundary.point.x = 3.0 77 | right_boundary.point.y = 4.0 78 | lane_element.right_boundary = right_boundary 79 | 80 | route_element.suggested_lane_idx = 0 81 | route_element.lane_elements.append(lane_element) 82 | route.route_elements.append(route_element) 83 | 84 | assert math.isclose(get_width_of_lane_element(lane_element), math.sqrt(8), rel_tol=EPS) 85 | assert math.isclose(get_width_of_suggested_lane_element(route_element), 2.8284271247461903, rel_tol=EPS) 86 | assert math.isclose(get_width_of_current_suggested_lane_element(route), 2.8284271247461903, rel_tol=EPS) 87 | 88 | 89 | def _make_lane(left_xy, right_xy): 90 | lane = LaneElement() 91 | lane.left_boundary.point.x, lane.left_boundary.point.y = left_xy 92 | lane.right_boundary.point.x, lane.right_boundary.point.y = right_xy 93 | return lane 94 | 95 | 96 | def test_route_helpers(): 97 | route = Route() 98 | route.starting_route_element_idx = 0 99 | route.current_route_element_idx = 1 100 | route.destination_route_element_idx = 2 101 | 102 | route_element_0 = RouteElement() 103 | route_element_0.s = 0.0 104 | lane0 = _make_lane((0.0, 1.0), (0.0, 0.0)) 105 | lane0.has_following_lane_idx = True 106 | lane0.following_lane_idx = 0 107 | route_element_0.lane_elements.append(lane0) 108 | route_element_0.suggested_lane_idx = 0 109 | 110 | route_element_1 = RouteElement() 111 | route_element_1.s = 10.0 112 | route_element_1.suggested_lane_idx = 0 113 | route_element_1.will_change_suggested_lane = True 114 | lane1_a = _make_lane((1.0, 1.0), (1.0, 0.0)) 115 | lane1_a.has_following_lane_idx = True 116 | lane1_a.following_lane_idx = 0 117 | lane1_b = _make_lane((1.5, 1.0), (1.5, 0.0)) 118 | lane1_b.has_following_lane_idx = True 119 | lane1_b.following_lane_idx = 1 120 | lane1_b.regulatory_element_idcs.append(0) 121 | reg = RegulatoryElement() 122 | route_element_1.regulatory_elements.append(reg) 123 | route_element_1.lane_elements.extend([lane1_a, lane1_b]) 124 | 125 | route_element_2 = RouteElement() 126 | route_element_2.s = 20.0 127 | route_element_2.suggested_lane_idx = 1 128 | lane2_a = _make_lane((2.0, 1.0), (2.0, 0.0)) 129 | lane2_b = _make_lane((2.5, 1.0), (2.5, 0.0)) 130 | route_element_2.lane_elements.extend([lane2_a, lane2_b]) 131 | 132 | route.route_elements.extend([route_element_0, route_element_1, route_element_2]) 133 | 134 | traveled = get_traveled_route_elements(route) 135 | assert traveled == [route_element_0] 136 | 137 | remaining = get_remaining_route_elements(route) 138 | assert remaining == [route_element_1, route_element_2] 139 | 140 | assert get_route_element_idx_closest_to_s(route, 11.0) == 1 141 | assert get_route_element_closest_to_s(route, 11.0) is route_element_1 142 | 143 | current_lane = get_current_suggested_lane_element(route) 144 | assert current_lane is lane1_a 145 | 146 | assert get_idx_of_lane_in_route_element(lane1_b, route_element_1) == 1 147 | 148 | following_idx = get_following_lane_element_idx(lane0, route_element_1) 149 | assert following_idx == 0 150 | assert get_following_lane_element(lane0, route_element_1) is lane1_a 151 | 152 | preceding_idx = get_preceding_lane_element_idx(1, route_element_1) 153 | assert preceding_idx == 1 154 | assert get_following_lane_element_idx(lane1_b, route_element_2) == 1 155 | 156 | regulatory_elements = get_regulatory_elements(route_element_1) 157 | assert regulatory_elements == [reg] 158 | 159 | lane_reg_elements = get_regulatory_elements_of_lane_element(route_element_1, 1) 160 | assert lane_reg_elements == [reg] 161 | assert get_regulatory_element_of_lane_element(lane1_b, route_element_1.regulatory_elements) == [reg] 162 | 163 | suggested_reg_elements = get_regulatory_elements_of_suggested_lane(route_element_1) 164 | assert suggested_reg_elements == [] 165 | 166 | assert has_adjacent_lane(route_element_1, 0, 1) 167 | assert has_right_adjacent_lane(route_element_1, 0) 168 | assert has_left_adjacent_lane(route_element_1, 1) 169 | assert get_adjacent_lane(route_element_1, 0, 1) is lane1_b 170 | 171 | lane_change_direction = get_lane_change_direction(route_element_1, route_element_2) 172 | assert lane_change_direction == 1 173 | 174 | 175 | if __name__ == "__main__": 176 | pytest.main() 177 | -------------------------------------------------------------------------------- /trajectory_planning_msgs_utils/include/trajectory_planning_msgs_utils/impl/state_index.h: -------------------------------------------------------------------------------- 1 | /** ============================================================================ 2 | MIT License 3 | 4 | Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | ============================================================================= */ 24 | 25 | #pragma once 26 | 27 | namespace trajectory_planning_msgs { 28 | 29 | namespace trajectory_access { 30 | 31 | const std::string kExceptionUnknownStateEntry = 32 | "Trajectory type with the following ID does not support requested entry: "; 33 | 34 | inline int indexT(const unsigned char& type_id) { 35 | switch (type_id) { 36 | case DRIVABLE::TYPE_ID: 37 | return DRIVABLE::T; 38 | case DRIVABLERWS::TYPE_ID: 39 | return DRIVABLERWS::T; 40 | case REFERENCE::TYPE_ID: 41 | return REFERENCE::T; 42 | default: 43 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "t"); 44 | } 45 | } 46 | 47 | inline int indexX(const unsigned char& type_id) { 48 | switch (type_id) { 49 | case DRIVABLE::TYPE_ID: 50 | return DRIVABLE::X; 51 | case DRIVABLERWS::TYPE_ID: 52 | return DRIVABLERWS::X; 53 | case REFERENCE::TYPE_ID: 54 | return REFERENCE::X; 55 | default: 56 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "x"); 57 | } 58 | } 59 | 60 | inline int indexY(const unsigned char& type_id) { 61 | switch (type_id) { 62 | case DRIVABLE::TYPE_ID: 63 | return DRIVABLE::Y; 64 | case DRIVABLERWS::TYPE_ID: 65 | return DRIVABLERWS::Y; 66 | case REFERENCE::TYPE_ID: 67 | return REFERENCE::Y; 68 | default: 69 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "y"); 70 | } 71 | } 72 | 73 | inline int indexV(const unsigned char& type_id) { 74 | switch (type_id) { 75 | case DRIVABLE::TYPE_ID: 76 | return DRIVABLE::V; 77 | case DRIVABLERWS::TYPE_ID: 78 | return DRIVABLERWS::V; 79 | case REFERENCE::TYPE_ID: 80 | return REFERENCE::V; 81 | default: 82 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "v"); 83 | } 84 | } 85 | 86 | inline int indexTheta(const unsigned char& type_id) { 87 | switch (type_id) { 88 | case DRIVABLE::TYPE_ID: 89 | return DRIVABLE::THETA; 90 | case DRIVABLERWS::TYPE_ID: 91 | return DRIVABLERWS::THETA; 92 | default: 93 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "theta"); 94 | } 95 | } 96 | 97 | inline int indexA(const unsigned char& type_id) { 98 | switch (type_id) { 99 | case DRIVABLE::TYPE_ID: 100 | return DRIVABLE::A; 101 | case DRIVABLERWS::TYPE_ID: 102 | return DRIVABLERWS::A; 103 | default: 104 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "a"); 105 | } 106 | } 107 | 108 | inline int indexBeta(const unsigned char& type_id) { 109 | switch (type_id) { 110 | case DRIVABLERWS::TYPE_ID: 111 | return DRIVABLERWS::BETA; 112 | default: 113 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "beta"); 114 | } 115 | } 116 | 117 | inline int indexDeltaFront(const unsigned char& type_id) { 118 | switch (type_id) { 119 | case DRIVABLERWS::TYPE_ID: 120 | return DRIVABLERWS::DELTAFRONT; 121 | default: 122 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "delta_front"); 123 | } 124 | } 125 | 126 | inline int indexDeltaRear(const unsigned char& type_id) { 127 | switch (type_id) { 128 | case DRIVABLERWS::TYPE_ID: 129 | return DRIVABLERWS::DELTAREAR; 130 | default: 131 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "delta_rear"); 132 | } 133 | } 134 | 135 | inline int indexDeltaAck(const unsigned char& type_id) { 136 | switch (type_id) { 137 | case DRIVABLE::TYPE_ID: 138 | return DRIVABLE::DELTA; 139 | default: 140 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "delta"); 141 | } 142 | } 143 | 144 | inline int indexS(const unsigned char& type_id) { 145 | switch (type_id) { 146 | case DRIVABLE::TYPE_ID: 147 | return DRIVABLE::S; 148 | case DRIVABLERWS::TYPE_ID: 149 | return DRIVABLERWS::S; 150 | default: 151 | throw std::invalid_argument(kExceptionUnknownStateEntry + std::to_string(type_id) + ", " + "s"); 152 | } 153 | } 154 | 155 | inline bool hasT(const unsigned char& type_id) { 156 | switch (type_id) { 157 | case DRIVABLE::TYPE_ID: 158 | return true; 159 | case DRIVABLERWS::TYPE_ID: 160 | return true; 161 | case REFERENCE::TYPE_ID: 162 | return true; 163 | default: 164 | return false; 165 | } 166 | } 167 | 168 | inline bool hasX(const unsigned char& type_id) { 169 | switch (type_id) { 170 | case DRIVABLE::TYPE_ID: 171 | return true; 172 | case DRIVABLERWS::TYPE_ID: 173 | return true; 174 | case REFERENCE::TYPE_ID: 175 | return true; 176 | default: 177 | return false; 178 | } 179 | } 180 | 181 | inline bool hasY(const unsigned char& type_id) { 182 | switch (type_id) { 183 | case DRIVABLE::TYPE_ID: 184 | return true; 185 | case DRIVABLERWS::TYPE_ID: 186 | return true; 187 | case REFERENCE::TYPE_ID: 188 | return true; 189 | default: 190 | return false; 191 | } 192 | } 193 | 194 | inline bool hasV(const unsigned char& type_id) { 195 | switch (type_id) { 196 | case DRIVABLE::TYPE_ID: 197 | return true; 198 | case DRIVABLERWS::TYPE_ID: 199 | return true; 200 | case REFERENCE::TYPE_ID: 201 | return true; 202 | default: 203 | return false; 204 | } 205 | } 206 | 207 | inline bool hasTheta(const unsigned char& type_id) { 208 | switch (type_id) { 209 | case DRIVABLE::TYPE_ID: 210 | return true; 211 | case DRIVABLERWS::TYPE_ID: 212 | return true; 213 | case REFERENCE::TYPE_ID: 214 | return false; 215 | default: 216 | return false; 217 | } 218 | } 219 | 220 | inline bool hasA(const unsigned char& type_id) { 221 | switch (type_id) { 222 | case DRIVABLE::TYPE_ID: 223 | return true; 224 | case DRIVABLERWS::TYPE_ID: 225 | return true; 226 | case REFERENCE::TYPE_ID: 227 | return false; 228 | default: 229 | return false; 230 | } 231 | } 232 | 233 | inline bool hasBeta(const unsigned char& type_id) { 234 | switch (type_id) { 235 | case DRIVABLE::TYPE_ID: 236 | return false; 237 | case DRIVABLERWS::TYPE_ID: 238 | return true; 239 | case REFERENCE::TYPE_ID: 240 | return false; 241 | default: 242 | return false; 243 | } 244 | } 245 | 246 | inline bool hasDeltaFront(const unsigned char& type_id) { 247 | switch (type_id) { 248 | case DRIVABLE::TYPE_ID: 249 | return false; 250 | case DRIVABLERWS::TYPE_ID: 251 | return true; 252 | case REFERENCE::TYPE_ID: 253 | return false; 254 | default: 255 | return false; 256 | } 257 | } 258 | 259 | inline bool hasDeltaRear(const unsigned char& type_id) { 260 | switch (type_id) { 261 | case DRIVABLE::TYPE_ID: 262 | return false; 263 | case DRIVABLERWS::TYPE_ID: 264 | return true; 265 | case REFERENCE::TYPE_ID: 266 | return false; 267 | default: 268 | return false; 269 | } 270 | } 271 | 272 | inline bool hasDeltaAck(const unsigned char& type_id) { 273 | switch (type_id) { 274 | case DRIVABLE::TYPE_ID: 275 | return true; 276 | case DRIVABLERWS::TYPE_ID: 277 | return false; 278 | case REFERENCE::TYPE_ID: 279 | return false; 280 | default: 281 | return false; 282 | } 283 | } 284 | 285 | inline bool hasS(const unsigned char& type_id) { 286 | switch (type_id) { 287 | case DRIVABLE::TYPE_ID: 288 | return true; 289 | case DRIVABLERWS::TYPE_ID: 290 | return true; 291 | case REFERENCE::TYPE_ID: 292 | return false; 293 | default: 294 | return false; 295 | } 296 | } 297 | 298 | } // namespace trajectory_access 299 | 300 | } // namespace trajectory_planning_msgs -------------------------------------------------------------------------------- /route_planning_msgs_utils/src/route_planning_msgs_utils/route_getters.py: -------------------------------------------------------------------------------- 1 | # ============================================================================ 2 | # MIT License 3 | # 4 | # Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # ============================================================================ 24 | 25 | import math 26 | from typing import List, Optional 27 | 28 | from route_planning_msgs.msg import ( 29 | LaneElement, 30 | RegulatoryElement, 31 | Route, 32 | RouteElement, 33 | ) 34 | 35 | 36 | def get_traveled_route_elements(route: Route, incl_undershoot: bool = False) -> List[RouteElement]: 37 | n_route_elements = len(route.route_elements) 38 | start_idx = 0 if incl_undershoot else route.starting_route_element_idx 39 | end_idx = route.current_route_element_idx 40 | start_idx = min(start_idx, n_route_elements) 41 | end_idx = min(end_idx, n_route_elements) 42 | if start_idx >= end_idx: 43 | return [] 44 | return list(route.route_elements[start_idx:end_idx]) 45 | 46 | 47 | def get_remaining_route_elements(route: Route, incl_overshoot: bool = False) -> List[RouteElement]: 48 | n_route_elements = len(route.route_elements) 49 | start_idx = route.current_route_element_idx 50 | end_idx = n_route_elements if incl_overshoot else route.destination_route_element_idx + 1 51 | start_idx = min(start_idx, n_route_elements) 52 | end_idx = min(end_idx, n_route_elements) 53 | if start_idx >= end_idx: 54 | return [] 55 | return list(route.route_elements[start_idx:end_idx]) 56 | 57 | 58 | def get_idx_of_lane_in_route_element(lane_element: LaneElement, route_element: RouteElement) -> int: 59 | for idx, candidate in enumerate(route_element.lane_elements): 60 | if candidate == lane_element or candidate is lane_element: 61 | return idx 62 | raise ValueError("Lane element not found in route element") 63 | 64 | 65 | def get_route_element_idx_closest_to_s(route: Route, s: float) -> int: 66 | n_route_elements = len(route.route_elements) 67 | if n_route_elements < 2: 68 | raise ValueError("Not enough route elements to calculate difference") 69 | 70 | min_difference = 2 * abs(route.route_elements[1].s - route.route_elements[0].s) 71 | closest_idx = min( 72 | range(n_route_elements), 73 | key=lambda idx: abs(route.route_elements[idx].s - s), 74 | ) 75 | 76 | if abs(route.route_elements[closest_idx].s - s) >= min_difference: 77 | raise ValueError("No route element found within acceptable difference") 78 | 79 | return closest_idx 80 | 81 | 82 | def get_route_element_closest_to_s(route: Route, s: float) -> RouteElement: 83 | return route.route_elements[get_route_element_idx_closest_to_s(route, s)] 84 | 85 | 86 | def get_width_of_lane_element(lane_element: LaneElement) -> float: 87 | dx = lane_element.left_boundary.point.x - lane_element.right_boundary.point.x 88 | dy = lane_element.left_boundary.point.y - lane_element.right_boundary.point.y 89 | return math.sqrt(dx * dx + dy * dy) 90 | 91 | 92 | def get_suggested_lane_element(route_element: RouteElement) -> LaneElement: 93 | lane_idx = route_element.suggested_lane_idx 94 | if lane_idx >= len(route_element.lane_elements): 95 | raise IndexError(f"Suggested lane index out of range: {lane_idx}") 96 | return route_element.lane_elements[lane_idx] 97 | 98 | 99 | def get_current_suggested_lane_element(route: Route) -> LaneElement: 100 | idx = route.current_route_element_idx 101 | if idx >= len(route.route_elements): 102 | raise IndexError(f"Current route element index out of range: {idx}") 103 | return get_suggested_lane_element(route.route_elements[idx]) 104 | 105 | 106 | def get_following_lane_element_idx( 107 | lane_element: LaneElement, following_route_element: RouteElement 108 | ) -> Optional[int]: 109 | if ( 110 | not lane_element.has_following_lane_idx 111 | or lane_element.following_lane_idx >= len(following_route_element.lane_elements) 112 | ): 113 | return None 114 | return lane_element.following_lane_idx 115 | 116 | 117 | def get_following_lane_element( 118 | lane_element: LaneElement, following_route_element: RouteElement 119 | ) -> Optional[LaneElement]: 120 | idx = get_following_lane_element_idx(lane_element, following_route_element) 121 | if idx is None: 122 | return None 123 | return following_route_element.lane_elements[idx] 124 | 125 | 126 | def get_preceding_lane_element( 127 | lane_element_idx: int, preceding_route_element: RouteElement 128 | ) -> Optional[LaneElement]: 129 | for preceding_lane in preceding_route_element.lane_elements: 130 | if ( 131 | preceding_lane.has_following_lane_idx 132 | and preceding_lane.following_lane_idx == lane_element_idx 133 | ): 134 | return preceding_lane 135 | return None 136 | 137 | 138 | def get_preceding_lane_element_idx( 139 | lane_element_idx: int, preceding_route_element: RouteElement 140 | ) -> Optional[int]: 141 | preceding_lane = get_preceding_lane_element(lane_element_idx, preceding_route_element) 142 | if preceding_lane is None: 143 | return None 144 | return get_idx_of_lane_in_route_element(preceding_lane, preceding_route_element) 145 | 146 | 147 | def get_width_of_suggested_lane_element(route_element: RouteElement) -> float: 148 | return get_width_of_lane_element(get_suggested_lane_element(route_element)) 149 | 150 | 151 | def get_width_of_current_suggested_lane_element(route: Route) -> float: 152 | return get_width_of_lane_element(get_current_suggested_lane_element(route)) 153 | 154 | 155 | def get_regulatory_elements(route_element: RouteElement) -> List[RegulatoryElement]: 156 | return list(route_element.regulatory_elements) 157 | 158 | 159 | def get_regulatory_elements_for_lane_element( 160 | lane_element: LaneElement, possible_regulatory_elements: List[RegulatoryElement] 161 | ) -> List[RegulatoryElement]: 162 | regulatory_elements: List[RegulatoryElement] = [] 163 | for regulatory_element_idx in lane_element.regulatory_element_idcs: 164 | if regulatory_element_idx >= len(possible_regulatory_elements): 165 | raise ValueError(f"Regulatory element index out of range: {regulatory_element_idx}") 166 | regulatory_elements.append(possible_regulatory_elements[regulatory_element_idx]) 167 | return regulatory_elements 168 | 169 | 170 | def get_regulatory_elements_of_lane_element( 171 | route_element: RouteElement, lane_idx: int 172 | ) -> List[RegulatoryElement]: 173 | if lane_idx >= len(route_element.lane_elements): 174 | raise IndexError(f"Lane index out of range: {lane_idx}") 175 | return get_regulatory_elements_for_lane_element( 176 | route_element.lane_elements[lane_idx], route_element.regulatory_elements 177 | ) 178 | 179 | 180 | def get_regulatory_elements_of_suggested_lane(route_element: RouteElement) -> List[RegulatoryElement]: 181 | return get_regulatory_elements_of_lane_element(route_element, route_element.suggested_lane_idx) 182 | 183 | 184 | def has_adjacent_lane(route_element: RouteElement, lane_idx: int, lane_diff_idx: int) -> bool: 185 | if lane_idx >= len(route_element.lane_elements): 186 | raise IndexError(f"Lane index out of range: {lane_idx}") 187 | adjacent_lane_idx = lane_idx + lane_diff_idx 188 | return 0 <= adjacent_lane_idx < len(route_element.lane_elements) 189 | 190 | 191 | def has_right_adjacent_lane(route_element: RouteElement, lane_idx: int) -> bool: 192 | return has_adjacent_lane(route_element, lane_idx, 1) 193 | 194 | 195 | def has_left_adjacent_lane(route_element: RouteElement, lane_idx: int) -> bool: 196 | return has_adjacent_lane(route_element, lane_idx, -1) 197 | 198 | 199 | def get_adjacent_lane(route_element: RouteElement, lane_idx: int, lane_diff_idx: int) -> LaneElement: 200 | if not has_adjacent_lane(route_element, lane_idx, lane_diff_idx): 201 | raise ValueError(f"No adjacent lane found for lane index: {lane_idx}") 202 | return route_element.lane_elements[lane_idx + lane_diff_idx] 203 | 204 | 205 | def get_lane_change_direction(route_element: RouteElement, following_route_element: RouteElement) -> int: 206 | if not route_element.will_change_suggested_lane: 207 | raise ValueError("Route element does not indicate a change of suggested lane") 208 | 209 | current_lane_idx = route_element.suggested_lane_idx 210 | preceding_idx = get_preceding_lane_element_idx( 211 | following_route_element.suggested_lane_idx, route_element 212 | ) 213 | if preceding_idx is None: 214 | raise ValueError( 215 | f"No preceding lane element found for route element with suggested lane {route_element.suggested_lane_idx}" 216 | ) 217 | return int(preceding_idx) - int(current_lane_idx) 218 | 219 | 220 | # Backwards compatibility alias (deprecated name) 221 | def get_regulatory_element_of_lane_element( 222 | lane_element: LaneElement, possible_regulatory_elements: List[RegulatoryElement] 223 | ) -> List[RegulatoryElement]: 224 | return get_regulatory_elements_for_lane_element(lane_element, possible_regulatory_elements) 225 | --------------------------------------------------------------------------------