├── .gitignore ├── LICENSE ├── README.md ├── car_demo ├── README.md ├── car_demo │ ├── CMakeLists.txt │ ├── config │ │ ├── prius_path_tracker_ros_parameters.yaml │ │ ├── prius_pid_parameters.yaml │ │ └── reeds_shepp_planner_ros.yaml │ ├── include │ │ └── car_demo │ │ │ ├── PIDController.hpp │ │ │ ├── PriusControllerRos.hpp │ │ │ └── PriusProgressValidator.hpp │ ├── launch │ │ ├── demo_autonomous.launch │ │ └── demo_joystick.launch │ ├── models │ │ ├── cloverleaf_interchange │ │ │ ├── materials │ │ │ │ └── textures │ │ │ │ │ ├── arrow.png │ │ │ │ │ ├── concrete.png │ │ │ │ │ ├── cross.png │ │ │ │ │ ├── grass.png │ │ │ │ │ └── road.png │ │ │ ├── meshes │ │ │ │ ├── cloverleaf.mtl │ │ │ │ └── cloverleaf.obj │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── construction_cone │ │ │ ├── materials │ │ │ │ └── textures │ │ │ │ │ └── Construction_Cone_Diffuse.png │ │ │ ├── meshes │ │ │ │ └── construction_cone.dae │ │ │ ├── model-1_3.sdf │ │ │ ├── model-1_4.sdf │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── dumpster │ │ │ ├── materials │ │ │ │ ├── scripts │ │ │ │ │ └── dumpster.material │ │ │ │ └── textures │ │ │ │ │ ├── Dumpster_Diffuse.png │ │ │ │ │ └── Dumpster_Spec.png │ │ │ ├── meshes │ │ │ │ └── dumpster.dae │ │ │ ├── model-1_3.sdf │ │ │ ├── model-1_4.sdf │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── gas_station │ │ │ ├── materials │ │ │ │ ├── scripts │ │ │ │ │ └── gas_station.material │ │ │ │ └── textures │ │ │ │ │ ├── GasStation_Diffuse.png │ │ │ │ │ ├── GasStation_Normal.png │ │ │ │ │ └── GasStation_Spec.png │ │ │ ├── meshes │ │ │ │ └── gas_station.dae │ │ │ ├── model-1_3.sdf │ │ │ ├── model-1_4.sdf │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── grey_wall │ │ │ ├── materials │ │ │ │ ├── scripts │ │ │ │ │ └── grey_wall.material │ │ │ │ └── textures │ │ │ │ │ └── grey_wall.png │ │ │ ├── model-1_3.sdf │ │ │ ├── model-1_4.sdf │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── house_1 │ │ │ ├── materials │ │ │ │ ├── scripts │ │ │ │ │ └── house_1.material │ │ │ │ └── textures │ │ │ │ │ ├── House_1_Diffuse.png │ │ │ │ │ ├── House_1_Normal.png │ │ │ │ │ └── House_1_Spec.png │ │ │ ├── meshes │ │ │ │ └── house_1.dae │ │ │ ├── model-1_3.sdf │ │ │ ├── model-1_4.sdf │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── house_2 │ │ │ ├── materials │ │ │ │ ├── scripts │ │ │ │ │ └── house_2.material │ │ │ │ └── textures │ │ │ │ │ └── House_2_Diffuse.png │ │ │ ├── meshes │ │ │ │ └── house_2.dae │ │ │ ├── model-1_3.sdf │ │ │ ├── model-1_4.sdf │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── house_3 │ │ │ ├── materials │ │ │ │ ├── scripts │ │ │ │ │ └── house_3.material │ │ │ │ └── textures │ │ │ │ │ └── House_3_Diffuse.png │ │ │ ├── meshes │ │ │ │ └── house_3.dae │ │ │ ├── model-1_3.sdf │ │ │ ├── model-1_4.sdf │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── jersey_barrier │ │ │ ├── materials │ │ │ │ └── textures │ │ │ │ │ ├── jersey_barrier.png │ │ │ │ │ ├── jersey_barrier_diffuse.png │ │ │ │ │ └── jersey_barrier_spec.png │ │ │ ├── meshes │ │ │ │ └── jersey_barrier.dae │ │ │ ├── model-1_4.sdf │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── mcity │ │ │ ├── materials │ │ │ │ └── textures │ │ │ │ │ ├── Asphalt.png │ │ │ │ │ ├── Brick.png │ │ │ │ │ ├── Buildings.png │ │ │ │ │ ├── Colors.png │ │ │ │ │ ├── Concrete_Ground.png │ │ │ │ │ ├── Crosswalk.png │ │ │ │ │ ├── Curb.png │ │ │ │ │ ├── Grass.png │ │ │ │ │ ├── Gravel.png │ │ │ │ │ ├── GuardRail.png │ │ │ │ │ ├── OverheadSignBase.png │ │ │ │ │ ├── OverheadSigns.png │ │ │ │ │ ├── RailroadGate.png │ │ │ │ │ ├── Road_Blank.png │ │ │ │ │ ├── Road_Blank_Light.png │ │ │ │ │ ├── Road_DoubleYellowLine.png │ │ │ │ │ ├── Road_DoubleYellowLine_Light.png │ │ │ │ │ ├── Road_SingleWhiteLine.png │ │ │ │ │ ├── StreetLamp_Short.png │ │ │ │ │ ├── StreetLamp_Tall.png │ │ │ │ │ ├── TrafficBarrel.png │ │ │ │ │ └── Tunnel.png │ │ │ ├── meshes │ │ │ │ ├── mcity.mtl │ │ │ │ └── mcity.obj │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── powerplant │ │ │ ├── materials │ │ │ │ └── textures │ │ │ │ │ ├── __auto_.png │ │ │ │ │ ├── __auto_1.png │ │ │ │ │ ├── __auto_10.png │ │ │ │ │ ├── __auto_11.png │ │ │ │ │ ├── __auto_12.png │ │ │ │ │ ├── __auto_13.png │ │ │ │ │ ├── __auto_17.png │ │ │ │ │ ├── __auto_19.png │ │ │ │ │ ├── __auto_2.png │ │ │ │ │ ├── __auto_20.png │ │ │ │ │ ├── __auto_21.png │ │ │ │ │ ├── __auto_22.png │ │ │ │ │ ├── __auto_23.png │ │ │ │ │ ├── __auto_25.png │ │ │ │ │ ├── __auto_26.png │ │ │ │ │ ├── __auto_27.png │ │ │ │ │ ├── __auto_3.png │ │ │ │ │ ├── __auto_32.png │ │ │ │ │ ├── __auto_33.png │ │ │ │ │ ├── __auto_34.png │ │ │ │ │ ├── __auto_35.png │ │ │ │ │ ├── __auto_36.png │ │ │ │ │ ├── __auto_37.png │ │ │ │ │ ├── __auto_38.png │ │ │ │ │ ├── __auto_6.png │ │ │ │ │ ├── __auto_7.png │ │ │ │ │ ├── __auto_8.png │ │ │ │ │ └── __auto_9.png │ │ │ ├── meshes │ │ │ │ └── powerplant.dae │ │ │ ├── model-1_2.sdf │ │ │ ├── model-1_3.sdf │ │ │ ├── model-1_4.sdf │ │ │ ├── model.config │ │ │ └── model.sdf │ │ └── speed_limit_sign │ │ │ ├── materials │ │ │ └── textures │ │ │ │ ├── Speed_100.png │ │ │ │ ├── Speed_110.png │ │ │ │ ├── Speed_120.png │ │ │ │ ├── Speed_130.png │ │ │ │ ├── Speed_16.png │ │ │ │ ├── Speed_30.png │ │ │ │ ├── Speed_40.png │ │ │ │ ├── Speed_50.png │ │ │ │ ├── Speed_60.png │ │ │ │ ├── Speed_70.png │ │ │ │ ├── Speed_80.png │ │ │ │ ├── Speed_90.png │ │ │ │ ├── Speed_Spec.png │ │ │ │ ├── StopSign_Diffuse.png │ │ │ │ └── StopSign_Spec.png │ │ │ ├── meshes │ │ │ └── speed_limit_sign.dae │ │ │ ├── model-1_3.sdf │ │ │ ├── model-1_4.sdf │ │ │ ├── model.config │ │ │ └── model.sdf │ ├── nodes │ │ └── joystick_translator │ ├── package.xml │ ├── plugins │ │ ├── PriusHybridPlugin.cc │ │ └── PriusHybridPlugin.hh │ ├── rviz │ │ ├── demo_autonomous.rviz │ │ └── demo_joystick.rviz │ ├── src │ │ ├── PIDController.cpp │ │ ├── PriusControllerRos.cpp │ │ └── prius_controller_node.cpp │ └── worlds │ │ ├── empty.world │ │ └── mcity.world ├── doc │ ├── car.gif │ ├── m545.gif │ └── spacebok_nav.gif ├── prius_description │ ├── CMakeLists.txt │ ├── meshes │ │ ├── Hybrid.png │ │ ├── Hybrid_Interior.png │ │ ├── Wheels3.png │ │ ├── hybrid_body.mtl │ │ ├── hybrid_body.obj │ │ ├── steering_wheel.mtl │ │ ├── steering_wheel.obj │ │ ├── wheel.mtl │ │ └── wheel.obj │ ├── package.xml │ └── urdf │ │ └── prius.urdf └── prius_msgs │ ├── CMakeLists.txt │ ├── include │ └── prius_msgs │ │ └── PriusControl.hpp │ ├── msg │ └── Control.msg │ └── package.xml ├── jenkins-pipeline ├── pure_pursuit_core ├── CMakeLists.txt ├── README.md ├── doc │ ├── controller_convention.pdf │ ├── controller_convention.png │ ├── controller_convention_back.pdf │ ├── controller_convention_back.png │ ├── path_conventions.pdf │ └── path_conventions.png ├── include │ └── pure_pursuit_core │ │ ├── Path.hpp │ │ ├── common.hpp │ │ ├── heading_control │ │ ├── AckermannSteeringController.hpp │ │ ├── HeadingController.hpp │ │ └── MobileBaseHeadingController.hpp │ │ ├── math.hpp │ │ ├── path_tracking │ │ ├── PathPreprocessor.hpp │ │ ├── PathTracker.hpp │ │ ├── ProgressValidator.hpp │ │ ├── SimplePathTracker.hpp │ │ └── Stopwatch.hpp │ │ ├── typedefs.hpp │ │ └── velocity_control │ │ ├── AdaptiveVelocityController.hpp │ │ ├── ConstantVelocityController.hpp │ │ └── LongitudinalVelocityController.hpp ├── package.xml ├── src │ ├── AckermannSteeringController.cpp │ ├── AdaptiveVelocityController.cpp │ ├── ConstantVelocityController.cpp │ ├── HeadingController.cpp │ ├── LongitudinalVelocityController.cpp │ ├── MobileBaseHeadingController.cpp │ ├── PathPreprocessor.cpp │ ├── PathTracker.cpp │ ├── ProgressValidator.cpp │ ├── SimplePathTracker.cpp │ ├── Stopwatch.cpp │ ├── common.cpp │ └── math.cpp └── test │ ├── AckermannSteeringControllerTest.cpp │ ├── GeometryTest.cpp │ ├── PathTest.cpp │ ├── test_helpers.cpp │ ├── test_helpers.hpp │ └── test_pure_pursuit.cpp ├── pure_pursuit_ros ├── CMakeLists.txt ├── README.md ├── config │ ├── PurePursuit.cfg │ ├── PurePursuitAdaptiveVel.cfg │ └── example.yaml ├── include │ └── pure_pursuit_ros │ │ ├── AckermannSteeringControllerRos.hpp │ │ ├── AdaptiveVelocityControllerRos.hpp │ │ ├── SimplePathTrackerRos.hpp │ │ └── loaders.hpp ├── launch │ └── standalone_example.launch ├── package.xml └── src │ ├── AckermannSteeringControllerRos.cpp │ ├── AdaptiveVelocityControllerRos.cpp │ ├── SimplePathTrackerRos.cpp │ ├── dd_standalone_example.cpp │ └── loaders.cpp ├── se2_approach_pose_planning ├── approach_pose_planner │ ├── CMakeLists.txt │ ├── README.md │ ├── doc │ │ ├── Approach_pose_conventions.pdf │ │ ├── Approach_pose_conventions.png │ │ ├── all_approach_poses.png │ │ ├── approach_planning_footprint.png │ │ ├── both_criterions.png │ │ ├── driving_footprint.png │ │ ├── feasiblity_checking_convention.pdf │ │ ├── feasiblity_checking_convention.png │ │ └── reachability_criterion_only.png │ ├── include │ │ └── approach_pose_planner │ │ │ ├── ApproachPoseHeuristic.hpp │ │ │ ├── ApproachPosePlanner.hpp │ │ │ ├── ApproachStateValidator.hpp │ │ │ ├── LineOfSightApproachStateValidator.hpp │ │ │ ├── Parameters.hpp │ │ │ └── common.hpp │ ├── package.xml │ ├── src │ │ ├── ApproachPoseHeuristic.cpp │ │ ├── ApproachPosePlanner.cpp │ │ └── LineOfSightApproachStateValidator.cpp │ └── test │ │ ├── ApproachPosePlannerTest.cpp │ │ └── test_planning.cpp ├── approach_pose_planner_msgs │ ├── CMakeLists.txt │ ├── README.md │ ├── msg │ │ └── ApproachPoseRequestMsg.msg │ ├── package.xml │ └── srv │ │ └── RequestApproachPoseSrv.srv └── approach_pose_planner_ros │ ├── CMakeLists.txt │ ├── README.md │ ├── config │ ├── approach_pose_planner.yaml │ └── grid_map_generation.yaml │ ├── doc │ └── rviz_vis.png │ ├── include │ └── approach_pose_planner_ros │ │ ├── ApproachPosePlannerRos.hpp │ │ ├── Parameters.hpp │ │ ├── creators.hpp │ │ └── loaders.hpp │ ├── launch │ ├── approach_pose_planner_ros.launch │ └── generate_grid_map.launch │ ├── package.xml │ ├── rviz │ ├── default.rviz │ └── generate_grid_map.rviz │ ├── scripts │ └── generate_grid_map_node.py │ └── src │ ├── ApproachPosePlannerRos.cpp │ ├── approach_pose_planner_node.cpp │ ├── creators.cpp │ └── loaders.cpp ├── se2_grid_map_generator ├── CMakeLists.txt ├── README.md ├── config │ ├── default.yaml │ └── rviz │ │ └── default.rviz ├── data │ └── .gitkeep ├── include │ └── se2_grid_map_generator │ │ └── GridMapGenerator.hpp ├── launch │ └── se2_grid_map_generator.launch ├── package.xml └── src │ ├── GridMapGenerator.cpp │ └── se2_grid_map_generator_node.cpp ├── se2_grid_map_generator_msgs ├── CMakeLists.txt ├── msg │ ├── Circle2D.msg │ ├── CircularObstacle.msg │ ├── Polygon2D.msg │ ├── PolygonObstacle.msg │ └── Position2D.msg ├── package.xml └── srv │ ├── AddCircularObstacle.srv │ ├── AddNan.srv │ ├── AddPolygonObstacle.srv │ ├── ResetMap.srv │ ├── SaveMap.srv │ ├── SetUniformValue.srv │ └── UpdateMapPosition.srv ├── se2_navigation_msgs ├── CMakeLists.txt ├── README.md ├── include │ └── se2_navigation_msgs │ │ ├── ControllerCommand.hpp │ │ ├── Path.hpp │ │ └── PathSegment.hpp ├── msg │ ├── ControllerCommandMsg.msg │ ├── PathMsg.msg │ ├── PathRequestMsg.msg │ └── PathSegmentMsg.msg ├── package.xml └── srv │ ├── RequestCurrentStateSrv.srv │ ├── RequestPathSrv.srv │ └── SendControllerCommandSrv.srv ├── se2_planning ├── CMakeLists.txt ├── README.md ├── doc │ ├── collision_footprint_conventions.pdf │ └── collision_footprint_conventions.png ├── include │ └── se2_planning │ │ ├── GridMapLazyStateValidator.hpp │ │ ├── GridMapStateValidator.hpp │ │ ├── HeightMap.hpp │ │ ├── OmplPlanner.hpp │ │ ├── OmplReedsSheppPlanner.hpp │ │ ├── Planner.hpp │ │ ├── State.hpp │ │ ├── StateValidator.hpp │ │ ├── common.hpp │ │ └── ompl_planner_creators.hpp ├── package.xml ├── src │ ├── GridMapLazyStateValidator.cpp │ ├── GridMapStateValidator.cpp │ ├── HeightMap.cpp │ ├── OmplPlanner.cpp │ ├── OmplReedsSheppPlanner.cpp │ ├── State.cpp │ ├── StateValidator.cpp │ ├── ompl_planner_creators.cpp │ └── planning_benchmark.cpp └── test │ ├── GridMapLazyStateValidatorTest.cpp │ ├── GridMapStateValidatorTest.cpp │ ├── OmplReedsSheppPlannerTest.cpp │ ├── ompl_planner_creators_test.cpp │ ├── test_helpers.cpp │ ├── test_helpers.hpp │ └── test_planning.cpp ├── se2_planning_ros ├── CMakeLists.txt ├── README.md ├── config │ └── reeds_shepp_planner_ros_example.yaml ├── doc │ └── rviz_panel.png ├── include │ └── se2_planning_ros │ │ ├── OmplReedsSheppPlannerRos.hpp │ │ ├── PlannerRos.hpp │ │ ├── common.hpp │ │ └── loaders.hpp ├── launch │ └── se2_planner.launch ├── package.xml └── src │ ├── OmplReedsSheppPlannerRos.cpp │ ├── PlannerRos.cpp │ ├── common.cpp │ ├── loaders.cpp │ └── se2_planner_node.cpp ├── se2_planning_rviz ├── CMakeLists.txt ├── README.md ├── doc │ ├── adding_panel.gif │ └── panel.png ├── include │ └── se2_planning_rviz │ │ ├── EditButton.hpp │ │ ├── PlanningInteractiveMarkers.hpp │ │ ├── PlanningPanel.hpp │ │ └── PoseWidget.hpp ├── package.xml ├── plugin_description.xml ├── rviz │ └── default.rviz └── src │ ├── EditButton.cpp │ ├── PlanningInteractiveMarkers.cpp │ ├── PlanningPanel.cpp │ └── PoseWidget.cpp └── se2_visualization_ros ├── CMakeLists.txt ├── README.md ├── include └── se2_visualization_ros │ └── visualization_helpers.hpp ├── package.xml └── src └── visualization_helpers.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | se2_grid_map_generator/data/* 2 | !se2_grid_map_generator/data/.gitkeep 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Robotic Systems Lab - ETH Zürich 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /car_demo/car_demo/config/prius_path_tracker_ros_parameters.yaml: -------------------------------------------------------------------------------- 1 | velocity_control: 2 | constant_velocity_control: 3 | desired_velocity: 1.0 4 | max_rate_of_change: 0.3 5 | adaptive_velocity_control: 6 | nominal_velocity: 1.0 7 | max_rate_of_change: 0.5 8 | distance_when_braking_starts: 5.0 9 | 10 | heading_control: 11 | lookahead_fwd: 4.0 12 | lookahead_bck: 4.0 13 | anchor_dist_fwd: 0.5 14 | anchor_dist_bck: 0.5 15 | dead_zone_width: 0.0 16 | avg_filter_current_sample_weight: 0.9 17 | 18 | 19 | heading_control_ackermann: 20 | wheel_base: 5.0 21 | max_steering_rate_of_change_in_deg_per_sec: 10 22 | max_steering_angle_magnitude_in_deg: 30 23 | 24 | path_tracking: 25 | simple_path_tracker: 26 | waiting_time_between_direction_changes: 2.5 27 | 28 | progress_validation: 29 | goal_distance_tolerance: 0.05 30 | 31 | path_preprocessing: 32 | minimal_segment_length: 1.0 33 | 34 | -------------------------------------------------------------------------------- /car_demo/car_demo/config/prius_pid_parameters.yaml: -------------------------------------------------------------------------------- 1 | PIDparameters: 2 | kp: 0.5 3 | kd: 0.01 4 | ki: 0.01 5 | -------------------------------------------------------------------------------- /car_demo/car_demo/config/reeds_shepp_planner_ros.yaml: -------------------------------------------------------------------------------- 1 | state_space: 2 | x_lower: -100.0 3 | x_upper: 100.0 4 | y_lower: -100.0 5 | y_upper: 100.0 6 | turning_radius: 10.0 7 | 8 | planner_ros: 9 | nav_msgs_path_topic: ompl_rs_planner_ros/nav_msgs_path 10 | planning_service_name: ompl_rs_planner_ros/planning_service 11 | path_msg_topic: ompl_rs_planner_ros/nav_msgs_path 12 | path_frame: map 13 | nav_msg_path_spatial_resolution: 1.5 14 | 15 | planner: 16 | path_spatial_resolution: 0.05 17 | ompl_planner: RRTstar # see ompl_planner_creators.hpp for list of available planners 18 | max_planning_time: 1.0 19 | 20 | ompl_planners: 21 | rrt_star: 22 | planner_range: 50.0 23 | rrt_sharp: 24 | planner_range: 50.0 -------------------------------------------------------------------------------- /car_demo/car_demo/include/car_demo/PriusProgressValidator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PriusProgressValidator.hpp 3 | * 4 | * Created on: Apr 9, 2020 5 | * Author: jelavice 6 | */ 7 | #includ 8 | 9 | namespace car_demo { 10 | class ProgressValidator { 11 | public: 12 | ProgressValidator() = default; 13 | virtual ~ProgressValidator() = default; 14 | 15 | virtual bool isPathSegmentTrackingFinished(const PathSegment& pathSegment, const RobotState& currentState) const; 16 | virtual bool isPathTrackingFinished(const Path& path, const RobotState& currentState, unsigned int currentSegmenet) const; 17 | 18 | void setParameters(const ProgressValidatorParameters& parameters); 19 | 20 | protected: 21 | ProgressValidatorParameters parameters_; 22 | }; 23 | 24 | std::unique_ptr createProgressValidator(const ProgressValidatorParameters& parameters); 25 | 26 | } // namespace car_demo 27 | -------------------------------------------------------------------------------- /car_demo/car_demo/launch/demo_joystick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/cloverleaf_interchange/materials/textures/arrow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/cloverleaf_interchange/materials/textures/arrow.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/cloverleaf_interchange/materials/textures/concrete.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/cloverleaf_interchange/materials/textures/concrete.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/cloverleaf_interchange/materials/textures/cross.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/cloverleaf_interchange/materials/textures/cross.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/cloverleaf_interchange/materials/textures/grass.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/cloverleaf_interchange/materials/textures/grass.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/cloverleaf_interchange/materials/textures/road.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/cloverleaf_interchange/materials/textures/road.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/cloverleaf_interchange/meshes/cloverleaf.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 27.09.2016 21:22:16 3 | 4 | newmtl Road 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka ../materials/textures/road.png 16 | map_Kd ../materials/textures/road.png 17 | 18 | newmtl Grass 19 | Ns 10.0000 20 | Ni 1.5000 21 | d 1.0000 22 | Tr 0.0000 23 | Tf 1.0000 1.0000 1.0000 24 | illum 2 25 | Ka 0.5882 0.5882 0.5882 26 | Kd 0.5882 0.5882 0.5882 27 | Ks 0.0000 0.0000 0.0000 28 | Ke 0.0000 0.0000 0.0000 29 | map_Ka ../materials/textures/grass.png 30 | map_Kd ../materials/textures/grass.png 31 | 32 | newmtl Concrete 33 | Ns 10.0000 34 | Ni 1.5000 35 | d 1.0000 36 | Tr 0.0000 37 | Tf 1.0000 1.0000 1.0000 38 | illum 2 39 | Ka 0.5882 0.5882 0.5882 40 | Kd 0.5882 0.5882 0.5882 41 | Ks 0.0000 0.0000 0.0000 42 | Ke 0.0000 0.0000 0.0000 43 | map_Ka ../materials/textures/concrete.png 44 | map_Kd ../materials/textures/concrete.png 45 | 46 | newmtl Arrow 47 | Ns 0.0000 48 | Ni 1.5000 49 | d 1.0000 50 | Tr 0.0000 51 | Tf 1.0000 1.0000 1.0000 52 | illum 2 53 | Ka 1.0000 1.0000 1.0000 54 | Kd 1.0000 1.0000 1.0000 55 | Ks 0.0000 0.0000 0.0000 56 | Ke 0.0000 0.0000 0.0000 57 | map_Ka ../materials/textures/arrow.png 58 | map_Kd ../materials/textures/arrow.png 59 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/cloverleaf_interchange/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Cloverleaf Interchange 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Cole Biesemeyer 10 | cole.bsmr@gmail.com 11 | 12 | 13 | 14 | Nate Koenig 15 | natekoenig@gmail.com 16 | 17 | 18 | 19 | A model of a cloverleaf highway interchange. 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/cloverleaf_interchange/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://cloverleaf_interchange/meshes/cloverleaf.obj 10 | 0.0254 0.0254 0.0254 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://cloverleaf_interchange/meshes/cloverleaf.obj 18 | 0.0254 0.0254 0.0254 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/construction_cone/materials/textures/Construction_Cone_Diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/construction_cone/materials/textures/Construction_Cone_Diffuse.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/construction_cone/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 10 10 10 10 | model://construction_cone/meshes/construction_cone.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 10 10 10 18 | model://construction_cone/meshes/construction_cone.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/construction_cone/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 10 10 9 | model://construction_cone/meshes/construction_cone.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | 10 10 10 17 | model://construction_cone/meshes/construction_cone.dae 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/construction_cone/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Construction Cone 5 | 1.0 6 | model-1_3.sdf 7 | model-1_4.sdf 8 | model.sdf 9 | 10 | 11 | Cole Biesemeyer 12 | cole.bsmr@gmail.com 13 | 14 | 15 | 16 | Nate Koenig 17 | nate@osrfoundation.com 18 | 19 | 20 | 21 | An orange construction cone 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/construction_cone/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 10 10 9 | model://construction_cone/meshes/construction_cone.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | 10 10 10 17 | model://construction_cone/meshes/construction_cone.dae 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/dumpster/materials/scripts/dumpster.material: -------------------------------------------------------------------------------- 1 | material Dumpster/Diffuse 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | texture_unit 9 | { 10 | texture Dumpster_Diffuse.png 11 | } 12 | } 13 | } 14 | } 15 | 16 | material Dumpster/Specular 17 | { 18 | receive_shadows off 19 | technique 20 | { 21 | pass 22 | { 23 | texture_unit 24 | { 25 | texture Dumpster_Spec.png 26 | } 27 | } 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/dumpster/materials/textures/Dumpster_Diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/dumpster/materials/textures/Dumpster_Diffuse.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/dumpster/materials/textures/Dumpster_Spec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/dumpster/materials/textures/Dumpster_Spec.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/dumpster/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | model://dumpster/meshes/dumpster.dae 9 | 10 | 11 | 12 | 13 | 14 | 15 | model://dumpster/meshes/dumpster.dae 16 | 17 | 18 | 19 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/dumpster/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | model://dumpster/meshes/dumpster.dae 9 | 10 | 11 | 12 | 13 | 14 | 15 | model://dumpster/meshes/dumpster.dae 16 | 17 | 18 | 19 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/dumpster/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Dumpster 5 | 1.0 6 | model-1_3.sdf 7 | model-1_4.sdf 8 | model.sdf 9 | 10 | 11 | Cole Biesemeyer 12 | cole.bsmr@gmail.com 13 | 14 | 15 | 16 | Nate Koenig 17 | natekoenig@gmail.com 18 | 19 | 20 | 21 | A dumpster. 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/dumpster/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | model://dumpster/meshes/dumpster.dae 9 | 10 | 11 | 12 | 13 | 14 | 15 | model://dumpster/meshes/dumpster.dae 16 | 17 | 18 | 19 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/gas_station/materials/scripts/gas_station.material: -------------------------------------------------------------------------------- 1 | material GasStation/Diffuse 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | texture_unit 9 | { 10 | texture GasStation_Diffuse.png 11 | } 12 | } 13 | } 14 | } 15 | 16 | material GasStation/Specular 17 | { 18 | receive_shadows off 19 | technique 20 | { 21 | pass 22 | { 23 | texture_unit 24 | { 25 | texture GasStation_Spec.png 26 | } 27 | } 28 | } 29 | } 30 | 31 | material GasStation/Normal 32 | { 33 | receive_shadows off 34 | technique 35 | { 36 | pass 37 | { 38 | texture_unit 39 | { 40 | texture GasStation_Normal.png 41 | } 42 | } 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/gas_station/materials/textures/GasStation_Diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/gas_station/materials/textures/GasStation_Diffuse.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/gas_station/materials/textures/GasStation_Normal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/gas_station/materials/textures/GasStation_Normal.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/gas_station/materials/textures/GasStation_Spec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/gas_station/materials/textures/GasStation_Spec.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/gas_station/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 0 0 0 0 0 0 7 | 8 | 9 | 10 | model://gas_station/meshes/gas_station.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://gas_station/meshes/gas_station.dae 18 | 19 | 20 | 21 | 26 | 27 | GasStation_Normal.png 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/gas_station/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 0 0 0 0 0 0 7 | 8 | 9 | 10 | model://gas_station/meshes/gas_station.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://gas_station/meshes/gas_station.dae 18 | 19 | 20 | 21 | 26 | 27 | GasStation_Normal.png 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/gas_station/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Gas Station 5 | 1.0 6 | model-1_3.sdf 7 | model-1_4.sdf 8 | model.sdf 9 | 10 | 11 | Cole Biesemeyer 12 | cole.bsmr@gmail.com 13 | 14 | 15 | 16 | Nate Koenig 17 | natekoenig@gmail.com 18 | 19 | 20 | 21 | A model of a gas station. 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/gas_station/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 0 0 0 0 0 0 7 | 8 | 9 | 10 | model://gas_station/meshes/gas_station.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://gas_station/meshes/gas_station.dae 18 | 19 | 20 | 21 | 26 | 27 | GasStation_Normal.png 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/grey_wall/materials/scripts/grey_wall.material: -------------------------------------------------------------------------------- 1 | material vrc/grey_wall 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | texture_unit 9 | { 10 | texture grey_wall.png 11 | filtering anistropic 12 | max_anisotropy 16 13 | scale 1 1 14 | } 15 | } 16 | } 17 | } 18 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/grey_wall/materials/textures/grey_wall.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/grey_wall/materials/textures/grey_wall.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/grey_wall/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 0 0 1.4 0 0 0 7 | 8 | 9 | 10 | 7.5 0.2 2.8 11 | 12 | 13 | 14 | 15 | false 16 | 17 | 18 | 7.5 0.2 2.8 19 | 20 | 21 | 22 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/grey_wall/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 0 0 1.4 0 0 0 7 | 8 | 9 | 10 | 7.5 0.2 2.8 11 | 12 | 13 | 14 | 15 | false 16 | 17 | 18 | 7.5 0.2 2.8 19 | 20 | 21 | 22 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/grey_wall/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Grey Wall 5 | 1.0 6 | model-1_3.sdf 7 | model-1_4.sdf 8 | model.sdf 9 | 10 | 11 | Maurice Fallon 12 | mauricefallon@gmail.com 13 | 14 | 15 | 16 | A grey wall. 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/grey_wall/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 0 0 1.4 0 0 0 7 | 8 | 9 | 10 | 7.5 0.2 2.8 11 | 12 | 13 | 14 | 15 | false 16 | 17 | 18 | 7.5 0.2 2.8 19 | 20 | 21 | 22 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_1/materials/scripts/house_1.material: -------------------------------------------------------------------------------- 1 | material House_1/Diffuse 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | texture_unit 9 | { 10 | texture House_1_Diffuse.png 11 | } 12 | 13 | rtshader_system 14 | { 15 | lighting_stage normal_map House_1_Normal.png tangent_space 0 16 | } 17 | } 18 | } 19 | } 20 | 21 | material House_1/Specular 22 | { 23 | receive_shadows off 24 | technique 25 | { 26 | pass 27 | { 28 | texture_unit 29 | { 30 | texture House_1_Spec.png 31 | } 32 | } 33 | } 34 | } 35 | 36 | material House_1/Normal 37 | { 38 | receive_shadows off 39 | technique 40 | { 41 | pass 42 | { 43 | texture_unit 44 | { 45 | texture House_1_Normal.png 46 | } 47 | } 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_1/materials/textures/House_1_Diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/house_1/materials/textures/House_1_Diffuse.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_1/materials/textures/House_1_Normal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/house_1/materials/textures/House_1_Normal.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_1/materials/textures/House_1_Spec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/house_1/materials/textures/House_1_Spec.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_1/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://house_1/meshes/house_1.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | model://house_1/meshes/house_1.dae 17 | 18 | 19 | 20 | 25 | 26 | House_1_Normal.png 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_1/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://house_1/meshes/house_1.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | model://house_1/meshes/house_1.dae 17 | 18 | 19 | 20 | 25 | 26 | 27 | House_1_Normal.png 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | House 1 5 | 1.0 6 | model-1_3.sdf 7 | model-1_4.sdf 8 | model.sdf 9 | 10 | 11 | Cole Biesemeyer 12 | cole.bsmr@gmail.com 13 | 14 | 15 | 16 | Nate Koenig 17 | natekoenig@gmail.com 18 | 19 | 20 | 21 | A model of a residential house. 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_1/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://house_1/meshes/house_1.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | model://house_1/meshes/house_1.dae 17 | 18 | 19 | 20 | 25 | 26 | House_1_Normal.png 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_2/materials/scripts/house_2.material: -------------------------------------------------------------------------------- 1 | material House_2/Diffuse 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | texture_unit 9 | { 10 | texture House_2_Diffuse.png 11 | } 12 | } 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_2/materials/textures/House_2_Diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/house_2/materials/textures/House_2_Diffuse.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_2/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://house_2/meshes/house_2.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | model://house_2/meshes/house_2.dae 17 | 18 | 19 | 20 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_2/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://house_2/meshes/house_2.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | model://house_2/meshes/house_2.dae 17 | 18 | 19 | 20 | 26 | 27 | 28 | model://house_1/materials/textures/House_1_Normal.png 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_2/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | House 2 5 | 1.0 6 | model-1_3.sdf 7 | model-1_4.sdf 8 | model.sdf 9 | 10 | 11 | Cole Biesemeyer 12 | cole.bsmr@gmail.com 13 | 14 | 15 | 16 | Nate Koenig 17 | natekoenig@gmail.com 18 | 19 | 20 | 21 | A model of a residential house. 22 | 23 | 24 | 25 | 26 | model://house_1 27 | 1.0 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_2/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://house_2/meshes/house_2.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | model://house_2/meshes/house_2.dae 17 | 18 | 19 | 20 | 26 | 27 | model://house_1/materials/textures/House_1_Normal.png 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_3/materials/scripts/house_3.material: -------------------------------------------------------------------------------- 1 | material House_3/Diffuse 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | texture_unit 9 | { 10 | texture House_3_Diffuse.png 11 | } 12 | } 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_3/materials/textures/House_3_Diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/house_3/materials/textures/House_3_Diffuse.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_3/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://house_3/meshes/house_3.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | model://house_3/meshes/house_3.dae 17 | 18 | 19 | 20 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_3/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://house_3/meshes/house_3.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | model://house_3/meshes/house_3.dae 17 | 18 | 19 | 20 | 26 | 27 | 28 | model://house_1/materials/textures/House_1_Normal.png 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_3/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | House 3 5 | 1.0 6 | model-1_3.sdf 7 | model-1_4.sdf 8 | model.sdf 9 | 10 | 11 | Cole Biesemeyer 12 | cole.bsmr@gmail.com 13 | 14 | 15 | 16 | Nate Koenig 17 | natekoenig@gmail.com 18 | 19 | 20 | 21 | A model of a residential house. 22 | 23 | 24 | 25 | 26 | model://house_1 27 | 1.0 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/house_3/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://house_3/meshes/house_3.dae 10 | 11 | 12 | 13 | 14 | 15 | 16 | model://house_3/meshes/house_3.dae 17 | 18 | 19 | 20 | 26 | 27 | model://house_1/materials/textures/House_1_Normal.png 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/jersey_barrier/materials/textures/jersey_barrier.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/jersey_barrier/materials/textures/jersey_barrier.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/jersey_barrier/materials/textures/jersey_barrier_diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/jersey_barrier/materials/textures/jersey_barrier_diffuse.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/jersey_barrier/materials/textures/jersey_barrier_spec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/jersey_barrier/materials/textures/jersey_barrier_spec.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/jersey_barrier/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://jersey_barrier/meshes/jersey_barrier.dae 10 | 11 | 12 | 13 | 14 | 15 | 0 0 0.5715 0 0 0 16 | 17 | 18 | 4.06542 0.3063 1.143 19 | 20 | 21 | 22 | 23 | 24 | 0 0 0.032258 0 0 0 25 | 26 | 27 | 4.06542 0.8107 0.064516 28 | 29 | 30 | 31 | 32 | 33 | 0 0 0.1 0 0 0 34 | 35 | 36 | 4.06542 0.65 0.1 37 | 38 | 39 | 40 | 41 | 42 | 0 0 0.2 0 0 0 43 | 44 | 45 | 4.06542 0.5 0.1 46 | 47 | 48 | 49 | 50 | 51 | 0 -0.224 0.2401 0.9 0 0 52 | 53 | 54 | 4.06542 0.5 0.064516 55 | 56 | 57 | 58 | 59 | 60 | 0 0.224 0.2401 -0.9 0 0 61 | 62 | 63 | 4.06542 0.5 0.064516 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/jersey_barrier/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Jersey Barrier 5 | 1.0 6 | model-1_4.sdf 7 | model.sdf 8 | 9 | 10 | Cole Biesemeyer 11 | cole.bsmr@gmail.com 12 | 13 | 14 | 15 | Nate Koenig 16 | natekoenig@gmail.com 17 | 18 | 19 | 20 | A model of a jersey barrier. 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/jersey_barrier/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://jersey_barrier/meshes/jersey_barrier.dae 10 | 11 | 12 | 13 | 14 | 0 0 0.5715 0 0 0 15 | 16 | 17 | 4.06542 0.3063 1.143 18 | 19 | 20 | 21 | 22 | 0 0 0.032258 0 0 0 23 | 24 | 25 | 4.06542 0.8107 0.064516 26 | 27 | 28 | 29 | 30 | 0 0 0.1 0 0 0 31 | 32 | 33 | 4.06542 0.65 0.1 34 | 35 | 36 | 37 | 38 | 0 0 0.2 0 0 0 39 | 40 | 41 | 4.06542 0.5 0.1 42 | 43 | 44 | 45 | 46 | 0 -0.224 0.2401 0.9 0 0 47 | 48 | 49 | 4.06542 0.5 0.064516 50 | 51 | 52 | 53 | 54 | 0 0.224 0.2401 -0.9 0 0 55 | 56 | 57 | 4.06542 0.5 0.064516 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Asphalt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Asphalt.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Brick.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Brick.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Buildings.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Buildings.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Colors.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Colors.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Concrete_Ground.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Concrete_Ground.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Crosswalk.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Crosswalk.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Curb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Curb.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Grass.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Grass.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Gravel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Gravel.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/GuardRail.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/GuardRail.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/OverheadSignBase.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/OverheadSignBase.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/OverheadSigns.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/OverheadSigns.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/RailroadGate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/RailroadGate.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Road_Blank.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Road_Blank.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Road_Blank_Light.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Road_Blank_Light.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Road_DoubleYellowLine.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Road_DoubleYellowLine.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Road_DoubleYellowLine_Light.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Road_DoubleYellowLine_Light.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Road_SingleWhiteLine.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Road_SingleWhiteLine.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/StreetLamp_Short.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/StreetLamp_Short.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/StreetLamp_Tall.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/StreetLamp_Tall.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/TrafficBarrel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/TrafficBarrel.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/materials/textures/Tunnel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/mcity/materials/textures/Tunnel.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | MCity 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | A model of MCity 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/mcity/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0.254 0.254 0.254 10 | model://mcity/meshes/mcity.obj 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 0.254 0.254 0.254 19 | model://mcity/meshes/mcity.obj 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_1.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_10.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_11.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_12.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_12.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_13.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_13.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_17.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_17.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_19.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_19.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_2.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_20.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_20.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_21.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_21.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_22.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_22.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_23.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_23.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_25.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_25.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_26.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_26.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_27.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_27.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_3.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_32.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_32.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_33.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_33.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_34.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_34.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_35.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_35.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_36.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_36.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_37.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_37.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_38.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_38.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_6.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_7.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_8.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/materials/textures/__auto_9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/powerplant/materials/textures/__auto_9.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/model-1_2.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 0 0 0.01 0 0 0 7 | 8 | 9 | 10 | 11 | model://powerplant/meshes/powerplant.dae 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | model://powerplant/meshes/powerplant.dae 20 | 21 | 22 | 23 | 24 | 25 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 0 0 0.01 0 0 0 7 | 8 | 9 | 10 | 11 | model://powerplant/meshes/powerplant.dae 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | model://powerplant/meshes/powerplant.dae 20 | 21 | 22 | 23 | 24 | 25 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 0 0 0.01 0 0 0 7 | 8 | 9 | 10 | 11 | model://powerplant/meshes/powerplant.dae 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | model://powerplant/meshes/powerplant.dae 20 | 21 | 22 | 23 | 24 | 25 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Power Plant 5 | 1.0 6 | 7 | model-1_2.sdf 8 | model-1_3.sdf 9 | model-1_4.sdf 10 | model.sdf 11 | 12 | 13 | Nate Koenig 14 | nate@osrfoundation.org 15 | 16 | 17 | 18 | A power plant. 19 | 20 | 21 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/powerplant/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 0 0 0.01 0 0 0 7 | 8 | 9 | 10 | model://powerplant/meshes/powerplant.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://powerplant/meshes/powerplant.dae 18 | 19 | 20 | 21 | 22 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_100.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_100.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_110.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_110.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_120.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_120.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_130.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_130.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_16.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_16.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_30.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_30.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_40.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_40.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_50.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_50.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_60.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_60.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_70.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_70.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_80.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_80.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_90.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_90.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_Spec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/Speed_Spec.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/StopSign_Diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/StopSign_Diffuse.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/materials/textures/StopSign_Spec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/car_demo/models/speed_limit_sign/materials/textures/StopSign_Spec.png -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 10 10 10 10 | model://speed_limit_sign/meshes/speed_limit_sign.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 10 10 10 18 | model://speed_limit_sign/meshes/speed_limit_sign.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 10 10 10 10 | model://speed_limit_sign/meshes/speed_limit_sign.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 10 10 10 18 | model://speed_limit_sign/meshes/speed_limit_sign.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Speed limit sign 5 | 1.0 6 | model-1_3.sdf 7 | model-1_4.sdf 8 | model.sdf 9 | 10 | 11 | Cole Biesemeyer 12 | cole.bsmr@gmail.com 13 | 14 | 15 | 16 | Nate Koenig 17 | nate@osrfoundation.com 18 | 19 | 20 | 21 | A speed limit sign 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /car_demo/car_demo/models/speed_limit_sign/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 10 10 10 10 | model://speed_limit_sign/meshes/speed_limit_sign.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 10 10 10 18 | model://speed_limit_sign/meshes/speed_limit_sign.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /car_demo/car_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | car_demo 3 | 0.0.1 4 | 5 | The car demo 6 | 7 | Tully Foote 8 | APACHE 2.0 9 | catkin 10 | 11 | gazebo_ros 12 | prius_msgs 13 | roscpp 14 | nav_msgs 15 | se2_navigation_msgs 16 | pure_pursuit_ros 17 | tf 18 | yaml-cpp 19 | 20 | 21 | fake_localization 22 | joy 23 | prius_description 24 | robot_state_publisher 25 | rospy 26 | rviz 27 | sensor_msgs 28 | tf2_ros 29 | se2_planning_ros 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /car_demo/car_demo/src/prius_controller_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * prius_controller_node.cpp 3 | * 4 | * Created on: Apr 6, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #include 9 | #include "car_demo/PriusControllerRos.hpp" 10 | 11 | using namespace car_demo; 12 | int main(int argc, char** argv) { 13 | ros::init(argc, argv, "prius_controller_node"); 14 | ros::NodeHandlePtr nh(new ros::NodeHandle("~")); 15 | PriusControllerRos controller(nh); 16 | const double frequency = 50.0; 17 | controller.initialize(1 / frequency); 18 | 19 | sleep(2.0); 20 | ros::Rate r(frequency); 21 | while (ros::ok()) { 22 | controller.advance(); 23 | ros::spinOnce(); 24 | r.sleep(); 25 | } 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /car_demo/doc/car.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/doc/car.gif -------------------------------------------------------------------------------- /car_demo/doc/m545.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/doc/m545.gif -------------------------------------------------------------------------------- /car_demo/doc/spacebok_nav.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/doc/spacebok_nav.gif -------------------------------------------------------------------------------- /car_demo/prius_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(prius_description) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 9 | -------------------------------------------------------------------------------- /car_demo/prius_description/meshes/Hybrid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/prius_description/meshes/Hybrid.png -------------------------------------------------------------------------------- /car_demo/prius_description/meshes/Hybrid_Interior.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/prius_description/meshes/Hybrid_Interior.png -------------------------------------------------------------------------------- /car_demo/prius_description/meshes/Wheels3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/car_demo/prius_description/meshes/Wheels3.png -------------------------------------------------------------------------------- /car_demo/prius_description/meshes/hybrid_body.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 3 3 | 4 | newmtl Hybrid 5 | Ns 9.803922 6 | Ka 0.588200 0.588200 0.588200 7 | Kd 0.588200 0.588200 0.588200 8 | Ks 0.000000 0.000000 0.000000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.500000 11 | d 1.000000 12 | illum 2 13 | map_Kd Hybrid.png 14 | map_Ka Hybrid.png 15 | 16 | newmtl Hybrid_Interior 17 | Ns 9.803922 18 | Ka 0.588200 0.588200 0.588200 19 | Kd 0.588200 0.588200 0.588200 20 | Ks 0.000000 0.000000 0.000000 21 | Ke 0.000000 0.000000 0.000000 22 | Ni 1.500000 23 | d 1.000000 24 | illum 2 25 | map_Kd Hybrid_Interior.png 26 | map_Ka Hybrid_Interior.png 27 | 28 | newmtl Windows 29 | Ns 0.000000 30 | Ka 0.262700 0.262700 0.262700 31 | Kd 0.262700 0.262700 0.262700 32 | Ks 0.000000 0.000000 0.000000 33 | Ke 0.000000 0.000000 0.000000 34 | Ni 1.500000 35 | d 0.100000 36 | illum 2 37 | -------------------------------------------------------------------------------- /car_demo/prius_description/meshes/steering_wheel.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl Hybrid_Interior 5 | Ns 9.803922 6 | Ka 0.588200 0.588200 0.588200 7 | Kd 0.588200 0.588200 0.588200 8 | Ks 0.000000 0.000000 0.000000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.500000 11 | d 1.000000 12 | illum 2 13 | map_Kd Hybrid_Interior.png 14 | map_Ka Hybrid_Interior.png 15 | -------------------------------------------------------------------------------- /car_demo/prius_description/meshes/wheel.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl Wheels3 5 | Ns 9.803922 6 | Ka 0.588200 0.588200 0.588200 7 | Kd 0.588200 0.588200 0.588200 8 | Ks 0.000000 0.000000 0.000000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.500000 11 | d 1.000000 12 | illum 2 13 | map_Kd Wheels3.png 14 | map_Ka Wheels3.png 15 | -------------------------------------------------------------------------------- /car_demo/prius_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | prius_description 3 | 0.0.1 4 | 5 | Provides a URDF for a prius 6 | 7 | shane loretz 8 | APACHE 2.0 9 | 10 | catkin 11 | urdf 12 | 13 | -------------------------------------------------------------------------------- /car_demo/prius_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(prius_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | ) 8 | 9 | add_message_files( 10 | FILES 11 | Control.msg 12 | ) 13 | 14 | generate_messages( 15 | DEPENDENCIES 16 | std_msgs 17 | ) 18 | 19 | catkin_package( 20 | INCLUDE_DIRS 21 | include 22 | CATKIN_DEPENDS 23 | std_msgs 24 | message_runtime 25 | ) 26 | 27 | include_directories( 28 | include 29 | ${catkin_INCLUDE_DIRS} 30 | ) 31 | 32 | install( 33 | DIRECTORY include/${PROJECT_NAME}/ 34 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 35 | FILES_MATCHING PATTERN "*.hpp" 36 | ) -------------------------------------------------------------------------------- /car_demo/prius_msgs/msg/Control.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # Range 0 to 1, 1 is max throttle 4 | float64 throttle 5 | # Range 0 to 1, 1 is max brake 6 | float64 brake 7 | # Range -1 to +1, +1 is maximum left turn 8 | float64 steer 9 | 10 | uint8 NO_COMMAND=0 11 | uint8 NEUTRAL=1 12 | uint8 FORWARD=2 13 | uint8 REVERSE=3 14 | 15 | uint8 shift_gears 16 | -------------------------------------------------------------------------------- /car_demo/prius_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | prius_msgs 3 | 0.0.1 4 | 5 | Messages for controllign prius 6 | 7 | shane loretz 8 | APACHE 2.0 9 | 10 | catkin 11 | 12 | message_generation 13 | message_runtime 14 | 15 | std_msgs 16 | 17 | 18 | -------------------------------------------------------------------------------- /jenkins-pipeline: -------------------------------------------------------------------------------- 1 | library 'continuous_integration_pipeline' 2 | ciPipeline("") -------------------------------------------------------------------------------- /pure_pursuit_core/doc/controller_convention.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/pure_pursuit_core/doc/controller_convention.pdf -------------------------------------------------------------------------------- /pure_pursuit_core/doc/controller_convention.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/pure_pursuit_core/doc/controller_convention.png -------------------------------------------------------------------------------- /pure_pursuit_core/doc/controller_convention_back.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/pure_pursuit_core/doc/controller_convention_back.pdf -------------------------------------------------------------------------------- /pure_pursuit_core/doc/controller_convention_back.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/pure_pursuit_core/doc/controller_convention_back.png -------------------------------------------------------------------------------- /pure_pursuit_core/doc/path_conventions.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/pure_pursuit_core/doc/path_conventions.pdf -------------------------------------------------------------------------------- /pure_pursuit_core/doc/path_conventions.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/pure_pursuit_core/doc/path_conventions.png -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/Path.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Path.hpp 3 | * 4 | * Created on: Mar 19, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include "pure_pursuit_core/common.hpp" 13 | #include "pure_pursuit_core/typedefs.hpp" 14 | 15 | namespace pure_pursuit { 16 | 17 | struct PathPoint { 18 | PathPoint() = default; 19 | explicit PathPoint(const Point& p) : position_(p) {} 20 | PathPoint(double x, double y) : position_(x, y) {} 21 | Point position_{0.0, 0.0}; 22 | friend std::ostream& operator<<(std::ostream& out, const PathPoint& pathPoint); 23 | }; 24 | 25 | struct PathSegment { 26 | DrivingDirection drivingDirection_; 27 | std::vector point_; 28 | friend std::ostream& operator<<(std::ostream& out, const PathSegment& pathSegment); 29 | }; 30 | 31 | struct Path { 32 | std::vector segment_; 33 | friend std::ostream& operator<<(std::ostream& out, const Path& path); 34 | }; 35 | 36 | } // namespace pure_pursuit 37 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/common.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * common.hpp 3 | * 4 | * Created on: Mar 19, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include "pure_pursuit_core/typedefs.hpp" 12 | 13 | namespace pure_pursuit { 14 | 15 | enum class DrivingDirection : int { 16 | FWD, // forward 17 | BCK // backwards 18 | }; 19 | 20 | struct RobotPose { 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | RobotPose() = default; 23 | RobotPose(double x, double y, double yaw); 24 | Point position_{0.0, 0.0}; 25 | double yaw_ = 0.0; 26 | friend std::ostream& operator<<(std::ostream& out, const RobotPose& robotPose); 27 | }; 28 | 29 | struct RobotTwist { 30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 31 | Vector linear_{0.0, 0.0}; 32 | double ang_ = 0.0; 33 | }; 34 | 35 | struct RobotState { 36 | RobotPose pose_; 37 | RobotTwist twist_; 38 | double desiredLongitudinalVelocity_; 39 | 40 | friend std::ostream& operator<<(std::ostream& out, const RobotState& robotState); 41 | }; 42 | 43 | std::string toString(DrivingDirection direction); 44 | 45 | } /* namespace pure_pursuit */ 46 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/heading_control/AckermannSteeringController.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AckermannSteeringController.hpp 3 | * 4 | * Created on: Mar 21, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include "pure_pursuit_core/heading_control/HeadingController.hpp" 13 | #include "pure_pursuit_core/math.hpp" 14 | 15 | namespace pure_pursuit { 16 | 17 | struct AckermannSteeringCtrlParameters : public HeadingControllerParameters { 18 | double wheelBase_ = 4.0; 19 | double maxSteeringAngleMagnitude_ = 0.5; // rad 20 | double maxSteeringRateOfChange_ = 0.2; // rad/s 21 | double dt_ = 0.01; // seconds 22 | std::string asString() const override; 23 | }; 24 | 25 | class AckermannSteeringController : public HeadingController { 26 | public: 27 | AckermannSteeringController() = default; 28 | ~AckermannSteeringController() override = default; 29 | 30 | virtual void setParameters(const AckermannSteeringCtrlParameters& parameters); 31 | const AckermannSteeringCtrlParameters& getParameters() const override; 32 | void updateCurrentPathSegment(const PathSegment& pathSegment) override; 33 | bool initialize() override; 34 | 35 | protected: 36 | bool advanceImpl() override; 37 | bool computeYawRate() override; 38 | bool computeTurningRadius() override; 39 | bool computeSteeringAngle() override; 40 | 41 | AckermannSteeringCtrlParameters parameters_; 42 | RateLimiter rateLimiter_; 43 | AverageFilter avgFilter_; 44 | Point currentAnchorPoint_, currentLookaheadPoint_; 45 | }; 46 | 47 | std::unique_ptr createAckermannSteeringController(const AckermannSteeringCtrlParameters& parameters); 48 | 49 | } // namespace pure_pursuit 50 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/heading_control/HeadingController.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * HeadingController.hpp 3 | * 4 | * Created on: Mar 21, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include "pure_pursuit_core/Path.hpp" 10 | #include "pure_pursuit_core/common.hpp" 11 | 12 | namespace pure_pursuit { 13 | 14 | struct HeadingControllerParameters { 15 | virtual ~HeadingControllerParameters() = default; 16 | double lookaheadDistanceFwd_ = 4.0; 17 | double lookaheadDistanceBck_ = 4.0; 18 | double anchorDistanceFwd_ = 0.2; 19 | double anchorDistanceBck_ = 0.2; 20 | double deadZoneWidth_ = 0.0; 21 | double avgFilgerCurrentSampleWeight_ = 1.0; 22 | virtual std::string asString() const; 23 | }; 24 | 25 | class HeadingController { 26 | public: 27 | HeadingController() = default; 28 | virtual ~HeadingController() = default; 29 | 30 | virtual bool initialize(); 31 | bool advance(); 32 | virtual void updateCurrentPathSegment(const PathSegment& pathSegment); 33 | virtual void updateCurrentState(const RobotState& robState); 34 | void updateDesiredVelocity(const Vector& v); 35 | double getTurningRadius() const; 36 | double getYawRate() const; 37 | double getSteeringAngle() const; 38 | virtual const HeadingControllerParameters& getParameters() const = 0; 39 | double getActiveLookaheadDistance() const; 40 | 41 | private: 42 | virtual bool advanceImpl() = 0; 43 | virtual bool computeYawRate() = 0; 44 | virtual bool computeTurningRadius() = 0; 45 | virtual bool computeSteeringAngle() = 0; 46 | 47 | protected: 48 | virtual void chooseActiveAnchorAndLookaheadDistance(const HeadingControllerParameters& parameters); 49 | RobotState currentRobotState_; 50 | double turningRadius_ = 0.0; 51 | double yawRate_ = 0.0; 52 | double steeringAngle_ = 0.0; 53 | double activeAnchorDistance_ = 0.2; 54 | double activeLookaheadDistance_ = 0.2; 55 | PathSegment currentPathSegment_; 56 | unsigned int lastClosestPointId_ = 0; 57 | Vector desiredLinearVelocity_; 58 | }; 59 | 60 | } // namespace pure_pursuit 61 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/heading_control/MobileBaseHeadingController.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MobileBaseHeadingController.hpp 3 | * 4 | * Created on: Mar 21, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "pure_pursuit_core/heading_control/HeadingController.hpp" 11 | 12 | namespace pure_pursuit { 13 | 14 | class MobileBaseHeadingController : public HeadingController { 15 | public: 16 | MobileBaseHeadingController() = default; 17 | ~MobileBaseHeadingController() override = default; 18 | const HeadingControllerParameters& getParameters() const override; 19 | 20 | private: 21 | bool advanceImpl() override; 22 | bool computeYawRate() override; 23 | bool computeTurningRadius() override; 24 | bool computeSteeringAngle() override; 25 | 26 | HeadingControllerParameters param_; 27 | }; 28 | 29 | } // namespace pure_pursuit 30 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/path_tracking/PathPreprocessor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PathPreprocessor.hpp 3 | * 4 | * Created on: Mar 24, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | namespace pure_pursuit { 13 | 14 | class Path; 15 | 16 | struct PathPreprocessorParameters { 17 | double minimumSegmentLength_ = 1.0; // meters 18 | }; 19 | 20 | class PathPreprocessor { 21 | public: 22 | virtual ~PathPreprocessor() = default; 23 | 24 | virtual bool preprocessPath(Path* path); 25 | void setParameters(const PathPreprocessorParameters& parameters); 26 | 27 | protected: 28 | int removeShortPathSegments(Path* path); 29 | int mergePathSegmentsWithSameDrivingDirections(Path* path); 30 | PathPreprocessorParameters parameters_; 31 | }; 32 | 33 | std::unique_ptr createPathPreprocessor(const PathPreprocessorParameters& parameters); 34 | 35 | } /* namespace pure_pursuit */ 36 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/path_tracking/ProgressValidator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TrackingProgress.hpp 3 | * 4 | * Created on: Mar 22, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | namespace pure_pursuit { 13 | 14 | class PathSegment; 15 | class RobotState; 16 | class Path; 17 | 18 | struct ProgressValidatorParameters { 19 | double goalDistanceTolerance_ = 0.05; 20 | }; 21 | 22 | class ProgressValidator { 23 | public: 24 | ProgressValidator() = default; 25 | virtual ~ProgressValidator() = default; 26 | 27 | virtual bool isPathSegmentTrackingFinished(const PathSegment& pathSegment, const RobotState& currentState) const; 28 | virtual bool isPathTrackingFinished(const Path& path, const RobotState& currentState, unsigned int currentSegmenet) const; 29 | 30 | void setParameters(const ProgressValidatorParameters& parameters); 31 | 32 | protected: 33 | ProgressValidatorParameters parameters_; 34 | }; 35 | 36 | std::unique_ptr createProgressValidator(const ProgressValidatorParameters& parameters); 37 | 38 | } // namespace pure_pursuit 39 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/path_tracking/SimplePathTracker.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SimplePathTracker.hpp 3 | * 4 | * Created on: Mar 24, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include "pure_pursuit_core/path_tracking/PathTracker.hpp" 13 | 14 | namespace pure_pursuit { 15 | 16 | struct SimplePathTrackerParameters { 17 | double waitingTimeBetweenDirectionSwitches_ = 1.0; 18 | }; 19 | 20 | class SimplePathTracker : public PathTracker { 21 | public: 22 | SimplePathTracker() = default; 23 | ~SimplePathTracker() override = default; 24 | 25 | void importCurrentPath(const Path& path) override; 26 | void updateCurrentPath(const Path& path) override; 27 | void stopTracking() override; 28 | void setParameters(const SimplePathTrackerParameters& parameters); 29 | 30 | protected: 31 | enum class States : int { NoOperation, Waiting, Driving }; 32 | void advanceStateMachine() override; 33 | bool advanceControllers() override; 34 | bool isPathAndCurrenStateWithinRadius(const Path& path, double raidus) const; 35 | 36 | States currentFSMState_ = States::NoOperation; 37 | bool isPathReceived_ = false; 38 | bool isPathUpdated_ = false; 39 | Stopwatch stopwatch_; 40 | SimplePathTrackerParameters parameters_; 41 | }; 42 | 43 | std::unique_ptr createSimplePathTracker(const SimplePathTrackerParameters& parameters, 44 | std::shared_ptr velocityController, 45 | std::shared_ptr headingController, 46 | std::shared_ptr validator, 47 | std::shared_ptr pathPreprocessor); 48 | 49 | } /* namespace pure_pursuit */ 50 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/path_tracking/Stopwatch.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Stopwatch.hpp 3 | * 4 | * Created on: Aug 25, 2019 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | 11 | namespace pure_pursuit { 12 | 13 | class Stopwatch { 14 | public: 15 | using Timepoint = std::chrono::steady_clock::time_point; 16 | 17 | Stopwatch() = default; 18 | virtual ~Stopwatch() = default; 19 | 20 | void start(); 21 | 22 | double getElapsedTimeSinceStartSeconds() const; 23 | 24 | private: 25 | Timepoint startingTime_; 26 | bool isStopwatchStarted_ = false; 27 | }; 28 | 29 | } /*namespace pure_pursuit */ 30 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/typedefs.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * typedefs.hpp 3 | * 4 | * Created on: Mar 19, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | 11 | namespace pure_pursuit { 12 | 13 | using Vector = Eigen::Vector2d; 14 | using Point = Eigen::Vector2d; 15 | using Matrix = Eigen::Matrix2d; 16 | 17 | } // namespace pure_pursuit 18 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/velocity_control/AdaptiveVelocityController.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AdaptiveVelocityController.hpp 3 | * 4 | * Created on: Apr 9, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include "pure_pursuit_core/math.hpp" 13 | #include "pure_pursuit_core/velocity_control/LongitudinalVelocityController.hpp" 14 | 15 | namespace pure_pursuit { 16 | 17 | struct AdaptiveVelocityControllerParameters { 18 | double desiredVelocity_ = 0.0; 19 | double timestep_ = 0.01; 20 | double maxVelocityRateOfChange_ = 0.3; 21 | double distanceToGoalWhenBrakingStarts_ = 4.0; // meters 22 | std::string asString() const; 23 | }; 24 | 25 | class AdaptiveVelocityController : public LongitudinalVelocityController { 26 | using BASE = LongitudinalVelocityController; 27 | 28 | public: 29 | AdaptiveVelocityController() = default; 30 | ~AdaptiveVelocityController() override = default; 31 | virtual void setParameters(const AdaptiveVelocityControllerParameters& parameters); 32 | void updateCurrentPathSegment(const PathSegment& pathSegment) override; 33 | 34 | private: 35 | bool computeVelocity() override; 36 | 37 | protected: 38 | AdaptiveVelocityControllerParameters parameters_; 39 | RateLimiter rateLimiter_; 40 | }; 41 | 42 | std::unique_ptr createAdaptiveVelocityController(const AdaptiveVelocityControllerParameters& parameters); 43 | 44 | } /* namespace pure_pursuit */ 45 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/velocity_control/ConstantVelocityController.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ConstantVelocityController.hpp 3 | * 4 | * Created on: Mar 20, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include "pure_pursuit_core/math.hpp" 13 | #include "pure_pursuit_core/velocity_control/LongitudinalVelocityController.hpp" 14 | 15 | namespace pure_pursuit { 16 | 17 | struct ConstantVelocityControllerParameters { 18 | double constantDesiredVelocity_ = 0.0; 19 | double timestep_ = 0.01; 20 | double maxVelocityRateOfChange_ = 0.3; 21 | }; 22 | 23 | class ConstantVelocityController : public LongitudinalVelocityController { 24 | public: 25 | ConstantVelocityController() = default; 26 | ~ConstantVelocityController() override = default; 27 | void setParameters(const ConstantVelocityControllerParameters& parameters); 28 | void updateDrivingDirection(DrivingDirection drivingDirection) override; 29 | 30 | private: 31 | bool computeVelocity() override; 32 | 33 | protected: 34 | ConstantVelocityControllerParameters parameters_; 35 | RateLimiter rateLimiter_; 36 | }; 37 | 38 | std::unique_ptr createConstantVelocityController(const ConstantVelocityControllerParameters& parameters); 39 | 40 | } /* namespace pure_pursuit */ 41 | -------------------------------------------------------------------------------- /pure_pursuit_core/include/pure_pursuit_core/velocity_control/LongitudinalVelocityController.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LongitudinalVelocityController.hpp 3 | * 4 | * Created on: Mar 20, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "pure_pursuit_core/Path.hpp" 11 | #include "pure_pursuit_core/common.hpp" 12 | 13 | namespace pure_pursuit { 14 | 15 | class LongitudinalVelocityController { 16 | public: 17 | LongitudinalVelocityController() = default; 18 | virtual ~LongitudinalVelocityController() = default; 19 | 20 | bool advance(); 21 | double getVelocity() const; 22 | virtual void updateDrivingDirection(DrivingDirection drivingDirection); 23 | 24 | virtual void updateCurrentState(const RobotState& robState); 25 | virtual bool initialize(); 26 | virtual void updateCurrentPathSegment(const PathSegment& pathSegment); 27 | 28 | private: 29 | virtual bool computeVelocity() = 0; 30 | 31 | protected: 32 | double desiredLongitudinalVelocity_ = 0.0; 33 | RobotState currentRobotState_; 34 | DrivingDirection drivingDirection_; 35 | PathSegment currentPathSegment_; 36 | }; 37 | 38 | } /* namespace pure_pursuit */ 39 | -------------------------------------------------------------------------------- /pure_pursuit_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pure_pursuit_core 4 | 0.1.0 5 | Pure pursuit controller. 6 | Edo Jelavic 7 | BSD 8 | 9 | catkin 10 | 11 | eigen 12 | 13 | 14 | -------------------------------------------------------------------------------- /pure_pursuit_core/src/ConstantVelocityController.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ConstantVelocityController.cpp 3 | * 4 | * Created on: Mar 20, 2020 5 | * Author: jelavice 6 | */ 7 | #include "pure_pursuit_core/velocity_control/ConstantVelocityController.hpp" 8 | #include 9 | 10 | namespace pure_pursuit { 11 | 12 | void ConstantVelocityController::updateDrivingDirection(DrivingDirection drivingDirection) { 13 | if (drivingDirection != drivingDirection_) { 14 | rateLimiter_.reset(0.0); // assumption is that the robot is at zero velocity when changing directions 15 | std::cout << "reseting the velocity rate limiter" << std::endl; 16 | } 17 | drivingDirection_ = drivingDirection; 18 | } 19 | 20 | bool ConstantVelocityController::computeVelocity() { 21 | switch (drivingDirection_) { 22 | case DrivingDirection::FWD: { 23 | desiredLongitudinalVelocity_ = rateLimiter_.limitRateOfChange(parameters_.constantDesiredVelocity_); 24 | break; 25 | } 26 | 27 | case DrivingDirection::BCK: { 28 | desiredLongitudinalVelocity_ = rateLimiter_.limitRateOfChange(-parameters_.constantDesiredVelocity_); 29 | break; 30 | } 31 | } 32 | return true; 33 | } 34 | 35 | void ConstantVelocityController::setParameters(const ConstantVelocityControllerParameters& parameters) { 36 | if (parameters.maxVelocityRateOfChange_ < 0) { 37 | throw std::runtime_error("maxVelocityRateOfChange_ is less than 0."); 38 | } 39 | 40 | if (parameters.timestep_ < 0) { 41 | throw std::runtime_error("timestep_ is less than 0."); 42 | } 43 | 44 | parameters_ = parameters; 45 | rateLimiter_.setFallingRate(-parameters.maxVelocityRateOfChange_); 46 | rateLimiter_.setRisingRate(parameters.maxVelocityRateOfChange_); 47 | rateLimiter_.setTimestep(parameters.timestep_); 48 | } 49 | 50 | std::unique_ptr createConstantVelocityController(const ConstantVelocityControllerParameters& parameters) { 51 | std::unique_ptr controller = std::make_unique(); 52 | controller->setParameters(parameters); 53 | return std::move(controller); 54 | } 55 | 56 | } /* namespace pure_pursuit */ 57 | -------------------------------------------------------------------------------- /pure_pursuit_core/src/LongitudinalVelocityController.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LongitudinalVelocityController.cpp 3 | * 4 | * Created on: Mar 20, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #include "pure_pursuit_core/velocity_control/LongitudinalVelocityController.hpp" 9 | 10 | namespace pure_pursuit { 11 | 12 | bool LongitudinalVelocityController::advance() { 13 | return computeVelocity(); 14 | } 15 | void LongitudinalVelocityController::updateCurrentPathSegment(const PathSegment& pathSegment) { 16 | currentPathSegment_ = pathSegment; 17 | } 18 | 19 | bool LongitudinalVelocityController::initialize() { 20 | return true; 21 | } 22 | double LongitudinalVelocityController::getVelocity() const { 23 | return desiredLongitudinalVelocity_; 24 | } 25 | 26 | void LongitudinalVelocityController::updateDrivingDirection(DrivingDirection drivingDirection) { 27 | drivingDirection_ = drivingDirection; 28 | } 29 | 30 | void LongitudinalVelocityController::updateCurrentState(const RobotState& robState) { 31 | currentRobotState_ = robState; 32 | } 33 | 34 | } /* namespace pure_pursuit */ 35 | -------------------------------------------------------------------------------- /pure_pursuit_core/src/MobileBaseHeadingController.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MobileBaseHeadingController.cpp 3 | * 4 | * Created on: Mar 21, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #include "pure_pursuit_core/heading_control/MobileBaseHeadingController.hpp" 9 | 10 | namespace pure_pursuit { 11 | 12 | bool MobileBaseHeadingController::advanceImpl() {} 13 | 14 | bool MobileBaseHeadingController::computeYawRate() {} 15 | 16 | bool MobileBaseHeadingController::computeTurningRadius() {} 17 | 18 | bool MobileBaseHeadingController::computeSteeringAngle() {} 19 | 20 | const HeadingControllerParameters& MobileBaseHeadingController::getParameters() const { 21 | return param_; 22 | } 23 | 24 | } /* namespace pure_pursuit */ 25 | -------------------------------------------------------------------------------- /pure_pursuit_core/src/ProgressValidator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ProgressValidator.cpp 3 | * 4 | * Created on: Mar 22, 2020 5 | * Author: jelavice 6 | */ 7 | #include "pure_pursuit_core/path_tracking/ProgressValidator.hpp" 8 | 9 | #include "pure_pursuit_core/Path.hpp" 10 | #include "pure_pursuit_core/math.hpp" 11 | 12 | namespace pure_pursuit { 13 | 14 | bool ProgressValidator::isPathSegmentTrackingFinished(const PathSegment& pathSegment, const RobotState& currentState) const { 15 | const Point currPosition = currentState.pose_.position_; 16 | const Point goalPosition = pathSegment.point_.back().position_; 17 | const bool isCloseEnough = (currPosition - goalPosition).norm() < parameters_.goalDistanceTolerance_; 18 | return isCloseEnough || isPastLastPoint(pathSegment, currPosition); 19 | } 20 | bool ProgressValidator::isPathTrackingFinished(const Path& path, const RobotState& currentState, unsigned int currentSegment) const { 21 | const bool isTrackingLastSegment = path.segment_.size() - 1 == currentSegment; 22 | return isTrackingLastSegment && isPathSegmentTrackingFinished(path.segment_.at(currentSegment), currentState); 23 | } 24 | 25 | void ProgressValidator::setParameters(const ProgressValidatorParameters& parameters) { 26 | parameters_ = parameters; 27 | } 28 | 29 | std::unique_ptr createProgressValidator(const ProgressValidatorParameters& parameters) { 30 | std::unique_ptr validator = std::make_unique(); 31 | validator->setParameters(parameters); 32 | return std::move(validator); 33 | } 34 | 35 | } // namespace pure_pursuit 36 | -------------------------------------------------------------------------------- /pure_pursuit_core/src/Stopwatch.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Stopwatch.cpp 3 | * 4 | * Created on: Aug 25, 2019 5 | * Author: jelavice 6 | */ 7 | 8 | #include "pure_pursuit_core/path_tracking/Stopwatch.hpp" 9 | 10 | #include 11 | 12 | namespace pure_pursuit { 13 | 14 | void Stopwatch::start() { 15 | startingTime_ = std::chrono::steady_clock::now(); 16 | isStopwatchStarted_ = true; 17 | } 18 | 19 | double Stopwatch::getElapsedTimeSinceStartSeconds() const { 20 | std::chrono::steady_clock::time_point timeNow = std::chrono::steady_clock::now(); 21 | 22 | if (!isStopwatchStarted_) { 23 | throw std::runtime_error("Stopwatch hasn't been started"); 24 | } 25 | 26 | auto elapsedTimeMicroseconds = std::chrono::duration_cast(timeNow - startingTime_).count(); 27 | 28 | return static_cast(elapsedTimeMicroseconds) / 1e6; 29 | } 30 | 31 | } /* namespace pure_pursuit*/ 32 | -------------------------------------------------------------------------------- /pure_pursuit_core/src/common.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * common.cpp 3 | * 4 | * Created on: Mar 27, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #include "pure_pursuit_core/common.hpp" 9 | #include "pure_pursuit_core/Path.hpp" 10 | 11 | namespace pure_pursuit { 12 | 13 | std::string toString(DrivingDirection direction) { 14 | switch (direction) { 15 | case DrivingDirection::FWD: 16 | return "FWD"; 17 | case DrivingDirection::BCK: 18 | return "BCK"; 19 | } 20 | } 21 | 22 | RobotPose::RobotPose(double x, double y, double yaw) : position_(x, y), yaw_(yaw) {} 23 | 24 | std::ostream& operator<<(std::ostream& out, const RobotPose& robotPose) { 25 | const auto& p = robotPose.position_; 26 | out << "x =" << p.x() << ", y=" << p.y() << ", yaw=" << robotPose.yaw_; // actual output done here 27 | return out; 28 | } 29 | 30 | std::ostream& operator<<(std::ostream& out, const RobotState& robotState) { 31 | out << robotState.pose_; // actual output done here 32 | return out; 33 | } 34 | 35 | std::ostream& operator<<(std::ostream& out, const PathPoint& pathPoint) { 36 | const auto& p = pathPoint.position_; 37 | out << "x=" << p.x() << ", y=" << p.y(); 38 | return out; 39 | } 40 | 41 | std::ostream& operator<<(std::ostream& out, const PathSegment& pathSegment) { 42 | out << "direction=" << toString(pathSegment.drivingDirection_) << ", numPoints: " << pathSegment.point_.size() << "\n"; 43 | out << "Points: \n"; 44 | for (const auto& point : pathSegment.point_) { 45 | out << point << "\n"; 46 | } 47 | return out; 48 | } 49 | 50 | std::ostream& operator<<(std::ostream& out, const Path& path) { 51 | const int N = path.segment_.size(); 52 | out << "num segments: " << N << "\n"; 53 | for (int i = 0; i < N; ++i) { 54 | out << "segment: " << i << "\n"; 55 | out << path.segment_.at(i); 56 | } 57 | return out; 58 | } 59 | 60 | } /* namespace pure_pursuit */ 61 | -------------------------------------------------------------------------------- /pure_pursuit_core/test/PathTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PathTest.cpp 3 | * 4 | * Created on: Mar 19, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #include 9 | 10 | // Math 11 | #include 12 | #include "pure_pursuit_core/math.hpp" 13 | #include "test_helpers.hpp" 14 | 15 | namespace ppt = pure_pursuit_test; 16 | namespace pp = pure_pursuit; 17 | using SolutionCase = pp::Intersection::SolutionCase; 18 | 19 | constexpr unsigned int numCasesPerTest = 2000; 20 | constexpr unsigned int numPointInPathSegment = 10; 21 | 22 | TEST(Path, ExtendingPathSegment) { 23 | const int seed = ppt::seedRndGenerator(); 24 | std::uniform_real_distribution distanceDist(0.1, 50.0); 25 | for (unsigned int i = 0; i < numCasesPerTest; ++i) { 26 | ppt::PathSegment pathSegment = ppt::createRandomPathSegment(numPointInPathSegment); 27 | const unsigned int numPointsBefore = pathSegment.point_.size(); 28 | const double extendingDistance = distanceDist(ppt::rndGenerator); 29 | pp::appendPointAlongFinalApproachDirection(extendingDistance, &pathSegment); 30 | EXPECT_EQ(numPointsBefore + 1, pathSegment.point_.size()); 31 | int id = pathSegment.point_.size() - 1; 32 | const auto lastPoint = pathSegment.point_.at(id--).position_; 33 | const auto secondLastPoint = pathSegment.point_.at(id--).position_; 34 | const auto thirdLastPoint = pathSegment.point_.at(id).position_; 35 | EXPECT_TRUE(ppt::isVectorsColinear(lastPoint - secondLastPoint, secondLastPoint - thirdLastPoint)); 36 | } 37 | 38 | if (::testing::Test::HasFailure()) { 39 | std::cout << "\n Test Geometry, ExtendingPathSegment failed with seed: " << seed << std::endl; 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /pure_pursuit_core/test/test_helpers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_helpers.hpp 3 | * 4 | * Created on: Mar 19, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include "pure_pursuit_core/math.hpp" 12 | 13 | namespace pure_pursuit_test { 14 | 15 | using Circle = pure_pursuit::Circle; 16 | using Line = pure_pursuit::Line; 17 | using Vector = pure_pursuit::Vector; 18 | using Matrix = pure_pursuit::Matrix; 19 | using Point = pure_pursuit::Point; 20 | using RobotState = pure_pursuit::RobotState; 21 | using PathPoint = pure_pursuit::PathPoint; 22 | using PathSegment = pure_pursuit::PathSegment; 23 | 24 | extern std::mt19937 rndGenerator; 25 | constexpr double testPlaneWidth = 100.0; 26 | 27 | int seedRndGenerator(); 28 | Circle createRandomCircle(); 29 | Vector createUnitVectorPerpendicularToLine(const Line& line); 30 | Line createRandomLineWithoutIntersection(const Circle& circle); 31 | Line createRandomLineWitOneIntersection(const Circle& circle); 32 | Line createRandomLineWithTwoIntersections(const Circle& circle); 33 | Point createRandomPoint(); 34 | Point createRandomPointOutside(const Circle& circle); 35 | Point createRandomPointInside(const Circle& circle); 36 | PathSegment createRandomPathSegment(unsigned int numPoints); 37 | bool isVectorsColinear(const Vector& v1, const Vector& v2); 38 | 39 | } /* namespace pure_pursuit_test */ 40 | -------------------------------------------------------------------------------- /pure_pursuit_core/test/test_pure_pursuit.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_pure_pursuit.cpp 3 | * 4 | * Created on: Mar 19, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | // gtest 9 | #include 10 | 11 | // Run all the tests that were declared with TEST() 12 | int main(int argc, char** argv) { 13 | testing::InitGoogleTest(&argc, argv); 14 | srand((int)time(0)); 15 | return RUN_ALL_TESTS(); 16 | } 17 | -------------------------------------------------------------------------------- /pure_pursuit_ros/README.md: -------------------------------------------------------------------------------- 1 | # Pure pursuit ros 2 | 3 | Pure pursuit controller with ros extension for publishing additional messages. This package also supports dynamic 4 | reconfigure for tuning both heading and velocity controllers in real-time. 5 | 6 | An example of chaning the nominal velocity for adaptive velocity controller with dynamic reconfigure can be 7 | found [here](https://youtu.be/rVHYbIhDWc8). 8 | 9 | ## Dependencies 10 | 11 | * roscpp 12 | * eigen 13 | * pure_pursuit_core (in this repo) 14 | * se2_visualization_ros (in this repo) 15 | * yaml-cpp 16 | 17 | Pure pursuit ros depends on [yaml-cpp](https://github.com/jbeder/yaml-cpp) package for parameter loading. You can 18 | install it from source (see [instructinos](https://github.com/jbeder/yaml-cpp/blob/master/install.txt)) or you can 19 | install the latest version from PPA. 20 | 21 | `sudo apt install libyaml-cpp-dev` 22 | 23 | ## Installation 24 | 25 | Build with 26 | `catkin build pure_pursuit_ros` 27 | 28 | ## Usage 29 | 30 | To be used inside another project. 31 | 32 | ## Topics published 33 | 34 | * `pure_pursuit_heading_control/lookahead_point` - visualization of the lookahead point 35 | * `pure_pursuit_heading_control/anchor_point` - visualization of the anchor point 36 | * `pure_pursuit_heading_control/path_segment` - path segment the the controller is currently tracking 37 | * `simple_path_tracker_ros/path` - current path that the tracker has received 38 | * `simple_path_tracker_ros/robot_pose` - current pose of the robot inside the tracker 39 | 40 | -------------------------------------------------------------------------------- /pure_pursuit_ros/config/PurePursuit.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "pure_pursuit_ros" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("lookahead_fwd", double_t, 0, "Lookahead distance fwd (m)", 4.0, 0.0, 15.0) 9 | gen.add("lookahead_bck", double_t, 0, "Lookahead distance bck (m)", 4.0, 0.0, 15.0) 10 | gen.add("anchor_dist_fwd", double_t, 0, "anchor_dist_fwd (m)", 0.2, 0.0, 10.0) 11 | gen.add("anchor_dist_bck", double_t, 0, "anchor_dist_bck (m)", 0.2, 0.0, 10.0) 12 | gen.add("dead_zone_width", double_t, 0, "dead_zone_width (rad)", 0.0, -5.0, 5.0) 13 | gen.add("avg_filter_current_sample_weight", double_t, 0, "avg_filter_current_sample_weight", 0.7, 0.0, 1.0) 14 | 15 | gen.add("wheel_base", double_t, 0, "wheel_base (m)", 5.0, 0.0, 15.0) 16 | gen.add("max_steering_rate_of_change_in_deg_per_sec", double_t, 0, "max_steering_rate_of_change (deg)", 30, 0.0, 200.0) 17 | gen.add("max_steering_angle_magnitude_in_deg", double_t, 0, "max_steering_angle_magnitude (deg)", 100, 0.0, 360.0) 18 | 19 | exit(gen.generate(PACKAGE, "pure_pursuit_ros", "PurePursuit")) -------------------------------------------------------------------------------- /pure_pursuit_ros/config/PurePursuitAdaptiveVel.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "pure_pursuit_ros" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("nominal_velocity", double_t, 0, "wheel_base (m/2)", 1.0, 0.0, 15.0) 9 | gen.add("max_vel_rate_of_change", double_t, 0, "max_steering_rate_of_change (m/s2)", 0.5, 0.0, 20.0) 10 | gen.add("distance_when_braking_starts", double_t, 0, "max_steering_angle_magnitude (m)", 2.0, 0.0, 20.0) 11 | 12 | exit(gen.generate(PACKAGE, "pure_pursuit_ros", "PurePursuitAdaptiveVel")) -------------------------------------------------------------------------------- /pure_pursuit_ros/config/example.yaml: -------------------------------------------------------------------------------- 1 | velocity_control: 2 | constant_velocity_control: 3 | desired_velocity: 1.0 4 | max_rate_of_change: 0.3 5 | adaptive_velocity_control: 6 | nominal_velocity: 1.0 7 | max_rate_of_change: 0.3 8 | distance_when_braking_starts: 5.0 9 | 10 | heading_control: 11 | lookahead_fwd: 4.5 12 | lookahead_bck: 4.0 13 | anchor_dist_fwd: 1.0 14 | anchor_dist_bck: 0.5 15 | dead_zone_width: 0.0 16 | avg_filter_current_sample_weight: 0.1 17 | 18 | 19 | heading_control_ackermann: 20 | wheel_base: 5.0 # was 3.0 21 | max_steering_rate_of_change_in_deg_per_sec: 10 22 | max_steering_angle_magnitude_in_deg: 5 23 | 24 | path_tracking: 25 | simple_path_tracker: 26 | waiting_time_between_direction_changes: 2.5 27 | 28 | progress_validation: 29 | goal_distance_tolerance: 0.05 30 | 31 | path_preprocessing: 32 | minimal_segment_length: 1.0 33 | 34 | -------------------------------------------------------------------------------- /pure_pursuit_ros/include/pure_pursuit_ros/AckermannSteeringControllerRos.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AckermannSteeringControllerRos.hpp 3 | * 4 | * Created on: Mar 27, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include "pure_pursuit_core/heading_control/AckermannSteeringController.hpp" 14 | 15 | namespace pure_pursuit { 16 | 17 | class AckermannSteeringControllerRos : public AckermannSteeringController { 18 | using BASE = AckermannSteeringController; 19 | 20 | public: 21 | explicit AckermannSteeringControllerRos(ros::NodeHandle* nh); 22 | void setParameters(const AckermannSteeringCtrlParameters& parameters) override; 23 | 24 | private: 25 | void ddCallback(pure_pursuit_ros::PurePursuitConfig& config, uint32_t level); 26 | 27 | void initRos(); 28 | bool advanceImpl() override; 29 | void publishAnchorPoint() const; 30 | void publishPathSegment() const; 31 | void publishLookaheadPoint() const; 32 | void publishP1() const; 33 | void publishP2() const; 34 | 35 | ros::NodeHandle* nh_; 36 | ros::NodeHandle ddnh_; 37 | ros::Publisher lookaheadPointPub_; 38 | ros::Publisher anchorPointPub_; 39 | ros::Publisher pathSegmentPub_; 40 | 41 | ros::Publisher p1Pub_; 42 | ros::Publisher p2Pub_; 43 | Point p1_, p2_; 44 | boost::recursive_mutex ddMutex_; 45 | std::unique_ptr> ddServer_; 46 | dynamic_reconfigure::Server::CallbackType ddCalback_; 47 | pure_pursuit_ros::PurePursuitConfig ddConfig_; 48 | }; 49 | 50 | void updateFromDD(const pure_pursuit_ros::PurePursuitConfig& config, AckermannSteeringCtrlParameters* param); 51 | void updateDD(const AckermannSteeringCtrlParameters& param, pure_pursuit_ros::PurePursuitConfig* config); 52 | 53 | std::unique_ptr createAckermannSteeringControllerRos(const AckermannSteeringCtrlParameters& parameters, 54 | ros::NodeHandle* nh); 55 | 56 | } /* namespace pure_pursuit */ 57 | -------------------------------------------------------------------------------- /pure_pursuit_ros/include/pure_pursuit_ros/AdaptiveVelocityControllerRos.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ConstantVelocityControllerRos.hpp 3 | * 4 | * Created on: Jul 4, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include "pure_pursuit_core/velocity_control/AdaptiveVelocityController.hpp" 14 | 15 | namespace pure_pursuit { 16 | 17 | class AdaptiveVelocityControllerRos : public AdaptiveVelocityController { 18 | using BASE = AdaptiveVelocityController; 19 | 20 | public: 21 | explicit AdaptiveVelocityControllerRos(ros::NodeHandle* nh); 22 | void setParameters(const AdaptiveVelocityControllerParameters& parameters) override; 23 | 24 | private: 25 | void ddCallback(pure_pursuit_ros::PurePursuitAdaptiveVelConfig& config, uint32_t level); 26 | void initRos(); 27 | 28 | ros::NodeHandle* nh_; 29 | ros::NodeHandle ddnh_; 30 | 31 | boost::recursive_mutex ddMutex_; 32 | std::unique_ptr> ddServer_; 33 | dynamic_reconfigure::Server::CallbackType ddCalback_; 34 | pure_pursuit_ros::PurePursuitAdaptiveVelConfig ddConfig_; 35 | }; 36 | 37 | void updateFromDD(const pure_pursuit_ros::PurePursuitAdaptiveVelConfig& config, AdaptiveVelocityControllerParameters* param); 38 | void updateDD(const AdaptiveVelocityControllerParameters& param, pure_pursuit_ros::PurePursuitAdaptiveVelConfig* config); 39 | 40 | std::unique_ptr createAdaptiveVelocityControllerRos(const AdaptiveVelocityControllerParameters& parameters, 41 | ros::NodeHandle* nh); 42 | 43 | } /* namespace pure_pursuit*/ 44 | -------------------------------------------------------------------------------- /pure_pursuit_ros/include/pure_pursuit_ros/SimplePathTrackerRos.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SimplePathTrackerRos.hpp 3 | * 4 | * Created on: Mar 27, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include "pure_pursuit_core/path_tracking/SimplePathTracker.hpp" 12 | 13 | namespace pure_pursuit { 14 | 15 | class SimplePathTrackerRos : public SimplePathTracker { 16 | using BASE = SimplePathTracker; 17 | 18 | public: 19 | explicit SimplePathTrackerRos(ros::NodeHandle* nh); 20 | 21 | void importCurrentPath(const Path& path) override; 22 | void updateCurrentPath(const Path& path) override; 23 | 24 | private: 25 | bool advanceControllers() override; 26 | void initRos(); 27 | void publishPath(const Path& path) const; 28 | void publishRobotPose() const; 29 | 30 | ros::NodeHandle* nh_; 31 | ros::Publisher pathPub_; 32 | ros::Publisher robotPosePub_; 33 | }; 34 | 35 | std::unique_ptr createSimplePathTrackerRos(const SimplePathTrackerParameters& parameters, 36 | std::shared_ptr velocityController, 37 | std::shared_ptr headingController, 38 | std::shared_ptr validator, 39 | std::shared_ptr pathPreprocessor, ros::NodeHandle* nh); 40 | 41 | } /* namespace pure_pursuit */ 42 | -------------------------------------------------------------------------------- /pure_pursuit_ros/include/pure_pursuit_ros/loaders.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * loaders.hpp 3 | * 4 | * Created on: Mar 26, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include "pure_pursuit_core/heading_control/AckermannSteeringController.hpp" 11 | #include "pure_pursuit_core/path_tracking/PathPreprocessor.hpp" 12 | #include "pure_pursuit_core/path_tracking/ProgressValidator.hpp" 13 | #include "pure_pursuit_core/path_tracking/SimplePathTracker.hpp" 14 | #include "pure_pursuit_core/velocity_control/AdaptiveVelocityController.hpp" 15 | #include "pure_pursuit_core/velocity_control/ConstantVelocityController.hpp" 16 | 17 | namespace pure_pursuit { 18 | 19 | AckermannSteeringCtrlParameters loadAckermannSteeringControllerParameters(const std::string& filename); 20 | 21 | ConstantVelocityControllerParameters loadConstantVelocityControllerParameters(const std::string& filename); 22 | 23 | AdaptiveVelocityControllerParameters loadAdaptiveVelocityControllerParameters(const std::string& filename); 24 | 25 | SimplePathTrackerParameters loadSimplePathTrackerParameters(const std::string& filename); 26 | 27 | ProgressValidatorParameters loadProgressValidatorParameters(const std::string& filename); 28 | 29 | PathPreprocessorParameters loadPathPreprocessorParameters(const std::string& filename); 30 | 31 | } /* namespace pure_pursuit */ 32 | -------------------------------------------------------------------------------- /pure_pursuit_ros/launch/standalone_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /pure_pursuit_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pure_pursuit_ros 4 | 0.1.0 5 | Pure pursuit controller. 6 | Edo Jelavic 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | eigen 13 | pure_pursuit_core 14 | yaml-cpp 15 | se2_visualization_ros 16 | dynamic_reconfigure 17 | 18 | 19 | -------------------------------------------------------------------------------- /pure_pursuit_ros/src/dd_standalone_example.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * dd_standalone_example.cpp 3 | * 4 | * Created on: Jul 4, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | void callback(pure_pursuit_ros::PurePursuitConfig& config, uint32_t level) { 14 | ROS_INFO("Reconfigure Request: \n LookaheadDistance: %f ", config.lookahead_fwd); 15 | } 16 | 17 | int main(int argc, char** argv) { 18 | ros::init(argc, argv, "dd_standalone_example"); 19 | 20 | boost::recursive_mutex ddMutex; 21 | dynamic_reconfigure::Server server(ddMutex); 22 | dynamic_reconfigure::Server::CallbackType f; 23 | 24 | f = boost::bind(&callback, _1, _2); 25 | server.setCallback(f); 26 | ros::spinOnce(); 27 | ros::spinOnce(); 28 | pure_pursuit_ros::PurePursuitConfig dummyCfg; 29 | 30 | dummyCfg.lookahead_fwd = 1.258; 31 | server.updateConfig(dummyCfg); 32 | ros::spinOnce(); 33 | ROS_INFO("Now the lookahead_fwd should be %f in the rqt console", dummyCfg.lookahead_fwd); 34 | 35 | ROS_INFO("Spinning node"); 36 | ros::spin(); 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/doc/Approach_pose_conventions.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_approach_pose_planning/approach_pose_planner/doc/Approach_pose_conventions.pdf -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/doc/Approach_pose_conventions.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_approach_pose_planning/approach_pose_planner/doc/Approach_pose_conventions.png -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/doc/all_approach_poses.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_approach_pose_planning/approach_pose_planner/doc/all_approach_poses.png -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/doc/approach_planning_footprint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_approach_pose_planning/approach_pose_planner/doc/approach_planning_footprint.png -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/doc/both_criterions.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_approach_pose_planning/approach_pose_planner/doc/both_criterions.png -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/doc/driving_footprint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_approach_pose_planning/approach_pose_planner/doc/driving_footprint.png -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/doc/feasiblity_checking_convention.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_approach_pose_planning/approach_pose_planner/doc/feasiblity_checking_convention.pdf -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/doc/feasiblity_checking_convention.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_approach_pose_planning/approach_pose_planner/doc/feasiblity_checking_convention.png -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/doc/reachability_criterion_only.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_approach_pose_planning/approach_pose_planner/doc/reachability_criterion_only.png -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/include/approach_pose_planner/ApproachPoseHeuristic.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ApproachPoseHeuristics.hpp 3 | * 4 | * Created on: Oct 14, 2019 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include "se2_planning/State.hpp" 12 | 13 | namespace se2_planning { 14 | 15 | struct ApproachPoseHeuristicParameters { 16 | static constexpr double kDegToRad = M_PI / 180.0; 17 | double yawTolerance_ = 360.0 * kDegToRad; 18 | double lateralTolerance_ = 10.0; 19 | bool isAlwaysSatisfied_ = true; 20 | }; 21 | 22 | class ApproachPoseHeuristic { 23 | public: 24 | ApproachPoseHeuristic() = default; 25 | virtual ~ApproachPoseHeuristic() = default; 26 | 27 | virtual bool isHeuristicsSatisfied(const SE2state& approachPose) const; 28 | void setRobotCurrentPose(const SE2state& robotPose); 29 | void setTargetPosition(const XYstate& targetPosition); 30 | void setParameters(const ApproachPoseHeuristicParameters& params); 31 | 32 | protected: 33 | double getYawAngleDeviation(const SE2state& approachPose) const; 34 | double getLateralDeviation(const SE2state& approachPose) const; 35 | 36 | XYstate targetPosition_; 37 | SE2state currentPose_; 38 | ApproachPoseHeuristicParameters parameters_; 39 | }; 40 | 41 | } // namespace se2_planning 42 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/include/approach_pose_planner/ApproachStateValidator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ApproachStateVal idator.hpp 3 | * 4 | * Created on: Feb 17, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include "se2_planning/StateValidator.hpp" 10 | 11 | namespace se2_planning { 12 | 13 | class ApproachStateValidator : public StateValidator { 14 | public: 15 | ApproachStateValidator() = default; 16 | virtual ~ApproachStateValidator() = default; 17 | 18 | virtual void setTargetState(const State& targetState) { throw std::runtime_error("not implemented"); } 19 | }; 20 | 21 | class SE2approachStateValidator : public ApproachStateValidator { 22 | public: 23 | SE2approachStateValidator() = default; 24 | ~SE2approachStateValidator() override = default; 25 | bool isStateValid(const State& state) const override { return true; } 26 | void setTargetState(const State& targetState) override {} 27 | }; 28 | 29 | } /* namespace se2_planning */ 30 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/include/approach_pose_planner/LineOfSightApproachStateValidator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ApproachStateValidatorGridMap.hpp 3 | * 4 | * Created on: Feb 17, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include "approach_pose_planner/ApproachStateValidator.hpp" 10 | #include "approach_pose_planner/Parameters.hpp" 11 | #include "se2_planning/GridMapLazyStateValidator.hpp" 12 | 13 | namespace se2_planning { 14 | 15 | class LineOfSightApproachStateValidator : public ApproachStateValidator { 16 | public: 17 | LineOfSightApproachStateValidator() = default; 18 | virtual ~LineOfSightApproachStateValidator() = default; 19 | 20 | void initialize() override; 21 | bool isStateValid(const State& approachState) const override; 22 | bool isInitialized() const override; 23 | void setTargetState(const State& targetState) override; 24 | bool isLineOfSightCollisionFree(const State& approachState, const State& targetState) const; 25 | 26 | void setGridMap(const grid_map::GridMap& gridMap); 27 | void setFootprint(const RobotFootprint& footprint); 28 | void setObstacleLayerName(const std::string& layer); 29 | void setParameters(const LineOfSightApproachStateValidatorParameters& p); 30 | 31 | const grid_map::GridMap& getGridMap() const; 32 | const RobotFootprint& getFootprint() const; 33 | const std::string& getObstacleLayerName() const; 34 | const LineOfSightApproachStateValidatorParameters& getParameters() const; 35 | 36 | private: 37 | GridMapLazyStateValidator footprintValidator_; 38 | mutable GridMapStateValidator lineOfSightValidator_; 39 | SE2state targetState_; 40 | LineOfSightApproachStateValidatorParameters param_; 41 | }; 42 | 43 | } // namespace se2_planning 44 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/include/approach_pose_planner/Parameters.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Parameters.hpp 3 | * 4 | * Created on: Feb 10, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include "se2_planning/OmplReedsSheppPlanner.hpp" 11 | 12 | namespace se2_planning { 13 | 14 | struct ApproachPosePlannerParameters { 15 | double goalPolarAngleSpacing_ = 10 * M_PI / 180.0; 16 | double candidatePoseYawSpacing_ = 10 * M_PI / 180.0; 17 | double minGoalDistance_ = 1.0; 18 | double maxGoalDistance_ = 5.0; 19 | double distanceStep_ = 0.5; 20 | bool isPrintInfo_ = false; 21 | bool isPrintApproachPoseStatisctics_ = false; 22 | OmplReedsSheppPlannerParameters plannerImplParams_; 23 | }; 24 | 25 | struct LineOfSightApproachStateValidatorParameters { 26 | double lengthOfLineOfSightPortionAllowedToBeInCollisisonWithTarget_ = 0.5; 27 | double collisionCheckingAreaWidth_ = 1.2; 28 | bool isAssumeTargetAlwaysReachable_ = false; 29 | }; 30 | 31 | } /* namespace se2_planning*/ 32 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/include/approach_pose_planner/common.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * common.hpp 3 | * 4 | * Created on: Feb 10, 2021 5 | * Author: jelavice 6 | */ 7 | #pragma once 8 | 9 | #include 10 | 11 | namespace se2_planning { 12 | 13 | using Position = Eigen::Vector2d; 14 | 15 | enum FootprintVertices { LH = 0, LF, RF, RH }; 16 | 17 | } /* namespace se2_planning */ 18 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | approach_pose_planner 4 | 0.1.0 5 | Approach pose planner in 2D space. 6 | EJ 7 | BSD-3 8 | EJ 9 | 10 | catkin 11 | 12 | 13 | se2_planning 14 | ompl 15 | 16 | 17 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/src/ApproachPoseHeuristic.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ApproachPoseHeuristic.cpp 3 | * 4 | * Created on: Oct 14, 2019 5 | * Author: jelavice 6 | */ 7 | 8 | #include "approach_pose_planner/ApproachPoseHeuristic.hpp" 9 | #include 10 | 11 | namespace se2_planning { 12 | 13 | void ApproachPoseHeuristic::setTargetPosition(const XYstate& targetPosition) { 14 | targetPosition_ = targetPosition; 15 | } 16 | 17 | void ApproachPoseHeuristic::setRobotCurrentPose(const SE2state& robotPose) { 18 | currentPose_ = robotPose; 19 | } 20 | 21 | void ApproachPoseHeuristic::setParameters(const ApproachPoseHeuristicParameters& params) { 22 | parameters_ = params; 23 | } 24 | 25 | double ApproachPoseHeuristic::getYawAngleDeviation(const SE2state& approachPose) const { 26 | const double yaw = currentPose_.yaw_; 27 | Eigen::Vector2d heading(std::cos(yaw), std::sin(yaw)); 28 | Eigen::Vector2d headingApproachPose(std::cos(approachPose.yaw_), std::sin(approachPose.yaw_)); 29 | double dotProduct = heading.dot(headingApproachPose); 30 | double deviation = std::acos(dotProduct); 31 | return deviation; 32 | } 33 | double ApproachPoseHeuristic::getLateralDeviation(const SE2state& approachPose) const { 34 | const double yaw = currentPose_.yaw_; 35 | Eigen::Vector2d heading(std::cos(yaw), std::sin(yaw)); 36 | Eigen::Vector2d currentPoseToDesired(approachPose.x_ - currentPose_.x_, approachPose.y_ - currentPose_.y_); 37 | double projectionlength = heading.dot(currentPoseToDesired); 38 | double deviation = std::sqrt(currentPoseToDesired.squaredNorm() - projectionlength * projectionlength); 39 | return deviation; 40 | } 41 | 42 | bool ApproachPoseHeuristic::isHeuristicsSatisfied(const SE2state& approachPose) const { 43 | if (parameters_.isAlwaysSatisfied_) { 44 | return true; 45 | } 46 | 47 | if (getYawAngleDeviation(approachPose) > parameters_.yawTolerance_) { 48 | return false; 49 | } 50 | 51 | if (getLateralDeviation(approachPose) > parameters_.lateralTolerance_) { 52 | return false; 53 | } 54 | 55 | return true; 56 | } 57 | 58 | } // namespace se2_planning 59 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner/test/test_planning.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_planning.cpp 3 | * 4 | * Created on: Apr 10, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | // gtest 9 | #include 10 | 11 | // Run all the tests that were declared with TEST() 12 | int main(int argc, char** argv) { 13 | testing::InitGoogleTest(&argc, argv); 14 | srand((int)time(0)); 15 | return RUN_ALL_TESTS(); 16 | } 17 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(approach_pose_planner_msgs) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | 7 | set(CATKIN_PACKAGE_DEPENDENCIES 8 | geometry_msgs 9 | std_msgs 10 | se2_navigation_msgs 11 | ) 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | ${CATKIN_PACKAGE_DEPENDENCIES} 15 | message_generation 16 | ) 17 | 18 | 19 | add_message_files( 20 | DIRECTORY 21 | msg 22 | ) 23 | 24 | add_service_files( 25 | DIRECTORY 26 | srv 27 | ) 28 | 29 | generate_messages( 30 | DEPENDENCIES 31 | ${CATKIN_PACKAGE_DEPENDENCIES} 32 | ) 33 | 34 | catkin_package( 35 | CATKIN_DEPENDS 36 | ${CATKIN_PACKAGE_DEPENDENCIES} 37 | message_runtime 38 | ) 39 | 40 | include_directories( 41 | ${catkin_INCLUDE_DIRS} 42 | ) 43 | 44 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_msgs/README.md: -------------------------------------------------------------------------------- 1 | # approach\_pose\_planner\_msgs 2 | 3 | Collection of messages that are used to request paths from the approach pose planner. 4 | 5 | ## Messages 6 | ----------- 7 | 8 | #### Approach Pose Request (ApproachPoseRequestMsg.msg) 9 | 10 | + *geometry_msgs/Pose startingPose* - current robot pose 11 | + *geometry_msgs/Point goalPoint* - target location (x,y) where the robot should come close to 12 | 13 | ## Services 14 | ----------- 15 | 16 | #### Request Approach Pose (RequestPathSrv.srv) 17 | 18 | + **Request** 19 | + *ApproachPoseRequestMsg approachPoseRequest* 20 | + **Response** 21 | + *bool status* - whether an approach pose has been succesfully has been succesfully found 22 | 23 | ## Dependencies 24 | 25 | * std\_msgs 26 | 27 | * geometry\_msgs 28 | 29 | * se2\_navigation\_msgs 30 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_msgs/msg/ApproachPoseRequestMsg.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose startingPose 2 | geometry_msgs/Point goalPoint 3 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | approach_pose_planner_msgs 5 | 0.0.0 6 | Approach pose planner msgs 7 | 8 | jelavice 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | message_generation 15 | std_msgs 16 | geometry_msgs 17 | se2_navigation_msgs 18 | message_runtime 19 | 20 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_msgs/srv/RequestApproachPoseSrv.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | ApproachPoseRequestMsg approachPoseRequest 3 | --- 4 | # Response 5 | bool status -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/config/approach_pose_planner.yaml: -------------------------------------------------------------------------------- 1 | approach_pose_planner: 2 | polar_angle_spacing_around_goal: 15.0 #degrees 3 | candiate_pose_yaw_spacing: 15.0 #degrees 4 | min_goal_distance: 2.0 5 | max_goal_distance: 5.0 6 | distance_step: 0.25 7 | is_print_info: false 8 | is_approach_pose_planner_print_info: false 9 | is_print_approach_pose_statistics: false 10 | 11 | obstacle_layer: obstacle 12 | 13 | line_of_sight_validator: 14 | is_assume_target_always_reachable: true 15 | collision_checking_area_width: 1.2 16 | line_of_sight_length_allowed_to_be_in_collision: 0.5 17 | 18 | robot_footprint: 19 | length_forward: 3.0 20 | length_backwards: 3.0 21 | width_left: 1.2 22 | width_right: 1.2 23 | 24 | height_map: 25 | height_layer: elevation 26 | topic: grid_map 27 | 28 | state_space: 29 | x_lower: -100.0 30 | x_upper: 100.0 31 | y_lower: -100.0 32 | y_upper: 100.0 33 | turning_radius: 8.0 34 | 35 | approach_planner_ros: 36 | nav_msgs_path_topic: nav_msgs_path 37 | planning_service_name: approach_pose_planning_service 38 | path_msg_topic: nav_msgs_path 39 | path_frame: map 40 | nav_msg_path_spatial_resolution: 1.5 41 | is_publish_path_automatically: true 42 | is_publish_poses_and_target_automatically: true 43 | approach_poses_visualization_decimation_factor: 100 44 | pose_arrow_radius: 0.2 45 | pose_arrow_length: 2.0 46 | target_location_sphere_radius: 0.5 47 | 48 | planner: 49 | path_spatial_resolution: 0.05 50 | ompl_planner: RRTstar # see ompl_planner_creators.hpp for list of available planners 51 | max_planning_time: 1.0 52 | 53 | ompl_planners: 54 | rrt_star: 55 | planner_range: 15.0 56 | rrt_sharp: 57 | planner_range: 15.0 -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/config/grid_map_generation.yaml: -------------------------------------------------------------------------------- 1 | map: 2 | frame_id: map 3 | layers: [ elevation, obstacle ] 4 | default_values: [ 0.0, 0.0 ] 5 | resolution: 0.1 6 | position: 7 | x: 0.0 8 | y: 0.0 9 | length: 40.0 # x dimension 10 | width: 40.0 # y dimension 11 | topic: grid_map -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/doc/rviz_vis.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_approach_pose_planning/approach_pose_planner_ros/doc/rviz_vis.png -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/include/approach_pose_planner_ros/ApproachPosePlannerRos.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ReedsSheppApproachPosePlannerRos.hpp 3 | * 4 | * Created on: May 28, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include "approach_pose_planner/ApproachPosePlanner.hpp" 11 | #include "approach_pose_planner_msgs/RequestApproachPoseSrv.h" 12 | #include "approach_pose_planner_ros/Parameters.hpp" 13 | #include "se2_planning/HeightMap.hpp" 14 | 15 | namespace se2_planning { 16 | 17 | class ApproachPosePlannerRos : public ApproachPosePlanner { 18 | using Base = ApproachPosePlanner; 19 | using PlanningService = approach_pose_planner_msgs::RequestApproachPoseSrv; 20 | 21 | public: 22 | explicit ApproachPosePlannerRos(ros::NodeHandlePtr nh); 23 | explicit ApproachPosePlannerRos(const ros::NodeHandle& nh); 24 | 25 | ~ApproachPosePlannerRos() override = default; 26 | 27 | bool initialize() override; 28 | bool plan() override; 29 | void setParameters(const ApproachPosePlannerRosParam& parameters); 30 | void publishPath() const; 31 | void publishPathNavMsgs() const; 32 | void setHeightMap(const HeightMap& hm); 33 | void publishApproachPose() const; 34 | void publishApproachPoseCandidates() const; 35 | void publishTargetPosition() const; 36 | void publishStartingPose() const; 37 | const ApproachPosePlannerRosParam& getParameters() const; 38 | 39 | private: 40 | void initRos(); 41 | bool planningService(PlanningService::Request& req, PlanningService::Response& res); 42 | 43 | ros::Publisher pathNavMsgsPublisher_; 44 | ros::Publisher pathPublisher_; 45 | ros::Publisher approachPosePublisher_; 46 | ros::Publisher approachPoseCandidatesPublisher_; 47 | ros::Publisher targetPositionPublisher_; 48 | ros::Publisher startPosePublisher_; 49 | ApproachPosePlannerRosParam parameters_; 50 | ros::ServiceServer planningService_; 51 | int planSeqNumber_ = -1; 52 | ros::NodeHandle nh_; 53 | HeightMap heightMap_; 54 | const double heightAboveGround_ = 0.3; 55 | }; 56 | 57 | } // namespace se2_planning 58 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/include/approach_pose_planner_ros/Parameters.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Parameters.hpp 3 | * 4 | * Created on: Feb 12, 2021 5 | * Author: jelavice 6 | */ 7 | #pragma once 8 | #include 9 | #include "approach_pose_planner/Parameters.hpp" 10 | 11 | namespace se2_planning { 12 | 13 | struct ApproachPosePlannerRosParam : public ApproachPosePlannerParameters { 14 | std::string pathFrame_ = "map"; 15 | std::string pathNavMsgTopic_ = "nav_msgs_path"; 16 | std::string planningSerivceName_ = "approach_pose_planning_service"; 17 | std::string pathMsgTopic_ = "path"; 18 | double pathNavMsgResolution_ = 1.0; 19 | bool isPublishPathAutomatically_ = false; 20 | int approachPoseNumberDecimationForVisualizatoin_ = 10; 21 | double poseArrowMarkerRadius_ = 0.2; 22 | double poseArrowMarkerLength_ = 1.0; 23 | bool isPublishPosesAndTargetAutomatically_ = false; 24 | double targetSphereMarkerRadius_ = 0.5; 25 | }; 26 | 27 | } // namespace se2_planning 28 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/include/approach_pose_planner_ros/creators.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * creators.hpp 3 | * 4 | * Created on: Apr 21, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include 11 | #include "approach_pose_planner/ApproachStateValidator.hpp" 12 | #include "approach_pose_planner_ros/ApproachPosePlannerRos.hpp" 13 | #include "se2_planning/HeightMap.hpp" 14 | #include "se2_planning/StateValidator.hpp" 15 | 16 | namespace se2_planning { 17 | 18 | std::unique_ptr createMap(const ros::NodeHandle& nh); 19 | std::unique_ptr createStateValidator(const ros::NodeHandle& nh); 20 | bool createGridMap(const ros::NodeHandle& nh, grid_map::GridMap* gm); 21 | std::unique_ptr createApproachStateValidator(const ros::NodeHandle& nh); 22 | std::unique_ptr createStateValidator(const ros::NodeHandle& nh); 23 | std::unique_ptr createPlanner(const ros::NodeHandle& nh); 24 | 25 | } /* namespace se2_planning */ 26 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/launch/approach_pose_planner_ros.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/launch/generate_grid_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | approach_pose_planner_ros 4 | 0.1.0 5 | Approach pose planner in 2D space. 6 | EJ 7 | BSD-3 8 | EJ 9 | 10 | catkin 11 | 12 | 13 | roscpp 14 | nav_msgs 15 | approach_pose_planner 16 | se2_planning_ros 17 | yaml-cpp 18 | approach_pose_planner_msgs 19 | se2_visualization_ros 20 | grid_map_ros 21 | 22 | 23 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/scripts/generate_grid_map_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from se2_grid_map_generator_msgs.msg import CircularObstacle 4 | from se2_grid_map_generator_msgs.srv import SaveMap, AddCircularObstacle 5 | from se2_grid_map_generator_msgs.srv import SaveMapRequest, AddCircularObstacleRequest 6 | from std_msgs.msg import String, Float64 7 | 8 | addCircObstacleSrvName = "/se2_grid_map_generator_node/addCircularObstacle" 9 | saveMapSrvName = "/se2_grid_map_generator_node/saveMap" 10 | 11 | 12 | def saveMap(filename): 13 | try: 14 | saveMapSrv = rospy.ServiceProxy(saveMapSrvName, SaveMap) 15 | req = SaveMapRequest() 16 | req.filepath = filename 17 | saveMapSrv(req) 18 | except rospy.ServiceException as e: 19 | print("Service call failed: %s" % e) 20 | 21 | 22 | def addCircObstacle(x, y, r): 23 | try: 24 | addCircObstacleSrv = rospy.ServiceProxy(addCircObstacleSrvName, AddCircularObstacle) 25 | circObstacle = CircularObstacle() 26 | circObstacle.layers = [String('elevation'), String('obstacle')] 27 | circObstacle.values = [Float64(1), Float64(1)] 28 | circObstacle.circle.radius.data = r 29 | circObstacle.circle.center.x.data = x 30 | circObstacle.circle.center.y.data = y 31 | req = AddCircularObstacleRequest() 32 | req.obstacle = circObstacle 33 | addCircObstacleSrv(req) 34 | except rospy.ServiceException as e: 35 | print("Service call failed: %s" % e) 36 | 37 | 38 | rospy.init_node('grid_map_generator') 39 | # generate some circles 40 | rospy.wait_for_service(addCircObstacleSrvName) 41 | addCircObstacle(10, 0, 1) 42 | addCircObstacle(0, 10, 1) 43 | addCircObstacle(10, 15, 3) 44 | addCircObstacle(-5, -10, 4) 45 | addCircObstacle(-8, 8, 3) 46 | addCircObstacle(10, -15, 3) 47 | 48 | saveMap("") 49 | print("generating map node done") 50 | -------------------------------------------------------------------------------- /se2_approach_pose_planning/approach_pose_planner_ros/src/approach_pose_planner_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * approach_pose_planner_node.cpp 3 | * 4 | * Created on: Feb 12, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #include 9 | #include "approach_pose_planner_ros/creators.hpp" 10 | #include "approach_pose_planner_ros/loaders.hpp" 11 | #include "grid_map_msgs/GridMap.h" 12 | #include "grid_map_ros/GridMapRosConverter.hpp" 13 | 14 | namespace { 15 | ros::Publisher mapPub; 16 | 17 | void publishMap(const ros::NodeHandle& nh) { 18 | grid_map::GridMap gm; 19 | if (!se2_planning::createGridMap(nh, &gm)) { 20 | return; 21 | } 22 | grid_map_msgs::GridMap msg; 23 | grid_map::GridMapRosConverter::toMessage(gm, msg); 24 | mapPub.publish(msg); 25 | } 26 | } // namespace 27 | 28 | int main(int argc, char** argv) { 29 | using namespace se2_planning; 30 | ros::init(argc, argv, "approach_pose_planner_node"); 31 | ros::NodeHandle nh("~"); 32 | 33 | const auto gridMapTopic = loadSingleParam(nh, {"height_map", "topic"}); 34 | mapPub = nh.advertise(gridMapTopic, 1, true); 35 | 36 | auto planner = createPlanner(nh); 37 | publishMap(nh); 38 | 39 | ros::spin(); 40 | 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /se2_grid_map_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(se2_grid_map_generator) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 7 | 8 | set(SRC_FILES 9 | src/GridMapGenerator.cpp 10 | ) 11 | 12 | set(CATKIN_PACKAGE_DEPENDENCIES 13 | roscpp 14 | grid_map_ros 15 | se2_grid_map_generator_msgs 16 | ) 17 | 18 | find_package(catkin REQUIRED 19 | COMPONENTS 20 | ${CATKIN_PACKAGE_DEPENDENCIES} 21 | ) 22 | 23 | catkin_package( 24 | INCLUDE_DIRS 25 | include 26 | LIBRARIES 27 | CATKIN_DEPENDS 28 | ${CATKIN_PACKAGE_DEPENDENCIES} 29 | ) 30 | 31 | include_directories( 32 | include 33 | SYSTEM 34 | ${catkin_INCLUDE_DIRS} 35 | ) 36 | 37 | add_library(${PROJECT_NAME} 38 | ${SRC_FILES} 39 | ) 40 | 41 | add_dependencies(${PROJECT_NAME} 42 | ${catkin_EXPORTED_TARGETS} 43 | ) 44 | 45 | target_link_libraries(${PROJECT_NAME} 46 | ${catkin_LIBRARIES} 47 | ) 48 | 49 | add_executable(se2_grid_map_generator_node 50 | src/se2_grid_map_generator_node.cpp 51 | ) 52 | 53 | target_link_libraries(se2_grid_map_generator_node 54 | ${catkin_LIBRARIES} 55 | ${PROJECT_NAME} 56 | ) -------------------------------------------------------------------------------- /se2_grid_map_generator/config/default.yaml: -------------------------------------------------------------------------------- 1 | map: 2 | frame_id: odom 3 | layers: [ elevation, traversability ] 4 | default_values: [ 0.0, 1.0 ] 5 | resolution: 0.1 6 | position: 7 | x: 0.0 8 | y: 0.0 9 | length: 20.0 # x dimension 10 | width: 20.0 # y dimension 11 | topic: grid_map -------------------------------------------------------------------------------- /se2_grid_map_generator/data/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_grid_map_generator/data/.gitkeep -------------------------------------------------------------------------------- /se2_grid_map_generator/launch/se2_grid_map_generator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 15 | 16 | 17 | 18 | 21 | 22 | -------------------------------------------------------------------------------- /se2_grid_map_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | se2_grid_map_generator 5 | 0.0.0 6 | Package to easily generate test maps for the se2_navigation path planner. 7 | 8 | Christoph Meyer 9 | jelavice 10 | 11 | TODO 12 | 13 | catkin 14 | 15 | roscpp 16 | grid_map_ros 17 | se2_grid_map_generator_msgs 18 | 19 | 20 | -------------------------------------------------------------------------------- /se2_grid_map_generator/src/se2_grid_map_generator_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * se2_grid_map_generator_node.cpp 3 | * 4 | * Created on: Feb 4, 2021 5 | * Author: meyerc 6 | */ 7 | 8 | #include 9 | 10 | #include "se2_grid_map_generator/GridMapGenerator.hpp" 11 | 12 | // Start node on the command line and then use in a second terminal 13 | // rostopic pub /se2_grid_map_generator_node/obstacle geometry_msgs/Point "{x: 1.0, y: 0.0, z: 1.0}" -1 14 | // to trigger a new map, the z value defines the height of the obstacle (can only vary between 0 and 1). 15 | 16 | int main(int argc, char** argv) { 17 | using namespace se2_planning; 18 | 19 | ros::init(argc, argv, "se2_grid_map_generator_node"); 20 | ros::NodeHandlePtr nh(new ros::NodeHandle("~")); 21 | 22 | GridMapGenerator mapGenerator = GridMapGenerator(nh); 23 | mapGenerator.initialize(); 24 | 25 | ros::spin(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(se2_grid_map_generator_msgs) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | 7 | set(CATKIN_PACKAGE_DEPENDENCIES 8 | std_msgs 9 | ) 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | ${CATKIN_PACKAGE_DEPENDENCIES} 13 | message_generation 14 | ) 15 | 16 | 17 | add_message_files( 18 | DIRECTORY 19 | msg 20 | ) 21 | 22 | add_service_files( 23 | DIRECTORY 24 | srv 25 | ) 26 | 27 | generate_messages( 28 | DEPENDENCIES 29 | ${CATKIN_PACKAGE_DEPENDENCIES} 30 | ) 31 | 32 | catkin_package( 33 | INCLUDE_DIRS 34 | CATKIN_DEPENDS 35 | ${CATKIN_PACKAGE_DEPENDENCIES} 36 | message_runtime 37 | ) 38 | -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/msg/Circle2D.msg: -------------------------------------------------------------------------------- 1 | se2_grid_map_generator_msgs/Position2D center 2 | std_msgs/Float64 radius -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/msg/CircularObstacle.msg: -------------------------------------------------------------------------------- 1 | se2_grid_map_generator_msgs/Circle2D circle 2 | std_msgs/String[] layers 3 | std_msgs/Float64[] values -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/msg/Polygon2D.msg: -------------------------------------------------------------------------------- 1 | se2_grid_map_generator_msgs/Position2D[] vertices -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/msg/PolygonObstacle.msg: -------------------------------------------------------------------------------- 1 | se2_grid_map_generator_msgs/Polygon2D polygon 2 | std_msgs/String[] layers 3 | std_msgs/Float64[] values -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/msg/Position2D.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Float64 x 2 | std_msgs/Float64 y -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | se2_grid_map_generator_msgs 5 | 0.0.0 6 | Messages for planners and controllers. 7 | 8 | jelavice 9 | 10 | TODO 11 | 12 | catkin 13 | 14 | message_generation 15 | std_msgs 16 | message_runtime 17 | 18 | -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/srv/AddCircularObstacle.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | se2_grid_map_generator_msgs/CircularObstacle obstacle 3 | --- 4 | # Response 5 | bool success 6 | -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/srv/AddNan.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | se2_grid_map_generator_msgs/Polygon2D polygon 3 | --- 4 | # Response 5 | bool success 6 | -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/srv/AddPolygonObstacle.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | se2_grid_map_generator_msgs/PolygonObstacle obstacle 3 | --- 4 | # Response 5 | bool success 6 | -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/srv/ResetMap.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | --- 3 | # Response 4 | bool success 5 | -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/srv/SaveMap.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | string filepath 3 | --- 4 | # Response 5 | bool success 6 | string filepath -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/srv/SetUniformValue.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | std_msgs/String[] layers 3 | std_msgs/Float64[] values 4 | --- 5 | # Response 6 | bool success 7 | -------------------------------------------------------------------------------- /se2_grid_map_generator_msgs/srv/UpdateMapPosition.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | se2_grid_map_generator_msgs/Position2D position 3 | --- 4 | # Response 5 | bool success 6 | -------------------------------------------------------------------------------- /se2_navigation_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(se2_navigation_msgs) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | 7 | set(CATKIN_PACKAGE_DEPENDENCIES 8 | geometry_msgs 9 | std_msgs 10 | ) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | ${CATKIN_PACKAGE_DEPENDENCIES} 14 | message_generation 15 | ) 16 | 17 | 18 | add_message_files( 19 | DIRECTORY 20 | msg 21 | ) 22 | 23 | add_service_files( 24 | DIRECTORY 25 | srv 26 | ) 27 | 28 | generate_messages( 29 | DEPENDENCIES 30 | ${CATKIN_PACKAGE_DEPENDENCIES} 31 | ) 32 | 33 | catkin_package( 34 | INCLUDE_DIRS 35 | include 36 | CATKIN_DEPENDS 37 | ${CATKIN_PACKAGE_DEPENDENCIES} 38 | message_runtime 39 | ) 40 | 41 | include_directories( 42 | include 43 | ${catkin_INCLUDE_DIRS} 44 | ) 45 | 46 | install( 47 | DIRECTORY include/${PROJECT_NAME}/ 48 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 49 | FILES_MATCHING PATTERN "*.hpp" 50 | ) 51 | 52 | -------------------------------------------------------------------------------- /se2_navigation_msgs/README.md: -------------------------------------------------------------------------------- 1 | # se2\_navigation_msgs 2 | 3 | Collection of messages and services for communication between ros planners and controllers. These messages are also used 4 | to communicate to the rviz planning interface. 5 | 6 | ## Messages 7 | ----------- 8 | 9 | #### Path Segment (PathSegmentMsg.msg) 10 | 11 | + *int8 FORWARD = 0* 12 | + *int8 BACKWARDS = 1* 13 | + *int8 drivingDirection* - variable holding the driving direction 14 | + *geometry_msgs/Pose[] points* - poses of the robot along the path 15 | 16 | #### Path (PathMsg.msg) 17 | 18 | + *std_msgs/Header header* 19 | + *se2_navigation_msgs/PathSegmentMsg[] segment* - path segments 20 | 21 | #### Path Request (PathRequestMsg.msg) 22 | 23 | + *geometry_msgs/Pose startingPose* 24 | + *geometry_msgs/Pose goalPose* 25 | 26 | #### Controller Command (ControllerCommandMsg.msg) 27 | 28 | + *int8 START_TRACKING=0* 29 | + *int8 STOP_TRACKING=1* 30 | + *int8 command* 31 | 32 | ## Services 33 | ----------- 34 | 35 | #### Request Current State (RequestCurrentStateSrv.srv) 36 | 37 | + **Request** 38 | + `-` 39 | + **Response** 40 | + *geometry_msgs/Pose* - current pose of the robot 41 | + *geometry_msgs/Twist* - twist of the robot 42 | 43 | #### Request Path (RequestPathSrv.srv) 44 | 45 | + **Request** 46 | + *PathRequestMsg pathRequest* - starting and goal pose for the planner 47 | + **Response** 48 | + *bool status* - whether path has been succesfully found 49 | 50 | #### Send Controller Command (SendControllerCommandSrv.srv) 51 | 52 | + **Request** 53 | + *ControllerCommandMsg command* - command for the controller 54 | + **Response** 55 | + *bool status* - whether controller has done what was asked for 56 | 57 | ## Dependencies 58 | 59 | * std_msgs 60 | 61 | * geometry_msgs 62 | -------------------------------------------------------------------------------- /se2_navigation_msgs/include/se2_navigation_msgs/ControllerCommand.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ControllerCommand.hpp 3 | * 4 | * Created on: Apr 7, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include "se2_navigation_msgs/ControllerCommandMsg.h" 12 | 13 | namespace se2_navigation_msgs { 14 | 15 | struct ControllerCommand { 16 | enum class Command : int { StartTracking, StopTracking, NumCommands }; 17 | 18 | Command command_; 19 | }; 20 | 21 | inline ControllerCommandMsg convert(const ControllerCommand& msg) { 22 | ControllerCommandMsg rosMsg; 23 | 24 | rosMsg.command = static_cast(msg.command_); 25 | 26 | return rosMsg; 27 | } 28 | 29 | inline ControllerCommand convert(const ControllerCommandMsg& rosMsg) { 30 | ControllerCommand msg; 31 | 32 | switch (rosMsg.command) { 33 | case static_cast(ControllerCommand::Command::StartTracking): { 34 | msg.command_ = ControllerCommand::Command::StartTracking; 35 | break; 36 | } 37 | case static_cast(ControllerCommand::Command::StopTracking): { 38 | msg.command_ = ControllerCommand::Command::StopTracking; 39 | break; 40 | } 41 | 42 | default: { 43 | ROS_ERROR_STREAM("se2 Controller: unknown controller command: " << rosMsg.command); 44 | throw std::runtime_error("se2 Controller: unknown controller command"); 45 | } 46 | } 47 | 48 | return msg; 49 | } 50 | 51 | } /* namespace se2_navigation_msgs */ 52 | -------------------------------------------------------------------------------- /se2_navigation_msgs/include/se2_navigation_msgs/Path.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Path.hpp 3 | * 4 | * Created on: Apr 7, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include "se2_navigation_msgs/PathMsg.h" 13 | #include "se2_navigation_msgs/PathSegment.hpp" 14 | 15 | namespace se2_navigation_msgs { 16 | 17 | struct Path { 18 | std_msgs::Header header_; 19 | std::vector segment_; 20 | inline int numPoints() const; 21 | }; 22 | 23 | inline PathMsg convert(const Path& msg) { 24 | PathMsg rosMsg; 25 | rosMsg.segment.reserve(msg.segment_.size()); 26 | for (const auto& segment : msg.segment_) { 27 | rosMsg.segment.push_back(convert(segment)); 28 | } 29 | rosMsg.header = msg.header_; 30 | return rosMsg; 31 | } 32 | 33 | inline Path convert(const PathMsg& rosMsg) { 34 | Path msg; 35 | msg.segment_.reserve(rosMsg.segment.size()); 36 | for (const auto& segment : rosMsg.segment) { 37 | msg.segment_.push_back(convert(segment)); 38 | } 39 | msg.header_ = rosMsg.header; 40 | return msg; 41 | } 42 | 43 | inline int Path::numPoints() const { 44 | int retVal = 0; 45 | for (const auto& segment : segment_) { 46 | retVal += segment.points_.size(); 47 | } 48 | return retVal; 49 | } 50 | 51 | } /* namespace se2_navigation_msgs */ 52 | -------------------------------------------------------------------------------- /se2_navigation_msgs/include/se2_navigation_msgs/PathSegment.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PathSegment.hpp 3 | * 4 | * Created on: Apr 7, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include "se2_navigation_msgs/PathSegmentMsg.h" 13 | 14 | namespace se2_navigation_msgs { 15 | 16 | struct PathSegment { 17 | enum class DrivingDirection : int { Forward, Backwards, NumDirections }; 18 | 19 | DrivingDirection direction_; 20 | std::vector points_; 21 | }; 22 | 23 | inline PathSegmentMsg convert(const PathSegment& msg) { 24 | PathSegmentMsg rosMsg; 25 | 26 | rosMsg.drivingDirection = static_cast(msg.direction_); 27 | rosMsg.points = msg.points_; 28 | 29 | return rosMsg; 30 | } 31 | 32 | inline PathSegment convert(const PathSegmentMsg& rosMsg) { 33 | PathSegment msg; 34 | using DrivingDirection = PathSegment::DrivingDirection; 35 | 36 | switch (rosMsg.drivingDirection) { 37 | case static_cast(DrivingDirection::Forward): { 38 | msg.direction_ = DrivingDirection::Forward; 39 | break; 40 | } 41 | case static_cast(DrivingDirection::Backwards): { 42 | msg.direction_ = DrivingDirection::Backwards; 43 | break; 44 | } 45 | 46 | default: { 47 | ROS_ERROR_STREAM("se2 PathSegment: unknown driving direction: " << rosMsg.drivingDirection); 48 | throw std::runtime_error("se2 PathSegment: unknown driving direction"); 49 | } 50 | } 51 | 52 | msg.points_ = rosMsg.points; 53 | 54 | return msg; 55 | } 56 | 57 | } /* namespace se2_navigation_msgs */ 58 | -------------------------------------------------------------------------------- /se2_navigation_msgs/msg/ControllerCommandMsg.msg: -------------------------------------------------------------------------------- 1 | 2 | int8 START_TRACKING=0 3 | int8 STOP_TRACKING=1 4 | 5 | int8 command -------------------------------------------------------------------------------- /se2_navigation_msgs/msg/PathMsg.msg: -------------------------------------------------------------------------------- 1 | 2 | std_msgs/Header header 3 | se2_navigation_msgs/PathSegmentMsg[] segment -------------------------------------------------------------------------------- /se2_navigation_msgs/msg/PathRequestMsg.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose startingPose 2 | geometry_msgs/Pose goalPose 3 | -------------------------------------------------------------------------------- /se2_navigation_msgs/msg/PathSegmentMsg.msg: -------------------------------------------------------------------------------- 1 | 2 | 3 | int8 FORWARD = 0 4 | int8 BACKWARDS = 1 5 | 6 | int8 drivingDirection 7 | 8 | 9 | geometry_msgs/Pose[] points -------------------------------------------------------------------------------- /se2_navigation_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | se2_navigation_msgs 5 | 0.0.0 6 | Messages for planners and controllers. 7 | 8 | jelavice 9 | 10 | TODO 11 | 12 | catkin 13 | 14 | message_generation 15 | std_msgs 16 | geometry_msgs 17 | message_runtime 18 | 19 | -------------------------------------------------------------------------------- /se2_navigation_msgs/srv/RequestCurrentStateSrv.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | --- 3 | # Response 4 | geometry_msgs/Pose pose 5 | geometry_msgs/Twist twist -------------------------------------------------------------------------------- /se2_navigation_msgs/srv/RequestPathSrv.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | PathRequestMsg pathRequest # Query, maybe useful for the future 3 | --- 4 | # Response 5 | bool status -------------------------------------------------------------------------------- /se2_navigation_msgs/srv/SendControllerCommandSrv.srv: -------------------------------------------------------------------------------- 1 | # Request 2 | ControllerCommandMsg command # Query, maybe useful for the future 3 | --- 4 | # Response 5 | bool success -------------------------------------------------------------------------------- /se2_planning/doc/collision_footprint_conventions.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_planning/doc/collision_footprint_conventions.pdf -------------------------------------------------------------------------------- /se2_planning/doc/collision_footprint_conventions.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_planning/doc/collision_footprint_conventions.png -------------------------------------------------------------------------------- /se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapLazyStateValidator.hpp 3 | * 4 | * Created on: Apr 27, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include "grid_map_core/GridMap.hpp" 13 | #include "grid_map_core/Polygon.hpp" 14 | #include "se2_planning/GridMapStateValidator.hpp" 15 | 16 | namespace se2_planning { 17 | 18 | class GridMapLazyStateValidator : public GridMapStateValidator { 19 | using BASE = GridMapStateValidator; 20 | 21 | public: 22 | GridMapLazyStateValidator() = default; 23 | ~GridMapLazyStateValidator() override = default; 24 | 25 | bool isStateValid(const State& state) const final; 26 | void initialize() final; 27 | bool isInitialized() const final; 28 | 29 | void setIsUseRandomizedStrategy(bool value); 30 | bool getIsUseRandomizedStrategy() const; 31 | 32 | void setIsUseEarlyStoppingHeuristic(bool value); 33 | bool setIsUseEarlyStoppingHeuristic() const; 34 | 35 | void setSeed(int value); 36 | int getSeed() const; 37 | 38 | private: 39 | std::vector nominalFootprintPoints_; 40 | bool isInitializeCalled_ = false; 41 | bool isUseRandomizedStrategy_ = false; 42 | bool isUseEarlyStoppingHeuristic_ = false; 43 | int seed_ = 0; 44 | }; 45 | 46 | void computeFootprintPoints(const grid_map::GridMap& gridMap, const RobotFootprint& footprint, std::vector* footprintPoints); 47 | bool isInCollision(const SE2state& state, const std::vector& nominalFootprintPoints, const grid_map::GridMap& gridMap, 48 | const std::string& obstacleLayer); 49 | void addExtraPointsForEarlyStopping(const RobotFootprint& footprint, std::vector* points, int seed); 50 | std::unique_ptr createGridMapLazyStateValidator(const grid_map::GridMap& gridMap, 51 | const RobotFootprint& footprint, 52 | const std::string& obstacleLayer); 53 | 54 | } /* namespace se2_planning */ 55 | -------------------------------------------------------------------------------- /se2_planning/include/se2_planning/HeightMap.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * HeightMap.hpp 3 | * 4 | * Created on: Feb 17, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include "grid_map_core/GridMap.hpp" 10 | 11 | namespace se2_planning { 12 | 13 | class HeightMap { 14 | public: 15 | HeightMap() = default; 16 | virtual ~HeightMap() = default; 17 | 18 | bool isInitialized() const; 19 | 20 | double getHeightAt(double x, double y) const; 21 | 22 | void setGridMap(const grid_map::GridMap& gm, const std::string& heightLayer); 23 | 24 | const grid_map::GridMap& getGridMap() const; 25 | 26 | private: 27 | grid_map::GridMap impl_; 28 | std::string heightLayer_ = ""; 29 | bool isInitialized_ = false; 30 | }; 31 | 32 | } /* namespace se2_planning*/ 33 | -------------------------------------------------------------------------------- /se2_planning/include/se2_planning/Planner.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Planner.hpp 3 | * 4 | * Created on: Apr 1, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "se2_planning/State.hpp" 11 | 12 | #include 13 | namespace se2_planning { 14 | 15 | class Planner { 16 | public: 17 | Planner() = default; 18 | virtual ~Planner() = default; 19 | 20 | virtual void setStartingState(const State& startingState) = 0; 21 | virtual void setGoalState(const State& goalState) = 0; 22 | virtual bool plan() = 0; 23 | virtual void getPath(Path* path) const = 0; 24 | 25 | virtual bool reset() { throw std::runtime_error("Planner: reset() not implemented"); } 26 | virtual bool initialize() { throw std::runtime_error("Planner: initialize() not implemented"); } 27 | virtual void getStartingState(State* startingState) const { throw std::runtime_error("Planner: getStartingState() not implemented"); } 28 | virtual void getGoalState(State* goalState) const { throw std::runtime_error("Planner: getGoalState() not implemented"); } 29 | 30 | template 31 | const T* as() const { 32 | BOOST_CONCEPT_ASSERT((boost::Convertible)); 33 | return static_cast(this); 34 | } 35 | 36 | template 37 | T* as() { 38 | BOOST_CONCEPT_ASSERT((boost::Convertible)); 39 | return static_cast(this); 40 | } 41 | }; 42 | 43 | } /* namespace se2_planning */ 44 | -------------------------------------------------------------------------------- /se2_planning/include/se2_planning/State.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * State.hpp 3 | * 4 | * Created on: Apr 1, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | 11 | namespace se2_planning { 12 | 13 | struct State { 14 | template 15 | const T* as() const { 16 | BOOST_CONCEPT_ASSERT((boost::Convertible)); 17 | 18 | return static_cast(this); 19 | } 20 | 21 | template 22 | T* as() { 23 | BOOST_CONCEPT_ASSERT((boost::Convertible)); 24 | 25 | return static_cast(this); 26 | } 27 | 28 | protected: 29 | State() = default; 30 | virtual ~State() = default; 31 | }; 32 | 33 | struct Path { 34 | template 35 | const T* as() const { 36 | BOOST_CONCEPT_ASSERT((boost::Convertible)); 37 | 38 | return static_cast(this); 39 | } 40 | 41 | template 42 | T* as() { 43 | BOOST_CONCEPT_ASSERT((boost::Convertible)); 44 | 45 | return static_cast(this); 46 | } 47 | 48 | protected: 49 | Path() = default; 50 | virtual ~Path() = default; 51 | }; 52 | 53 | struct XYstate : public State { 54 | XYstate() = default; 55 | XYstate(double x, double y); 56 | ~XYstate() override = default; 57 | double x_ = 0.0; 58 | double y_ = 0.0; 59 | }; 60 | 61 | struct SE2state : public XYstate { 62 | SE2state() = default; 63 | SE2state(double x, double y, double yaw); 64 | ~SE2state() override = default; 65 | double yaw_ = 0.0; 66 | friend std::ostream& operator<<(std::ostream& out, const SE2state& rsState); 67 | friend bool operator==(const SE2state& s1, const SE2state& s2); 68 | }; 69 | 70 | } // namespace se2_planning 71 | -------------------------------------------------------------------------------- /se2_planning/include/se2_planning/StateValidator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StateValidator.hpp 3 | * 4 | * Created on: Apr 24, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include "se2_planning/State.hpp" 10 | 11 | namespace se2_planning { 12 | 13 | class StateValidator { 14 | public: 15 | StateValidator() = default; 16 | virtual ~StateValidator() = default; 17 | virtual bool isStateValid(const State& state) const = 0; 18 | virtual void initialize(); 19 | virtual bool isInitialized() const; 20 | }; 21 | 22 | class SE2stateValidator : public StateValidator { 23 | public: 24 | SE2stateValidator() = default; 25 | ~SE2stateValidator() override = default; 26 | bool isStateValid(const State& state) const override; 27 | }; 28 | 29 | } /* namespace se2_planning */ 30 | -------------------------------------------------------------------------------- /se2_planning/include/se2_planning/common.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Planner.hpp 3 | * 4 | * Created on: Dec 16, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | namespace se2_planning { 11 | 12 | template 13 | T bindToRange(const T value, const T low, const T high) { 14 | if (value < low) { 15 | return low; 16 | } else if (value > high) { 17 | return high; 18 | } else { 19 | return value; 20 | } 21 | } 22 | 23 | } /* namespace se2_planning */ -------------------------------------------------------------------------------- /se2_planning/include/se2_planning/ompl_planner_creators.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ompl_planner_creators.hpp 3 | * 4 | * Created on: May 1, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace se2_planning { 17 | 18 | enum class OmplPlanners : int { RRTstar, RRTsharp, BITstar, NUM_PLANNERS }; 19 | using BimapType = boost::bimap; 20 | const BimapType plannerKeyAndName = boost::assign::list_of(OmplPlanners::RRTstar, "RRTstar")( 21 | OmplPlanners::RRTsharp, "RRTsharp")(OmplPlanners::BITstar, "BITstar"); 22 | 23 | struct OmplPlannerParameters { 24 | virtual ~OmplPlannerParameters() = default; 25 | template 26 | const T* as() const { 27 | BOOST_CONCEPT_ASSERT((boost::Convertible)); 28 | return static_cast(this); 29 | } 30 | 31 | template 32 | T* as() { 33 | BOOST_CONCEPT_ASSERT((boost::Convertible)); 34 | return static_cast(this); 35 | } 36 | }; 37 | 38 | struct RRTstarParameters : public OmplPlannerParameters { 39 | double range_ = 10.0; 40 | }; 41 | 42 | struct RRTsharpParameters : public OmplPlannerParameters { 43 | double range_ = 10.0; 44 | }; 45 | ompl::base::PlannerPtr createPlanner(const ompl::base::SpaceInformationPtr& si, const std::string& plannerName); 46 | ompl::base::PlannerPtr createPlanner(const ompl::base::SpaceInformationPtr& si, OmplPlanners type); 47 | void setRRTstarParameters(const OmplPlannerParameters& params, ompl::base::PlannerPtr planner); 48 | void setRRTsharpParameters(const OmplPlannerParameters& params, ompl::base::PlannerPtr planner); 49 | void setPlannerParameters(const OmplPlannerParameters& params, OmplPlanners type, ompl::base::PlannerPtr planner); 50 | void setPlannerParameters(const OmplPlannerParameters& params, const std::string& plannerName, ompl::base::PlannerPtr planner); 51 | 52 | } /* namespace se2_planning */ 53 | -------------------------------------------------------------------------------- /se2_planning/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | se2_planning 4 | 0.1.0 5 | Pure pursuit controller. 6 | Edo Jelavic 7 | BSD 8 | 9 | catkin 10 | 11 | eigen 12 | ompl 13 | grid_map_core 14 | boost 15 | 16 | 17 | -------------------------------------------------------------------------------- /se2_planning/src/HeightMap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * HeightMap.cpp 3 | * 4 | * Created on: Feb 17, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | /* 9 | * HeightMap.hpp 10 | * 11 | * Created on: Feb 17, 2021 12 | * Author: jelavice 13 | */ 14 | 15 | #include "se2_planning/HeightMap.hpp" 16 | 17 | namespace se2_planning { 18 | 19 | bool HeightMap::isInitialized() const { 20 | return isInitialized_; 21 | } 22 | 23 | double HeightMap::getHeightAt(double x, double y) const { 24 | return impl_.atPosition(heightLayer_, grid_map::Position(x, y)); 25 | } 26 | 27 | void HeightMap::setGridMap(const grid_map::GridMap& gm, const std::string& heightLayer) { 28 | heightLayer_ = heightLayer; 29 | impl_ = gm; 30 | isInitialized_ = true; 31 | } 32 | 33 | const grid_map::GridMap& HeightMap::getGridMap() const { 34 | return impl_; 35 | } 36 | 37 | } /* namespace se2_planning*/ 38 | -------------------------------------------------------------------------------- /se2_planning/src/State.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * State.cpp 3 | * 4 | * Created on: Apr 24, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #include "se2_planning/State.hpp" 9 | 10 | #include 11 | 12 | namespace se2_planning { 13 | 14 | std::ostream& operator<<(std::ostream& out, const SE2state& state) { 15 | out << "x =" << state.x_ << ", y=" << state.y_ << ", yaw=" << state.yaw_; // actual output done here 16 | return out; 17 | } 18 | 19 | bool operator==(const SE2state& s1, const SE2state& s2) { 20 | const double tolerance = 1e-3; 21 | auto isEqual = [](double x, double y, double tolerance) { return std::fabs(x - y) <= tolerance; }; 22 | const bool isPositionEqual = isEqual(s1.x_, s2.x_, tolerance) && isEqual(s1.y_, s2.y_, tolerance); 23 | const bool isYawEqual = isEqual(s1.yaw_, s2.yaw_, tolerance); 24 | return isPositionEqual && isYawEqual; 25 | } 26 | 27 | SE2state::SE2state(double x, double y, double yaw) : XYstate(x, y), yaw_(yaw) {} 28 | XYstate::XYstate(double x, double y) : x_(x), y_(y) {} 29 | 30 | } /* namespace se2_planning */ 31 | -------------------------------------------------------------------------------- /se2_planning/src/StateValidator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StateValidator.cpp 3 | * 4 | * Created on: Apr 24, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #include "se2_planning/StateValidator.hpp" 9 | 10 | namespace se2_planning { 11 | 12 | void StateValidator::initialize() {} 13 | 14 | bool StateValidator::isInitialized() const { 15 | return true; 16 | } 17 | 18 | bool SE2stateValidator::isStateValid(const State& state) const { 19 | // implement something useful here 20 | return true; 21 | } 22 | 23 | } // namespace se2_planning 24 | -------------------------------------------------------------------------------- /se2_planning/test/test_helpers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_helpers.hpp 3 | * 4 | * Created on: Apr 10, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include "grid_map_core/GridMap.hpp" 14 | #include "se2_planning/OmplReedsSheppPlanner.hpp" 15 | #include "se2_planning/State.hpp" 16 | #include "se2_planning/StateValidator.hpp" 17 | 18 | namespace se2_planning_test { 19 | 20 | const std::string testLayer = "occupancy"; 21 | 22 | extern std::mt19937 rndGenerator; 23 | int seedRndGenerator(); 24 | double randomNumber(double lo, double hi); 25 | void addObstacles(std::function isObstacle, const std::string& layer, grid_map::GridMap* map); 26 | bool isInsideRectangle(double _x, double _y, double x0, double y0, double xLength, double yLength); 27 | se2_planning::SE2state randomState(const grid_map::GridMap& gm, double margin); 28 | grid_map::GridMap createGridMap(double length, double width, double resolution, std::function isAnObstacle); 29 | 30 | bool isPathCollisionFree(const se2_planning::ReedsSheppPath& path, const se2_planning::StateValidator& validator); 31 | 32 | bool isStartAndGoalStateOK(const se2_planning::ReedsSheppPath& path, const se2_planning::ReedsSheppState& start, 33 | const se2_planning::ReedsSheppState& goal); 34 | 35 | se2_planning::ReedsSheppState randomState(const se2_planning::OmplReedsSheppPlannerParameters& parameters); 36 | 37 | void setCostThreshold(se2_planning::OmplReedsSheppPlanner* planner); 38 | 39 | se2_planning::OmplReedsSheppPlannerParameters createRectangularStateSpaceWithDefaultParams(double stateBound); 40 | 41 | void setupPlanner(const se2_planning::OmplReedsSheppPlannerParameters& parameters, se2_planning::OmplReedsSheppPlanner* planner); 42 | 43 | bool simplePlanBetweenRandomStartAndGoalTest(se2_planning::OmplReedsSheppPlanner& planner, 44 | const se2_planning::OmplReedsSheppPlannerParameters& parameters); 45 | 46 | } /* namespace se2_planning_test */ 47 | -------------------------------------------------------------------------------- /se2_planning/test/test_planning.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_planning.cpp 3 | * 4 | * Created on: Apr 10, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | // gtest 9 | #include 10 | 11 | // Run all the tests that were declared with TEST() 12 | int main(int argc, char** argv) { 13 | testing::InitGoogleTest(&argc, argv); 14 | srand((int)time(0)); 15 | return RUN_ALL_TESTS(); 16 | } 17 | -------------------------------------------------------------------------------- /se2_planning_ros/config/reeds_shepp_planner_ros_example.yaml: -------------------------------------------------------------------------------- 1 | state_space: 2 | x_lower: -100.0 3 | x_upper: 100.0 4 | y_lower: -100.0 5 | y_upper: 100.0 6 | turning_radius: 10.0 7 | 8 | planner_ros: 9 | nav_msgs_path_topic: ompl_rs_planner_ros/nav_msgs_path 10 | planning_service_name: ompl_rs_planner_ros/planning_service 11 | path_msg_topic: ompl_rs_planner_ros/nav_msgs_path 12 | path_frame: map 13 | nav_msg_path_spatial_resolution: 1.5 14 | 15 | planner: 16 | path_spatial_resolution: 0.05 17 | ompl_planner: RRTstar # see ompl_planner_creators.hpp for list of available planners 18 | max_planning_time: 1.0 19 | 20 | ompl_planners: 21 | rrt_star: 22 | planner_range: 15.0 23 | rrt_sharp: 24 | planner_range: 15.0 25 | -------------------------------------------------------------------------------- /se2_planning_ros/doc/rviz_panel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_planning_ros/doc/rviz_panel.png -------------------------------------------------------------------------------- /se2_planning_ros/include/se2_planning_ros/OmplReedsSheppPlannerRos.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OmplReedsSheppPlannerRos.hpp 3 | * 4 | * Created on: Apr 2, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include "se2_navigation_msgs/Path.hpp" 14 | #include "se2_navigation_msgs/RequestPathSrv.h" 15 | #include "se2_planning/OmplReedsSheppPlanner.hpp" 16 | #include "se2_planning_ros/PlannerRos.hpp" 17 | 18 | namespace se2_planning { 19 | 20 | struct OmplReedsSheppPlannerRosParameters { 21 | std::string pathFrame_ = "map"; 22 | std::string pathNavMsgTopic_ = "ompl_rs_planner_ros/nav_msgs_path"; 23 | std::string planningSerivceName_ = "ompl_rs_planner_ros/planning_service"; 24 | std::string pathMsgTopic_ = "ompl_rs_planner_ros/path"; 25 | double pathNavMsgResolution_ = 1.0; 26 | }; 27 | 28 | class OmplReedsSheppPlannerRos : public PlannerRos { 29 | using BASE = PlannerRos; 30 | 31 | public: 32 | explicit OmplReedsSheppPlannerRos(ros::NodeHandlePtr nh); 33 | ~OmplReedsSheppPlannerRos() override = default; 34 | 35 | bool initialize() override; 36 | bool plan() override; 37 | void setParameters(const OmplReedsSheppPlannerRosParameters& parameters); 38 | void publishPath() const final; 39 | 40 | private: 41 | void initRos(); 42 | void publishPathNavMsgs() const; 43 | bool planningService(PlanningService::Request& req, PlanningService::Response& res) override; 44 | 45 | ros::Publisher pathNavMsgsPublisher_; 46 | ros::Publisher pathPublisher_; 47 | OmplReedsSheppPlannerRosParameters parameters_; 48 | ros::ServiceServer planningService_; 49 | int planSeqNumber_ = -1; 50 | }; 51 | 52 | } /* namespace se2_planning */ 53 | -------------------------------------------------------------------------------- /se2_planning_ros/include/se2_planning_ros/PlannerRos.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PlannerRos.hpp 3 | * 4 | * Created on: Apr 5, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include "se2_navigation_msgs/RequestPathSrv.h" 12 | #include "se2_planning/Planner.hpp" 13 | 14 | namespace se2_planning { 15 | 16 | class PlannerRos : public Planner { 17 | public: 18 | void setStartingState(const State& startingState) override; 19 | void setGoalState(const State& goalState) override; 20 | bool plan() override; 21 | void getPath(Path* path) const override; 22 | bool reset() override; 23 | bool initialize() override; 24 | void getStartingState(State* startingState) const override; 25 | void getGoalState(State* goalState) const override; 26 | 27 | void setPlanningStrategy(std::shared_ptr planner); 28 | virtual void publishPath() const; 29 | 30 | protected: 31 | using PlanningService = se2_navigation_msgs::RequestPathSrv; 32 | 33 | explicit PlannerRos(ros::NodeHandlePtr nh); 34 | ~PlannerRos() override = default; 35 | virtual bool planningService(PlanningService::Request& req, PlanningService::Response& res) = 0; 36 | 37 | ros::NodeHandlePtr nh_; 38 | std::shared_ptr planner_; 39 | }; 40 | 41 | } /* namespace se2_planning*/ 42 | -------------------------------------------------------------------------------- /se2_planning_ros/include/se2_planning_ros/common.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * common.hpp 3 | * 4 | * Created on: Feb 12, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include 11 | #include 12 | #include "se2_navigation_msgs/Path.hpp" 13 | #include "se2_navigation_msgs/RequestPathSrv.h" 14 | 15 | // todo separate this in the non ros package 16 | #include "se2_planning/OmplReedsSheppPlanner.hpp" 17 | 18 | namespace se2_planning { 19 | 20 | nav_msgs::Path copyAllPoints(const ReedsSheppPath& path); 21 | geometry_msgs::Pose convert(const ReedsSheppState& state, double z = 0.0); 22 | ReedsSheppState convert(const geometry_msgs::Pose& state); 23 | se2_navigation_msgs::Path convert(const ReedsSheppPath& path); 24 | 25 | } /* namespace se2_planning */ 26 | -------------------------------------------------------------------------------- /se2_planning_ros/include/se2_planning_ros/loaders.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * loaders.hpp 3 | * 4 | * Created on: Apr 3, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include "se2_planning/OmplReedsSheppPlanner.hpp" 10 | #include "se2_planning/ompl_planner_creators.hpp" 11 | #include "se2_planning_ros/OmplReedsSheppPlannerRos.hpp" 12 | 13 | namespace se2_planning { 14 | 15 | OmplReedsSheppPlannerParameters loadOmplReedsSheppPlannerParameters(const std::string& filename); 16 | OmplReedsSheppPlannerRosParameters loadOmplReedsSheppPlannerRosParameters(const std::string& filename); 17 | 18 | void loadOmplPlannerParameters(const std::string& plannerName, const std::string& filename, OmplPlannerParameters* params); 19 | void loadOmplPlannerParameters(OmplPlanners type, const std::string& filename, OmplPlannerParameters* params); 20 | void loadRRTstarParameters(const std::string& filename, RRTstarParameters* parameters); 21 | void loadRRTsharpParameters(const std::string& filename, RRTsharpParameters* parameters); 22 | 23 | } /* namespace se2_planning */ 24 | -------------------------------------------------------------------------------- /se2_planning_ros/launch/se2_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 11 | 12 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /se2_planning_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | se2_planning_ros 4 | 0.1.0 5 | Pure pursuit controller. 6 | Edo Jelavic 7 | BSD 8 | 9 | catkin 10 | 11 | eigen 12 | ompl 13 | se2_planning 14 | roscpp 15 | nav_msgs 16 | tf2 17 | yaml-cpp 18 | se2_navigation_msgs 19 | se2_planning_rviz 20 | 21 | 22 | -------------------------------------------------------------------------------- /se2_planning_ros/src/PlannerRos.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PlannerRos.cpp 3 | * 4 | * Created on: Apr 5, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #include "se2_planning_ros/PlannerRos.hpp" 9 | 10 | namespace se2_planning { 11 | 12 | PlannerRos::PlannerRos(ros::NodeHandlePtr nh) : nh_(nh) {} 13 | 14 | void PlannerRos::publishPath() const { 15 | throw std::runtime_error("Publish path not implemented"); 16 | } 17 | 18 | void PlannerRos::setPlanningStrategy(std::shared_ptr planner) { 19 | planner_ = planner; 20 | } 21 | 22 | void PlannerRos::setStartingState(const State& startingState) { 23 | planner_->setStartingState(startingState); 24 | } 25 | void PlannerRos::setGoalState(const State& goalState) { 26 | planner_->setGoalState(goalState); 27 | } 28 | bool PlannerRos::plan() { 29 | return planner_->plan(); 30 | } 31 | void PlannerRos::getPath(Path* path) const { 32 | planner_->getPath(path); 33 | } 34 | bool PlannerRos::reset() { 35 | return planner_->reset(); 36 | } 37 | bool PlannerRos::initialize() { 38 | return planner_->initialize(); 39 | } 40 | void PlannerRos::getStartingState(State* startingState) const { 41 | planner_->getStartingState(startingState); 42 | } 43 | void PlannerRos::getGoalState(State* goalState) const { 44 | planner_->getGoalState(goalState); 45 | } 46 | 47 | } // namespace se2_planning 48 | -------------------------------------------------------------------------------- /se2_planning_ros/src/se2_planner_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * se2_planner_node.cpp 3 | * 4 | * Created on: Apr 1, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #include 9 | 10 | #include "se2_planning_ros/loaders.hpp" 11 | 12 | int main(int argc, char** argv) { 13 | using namespace se2_planning; 14 | 15 | ros::init(argc, argv, "se2_planner_node"); 16 | ros::NodeHandlePtr nh(new ros::NodeHandle("~")); 17 | 18 | std::string filename = nh->param("/ompl_planner_ros/parameter_path", "ompl_rs_planner_ros/nav_msgs_path"); 19 | const auto plannerParameters = loadOmplReedsSheppPlannerParameters(filename); 20 | const auto plannerRosParameters = loadOmplReedsSheppPlannerRosParameters(filename); 21 | auto planner = std::make_shared(); 22 | planner->setParameters(plannerParameters); 23 | se2_planning::OmplReedsSheppPlannerRos plannerRos(nh); 24 | plannerRos.setPlanningStrategy(planner); 25 | plannerRos.setParameters(plannerRosParameters); 26 | plannerRos.initialize(); 27 | OmplPlannerParameters plannerOmplParameters; 28 | const std::string plannerName = plannerParameters.omplPlannerName_; 29 | loadOmplPlannerParameters(plannerName, filename, &plannerOmplParameters); 30 | auto omplPlanner = createPlanner(planner->getSimpleSetup()->getSpaceInformation(), plannerName); 31 | setPlannerParameters(plannerOmplParameters, plannerName, omplPlanner); 32 | planner->setOmplPlanner(omplPlanner); 33 | 34 | ros::spin(); 35 | 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /se2_planning_rviz/doc/adding_panel.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_planning_rviz/doc/adding_panel.gif -------------------------------------------------------------------------------- /se2_planning_rviz/doc/panel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/se2_navigation/6cd2ee0794ebaddc32a910e77c9d8f0f005b3456/se2_planning_rviz/doc/panel.png -------------------------------------------------------------------------------- /se2_planning_rviz/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | se2_planning_rviz 4 | 0.0.0 5 | Planning gui for the SE(2) space 6 | EJ 7 | BSD 8 | EJ 9 | 10 | catkin 11 | 12 | roscpp 13 | geometry_msgs 14 | tf2 15 | visualization_msgs 16 | rviz 17 | se2_visualization_ros 18 | se2_navigation_msgs 19 | approach_pose_planner_msgs 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /se2_planning_rviz/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel that allows goal point setting. 7 | 8 | 9 | -------------------------------------------------------------------------------- /se2_visualization_ros/README.md: -------------------------------------------------------------------------------- 1 | # se2\_navigation_msgs 2 | 3 | Helper functions shared across packages within se2_navigation. Mainly used for visualization purposes. 4 | 5 | ## Dependencies 6 | 7 | * roscpp (ros-melodic-roscpp) 8 | * eigen 9 | * visualization_msgs (ros-melodic-visualization-msgs) 10 | * geometry_msgs (ros-melodic-geometry-msgs) 11 | * tf2 (ros-melodic-tf2) 12 | * eigen_conversions (ros-melodic-eigen-conversions) 13 | 14 | ## Installation 15 | 16 | Build with: 17 | `catkin build se2_visualization_ros` 18 | 19 | ## Usage 20 | 21 | This package is not a standalone package. It is meant to be used within another package" -------------------------------------------------------------------------------- /se2_visualization_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | se2_visualization_ros 4 | 0.1.0 5 | Visualization helpers for planner and controller 6 | Edo Jelavic 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | eigen 13 | visualization_msgs 14 | geometry_msgs 15 | tf2 16 | eigen_conversions 17 | 18 | 19 | --------------------------------------------------------------------------------