├── src ├── 3_control │ ├── dependencies.rosinstall │ ├── control_meta │ │ ├── CMakeLists.txt │ │ ├── missions │ │ │ ├── autox.launch │ │ │ ├── scruti.launch │ │ │ ├── skidpad.launch │ │ │ ├── acceleration.launch │ │ │ └── trackdrive.launch │ │ └── package.xml │ └── control_pure_pursuit │ │ ├── launch │ │ └── pure_pursuit.launch │ │ ├── config │ │ └── pure_pursuit.yaml │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── src │ │ ├── main.cpp │ │ ├── pure_pursuit.cpp │ │ └── pure_pursuit_handle.cpp │ │ └── include │ │ ├── pure_pursuit_handle.hpp │ │ └── pure_pursuit.hpp ├── 1_perception │ ├── dependencies.rosinstall │ ├── perception_meta │ │ ├── missions │ │ │ ├── autox.launch │ │ │ ├── scruti.launch │ │ │ ├── skidpad.launch │ │ │ ├── acceleration.launch │ │ │ └── trackdrive.launch │ │ ├── CMakeLists.txt │ │ └── package.xml │ ├── lidar_cone_detector │ │ ├── config │ │ │ └── cone_detector.yaml │ │ ├── launch │ │ │ └── cone_detector.launch │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── src │ │ │ ├── main.cpp │ │ │ ├── cone_detector.cpp │ │ │ └── cone_detector_handle.cpp │ │ └── include │ │ │ ├── cone_detector.hpp │ │ │ └── cone_detector_handle.hpp │ └── vision_cone_detector │ │ ├── config │ │ └── cone_detector.yaml │ │ ├── launch │ │ └── cone_detector.launch │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── src │ │ ├── main.cpp │ │ ├── cone_detector.cpp │ │ └── cone_detector_handle.cpp │ │ └── include │ │ ├── cone_detector.hpp │ │ └── cone_detector_handle.hpp ├── 2_estimation │ ├── dependencies.rosinstall │ ├── velocity_estimator │ │ ├── config │ │ │ └── velocity_estimator.yaml │ │ ├── launch │ │ │ └── velocity_estimator.launch │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── src │ │ │ ├── main.cpp │ │ │ ├── velocity_estimator.cpp │ │ │ └── velocity_estimator_handle.cpp │ │ └── include │ │ │ ├── velocity_estimator.hpp │ │ │ └── velocity_estimator_handle.hpp │ ├── estimation_meta │ │ ├── CMakeLists.txt │ │ ├── missions │ │ │ ├── autox.launch │ │ │ ├── scruti.launch │ │ │ ├── skidpad.launch │ │ │ ├── trackdrive.launch │ │ │ └── acceleration.launch │ │ └── package.xml │ └── slam_slam │ │ ├── launch │ │ └── slam.launch │ │ ├── config │ │ └── slam.yaml │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── src │ │ ├── main.cpp │ │ ├── slam.cpp │ │ └── slam_handle.cpp │ │ └── include │ │ ├── slam.hpp │ │ └── slam_handle.hpp ├── 0_fsd_common │ ├── fsd_common_msgs │ │ ├── msg │ │ │ ├── Mission.msg │ │ │ ├── ConeDetections.msg │ │ │ ├── CarStateDt.msg │ │ │ ├── Map.msg │ │ │ ├── Cone.msg │ │ │ ├── CarState.msg │ │ │ └── ControlCommand.msg │ │ ├── package.xml │ │ └── CMakeLists.txt │ └── fsd_common_meta │ │ ├── CMakeLists.txt │ │ ├── missions │ │ ├── sim_trackdrive.launch │ │ ├── autox.launch │ │ ├── scruti.launch │ │ ├── skidpad.launch │ │ ├── trackdrive.launch │ │ └── acceleration.launch │ │ ├── visualization │ │ ├── rviz_autox.launch │ │ ├── rviz_skidpad.launch │ │ ├── rviz_trackdrive.launch │ │ ├── rviz_acceleration.launch │ │ └── rviz │ │ │ ├── autox.rviz │ │ │ ├── skidpad.rviz │ │ │ ├── acceleration.rviz │ │ │ └── trackdrive.rviz │ │ └── package.xml ├── 4_continuous_integration │ └── rosdep │ │ ├── 10-fsd-dependencies.list │ │ └── fsd-dependencies.yaml └── fssim_interface │ ├── dependencies.rosinstall │ ├── launch │ ├── record_sim.launch │ ├── rviz.launch │ ├── only_interface.launch │ └── fssim.launch │ ├── mission_launch │ └── trackdrive.launch │ ├── package.xml │ ├── README.md │ ├── config │ └── config.yaml │ ├── CMakeLists.txt │ ├── fssim_config │ ├── local_simulation.yaml │ ├── ats_simulation.yaml │ └── sensors │ │ └── sensors_1.yaml │ ├── include │ └── interface.hpp │ ├── src │ └── fssim_interface_node.cpp │ └── rviz │ ├── trackdrive.rviz │ └── fssim.xml ├── .gitignore ├── fsd_environment.sh ├── fsd_aliases ├── README.md └── update_dependencies.sh /src/3_control/dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/1_perception/dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/2_estimation/dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/1_perception/perception_meta/missions/autox.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/1_perception/perception_meta/missions/scruti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/1_perception/perception_meta/missions/skidpad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_msgs/msg/Mission.msg: -------------------------------------------------------------------------------- 1 | # Header 2 | std_msgs/Header header 3 | string mission 4 | bool finished -------------------------------------------------------------------------------- /src/1_perception/perception_meta/missions/acceleration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_msgs/msg/ConeDetections.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | fsd_common_msgs/Cone[] cone_detections -------------------------------------------------------------------------------- /src/4_continuous_integration/rosdep/10-fsd-dependencies.list: -------------------------------------------------------------------------------- 1 | yaml file:///etc/ros/rosdep/sources.list.d/fsd-dependencies.yaml 2 | -------------------------------------------------------------------------------- /src/fssim_interface/dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: src/fssim 3 | uri: https://github.com/AMZ-Driverless/fssim.git 4 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_msgs/msg/CarStateDt.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | geometry_msgs/Pose2D car_state_dt # Velocity in x, y, theta -------------------------------------------------------------------------------- /src/1_perception/lidar_cone_detector/config/cone_detector.yaml: -------------------------------------------------------------------------------- 1 | 2 | cone_detections_topic_name: /perception/lidar/cone_detections 3 | 4 | node_rate: 1 # [Herz] -------------------------------------------------------------------------------- /src/1_perception/vision_cone_detector/config/cone_detector.yaml: -------------------------------------------------------------------------------- 1 | 2 | cone_detections_topic_name: /perception/vision/cone_detections 3 | 4 | node_rate: 1 # [Herz] -------------------------------------------------------------------------------- /src/3_control/control_meta/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(control_meta) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_package() -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_msgs/msg/Map.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | fsd_common_msgs/Cone[] cone_yellow 4 | fsd_common_msgs/Cone[] cone_blue 5 | fsd_common_msgs/Cone[] cone_orange -------------------------------------------------------------------------------- /src/2_estimation/velocity_estimator/config/velocity_estimator.yaml: -------------------------------------------------------------------------------- 1 | 2 | velocity_estimation_topic_name: /estimation/velocity_estimation/velocity_estimate 3 | 4 | node_rate: 5 # [Herz] -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(fsd_common_meta) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_package() -------------------------------------------------------------------------------- /src/1_perception/perception_meta/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(perception_meta) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_package() -------------------------------------------------------------------------------- /src/2_estimation/estimation_meta/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(estimation_meta) 3 | 4 | find_package(catkin REQUIRED COMPONENTS) 5 | 6 | catkin_package() -------------------------------------------------------------------------------- /src/3_control/control_meta/missions/autox.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/3_control/control_meta/missions/scruti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/3_control/control_meta/missions/skidpad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/missions/sim_trackdrive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_msgs/msg/Cone.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point position # coordinate of cone in [x, y] 2 | std_msgs/String color # color of cone, 'b' = blue, 'y' = yellow, 'o' = orange -------------------------------------------------------------------------------- /src/3_control/control_meta/missions/acceleration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/3_control/control_meta/missions/trackdrive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/visualization/rviz_autox.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_msgs/msg/CarState.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | geometry_msgs/Pose2D car_state # Position in x, y, theta 4 | fsd_common_msgs/CarStateDt car_state_dt # Velocities -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/visualization/rviz_skidpad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/visualization/rviz_trackdrive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/visualization/rviz_acceleration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_msgs/msg/ControlCommand.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | std_msgs/Float32 throttle # Throttle value between [-1, 1] 4 | std_msgs/Float32 steering_angle # Steering angle value between [-1, 1], where left < 0, right > 0 -------------------------------------------------------------------------------- /src/2_estimation/estimation_meta/missions/autox.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/2_estimation/estimation_meta/missions/scruti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/2_estimation/estimation_meta/missions/skidpad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/2_estimation/estimation_meta/missions/trackdrive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/1_perception/perception_meta/missions/trackdrive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/2_estimation/estimation_meta/missions/acceleration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/2_estimation/slam_slam/launch/slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/fssim_interface/launch/record_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/missions/autox.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/missions/scruti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/missions/skidpad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/fssim_interface/mission_launch/trackdrive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/missions/trackdrive.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/missions/acceleration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/fssim_interface/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/1_perception/lidar_cone_detector/launch/cone_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/3_control/control_pure_pursuit/launch/pure_pursuit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/1_perception/vision_cone_detector/launch/cone_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/2_estimation/velocity_estimator/launch/velocity_estimator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/fssim_interface/launch/only_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/2_estimation/slam_slam/config/slam.yaml: -------------------------------------------------------------------------------- 1 | 2 | vision_cone_detections_topic_name: /perception/vision/cone_detections 3 | lidar_cone_detections_topic_name: /perception/lidar/cone_detections 4 | velocity_estimate_topic_name: /estimation/velocity_estimation/velocity_estimate 5 | slam_map_topic_name: /estimation/slam/map 6 | slam_state_topic_name: /estimation/slam/state 7 | 8 | max_map_size: 25 9 | 10 | node_rate: 1 # [Herz] -------------------------------------------------------------------------------- /src/3_control/control_pure_pursuit/config/pure_pursuit.yaml: -------------------------------------------------------------------------------- 1 | velocity_estimate_topic_name: /estimation/velocity_estimation/velocity_estimate 2 | slam_map_topic_name: /estimation/slam/map 3 | slam_state_topic_name: /estimation/slam/state 4 | control_command_topic_name: /control/pure_pursuit/control_command 5 | center_line_topic_name: /control/pure_pursuit/center_line 6 | 7 | max_speed: 4 # [m/s] 8 | 9 | node_rate: 10 # [Herz] 10 | 11 | controller: 12 | speed: 13 | p: 0.03 14 | steering: 15 | p: 1.0 -------------------------------------------------------------------------------- /src/4_continuous_integration/rosdep/fsd-dependencies.yaml: -------------------------------------------------------------------------------- 1 | fsd-python-wstool: 2 | ubuntu: [python-wstool] 3 | fsd-checkinstall: 4 | ubuntu: [checkinstall] 5 | fsd-python-pip: 6 | ubuntu: [python-pip] 7 | fsd-python-catkin-tools: 8 | ubuntu: [python-catkin-tools] 9 | fsd-libcgal-qt5-dev: 10 | ubuntu: [libcgal-qt5-dev] 11 | fsd-python-rospkg: 12 | ubuntu: [python-rospkg] 13 | fsd-python-empy: 14 | ubuntu: [python-empy] 15 | fsd-libgsl-dev: 16 | ubuntu: [libgsl-dev] 17 | fsd-python-git: 18 | ubuntu: [python-git] 19 | -------------------------------------------------------------------------------- /src/fssim_interface/launch/fssim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /src/3_control/control_meta/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | control_meta 4 | 0.0.0 5 | Meta package for control 6 | Sonja Brits 7 | TODO 8 | Sonja Brits 9 | 10 | 11 | catkin 12 | 13 | 14 | control_pure_pursuit 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore catkin generated folders 2 | devel/ 3 | devel/* 4 | build/ 5 | build/* 6 | logs/ 7 | logs/* 8 | .rosinstall 9 | .rosinstall.bak 10 | 11 | # Ignore all Rosbags 12 | **.bag 13 | # Ignore all images 14 | **.jpg 15 | **.png 16 | **.jpeg 17 | 18 | # Others 19 | .idea/ 20 | .idea/* 21 | cmake-build-debug/ 22 | cmake-build-debug/* 23 | cmake-build-release/ 24 | cmake-build-release/* 25 | clion/ 26 | clion-cmake/ 27 | clion-cmake/* 28 | 29 | # Catkin stuff 30 | .catkin_tools/* 31 | .catkin_tools/ 32 | 33 | **.swp 34 | 35 | # binary files 36 | *.pyc 37 | 38 | # Profiler files 39 | *.prof 40 | -------------------------------------------------------------------------------- /fsd_environment.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CATKIN_SHELL=bash 4 | 5 | export FSD_ROOT=$( cd "$(dirname "${BASH_SOURCE[0]}" )" && pwd) 6 | 7 | printf "Sourcing fsd_skeleton/devel/setup.bash\n" 8 | 9 | # check whether devel folder exists 10 | if [ -f "${FSD_ROOT}/devel/setup.bash" ]; then 11 | # source setup.sh from same directory as this file 12 | source "${FSD_ROOT}/devel/setup.bash" 13 | else 14 | source "/opt/ros/kinetic/setup.bash" 15 | printf "You need to build first before you can source\n" 16 | printf "Run 'catkin build' in the skeleton_repo directory\n" 17 | fi 18 | 19 | source ${FSD_ROOT}/fsd_aliases 20 | -------------------------------------------------------------------------------- /src/2_estimation/estimation_meta/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | estimation_meta 4 | 0.0.0 5 | Meta package for estimation 6 | Sonja Brits 7 | TODO 8 | Sonja Brits 9 | 10 | 11 | catkin 12 | 13 | 14 | slam_slam 15 | velocity_estimator 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/1_perception/perception_meta/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | perception_meta 4 | 0.0.0 5 | Meta package for perception 6 | Sonja Brits 7 | TODO 8 | Sonja Brits 9 | 10 | 11 | catkin 12 | 13 | 14 | lidar_cone_detector 15 | vision_cone_detector 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/1_perception/lidar_cone_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_cone_detector 4 | 0.0.0 5 | Simple dummy cone detector, that outputs "cone detections" at constant rate 6 | Sonja Brits 7 | TODO 8 | 9 | 10 | catkin 11 | 12 | 13 | fsd_common_msgs 14 | 15 | 16 | roscpp 17 | std_msgs 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /src/2_estimation/velocity_estimator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velocity_estimator 4 | 0.0.0 5 | Simple velocity estimator, that outputs "velocity estimation" at constant rate 6 | Sonja Brits 7 | TODO 8 | 9 | 10 | catkin 11 | 12 | 13 | fsd_common_msgs 14 | 15 | 16 | roscpp 17 | std_msgs 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /src/1_perception/vision_cone_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vision_cone_detector 4 | 0.0.0 5 | Simple dummy cone detector, that outputs "cone detections" at constant rate 6 | Sonja Brits 7 | TODO 8 | 9 | 10 | catkin 11 | 12 | 13 | fsd_common_msgs 14 | 15 | 16 | roscpp 17 | std_msgs 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fsd_common_meta 5 | 0.0.0 6 | Meta package for car launching 7 | Sonja Brits 8 | TODO 9 | Sonja Brits 10 | 11 | 12 | catkin 13 | 14 | perception_meta 15 | estimation_meta 16 | control_meta 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/2_estimation/slam_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | slam_slam 4 | 0.0.0 5 | Simple SLAM node, that inputs "cone detections" and outputs array of points 6 | Sonja Brits 7 | TODO 8 | 9 | 10 | catkin 11 | 12 | 13 | fsd_common_msgs 14 | 15 | 16 | roscpp 17 | std_msgs 18 | geometry_msgs 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/3_control/control_pure_pursuit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | control_pure_pursuit 4 | 0.0.0 5 | Simple pure pursuit control node, that inputs pose, map and velocity estimate, and outputs control command 6 | Sonja Brits 7 | TODO 8 | 9 | 10 | catkin 11 | 12 | 13 | fsd_common_msgs 14 | 15 | 16 | roscpp 17 | std_msgs 18 | geometry_msgs 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/fssim_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fssim_interface 5 | 0.0.0 6 | Remmaper for FSSIM 7 | Juraj Kabzan 8 | Juraj Kabzan 9 | MIT 10 | 11 | catkin 12 | 13 | 14 | roscpp 15 | roscpp 16 | roscpp 17 | 18 | nav_msgs 19 | fsd_common_msgs 20 | tf 21 | fssim 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/2_estimation/slam_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(slam_slam) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | set(PROJECT_DEPS 7 | roscpp 8 | std_msgs 9 | fsd_common_msgs 10 | ) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | geometry_msgs 16 | fsd_common_msgs 17 | ) 18 | 19 | catkin_package( 20 | INCLUDE_DIRS 21 | LIBRARIES 22 | CATKIN_DEPENDS 23 | DEPENDS 24 | ) 25 | 26 | include_directories( 27 | include 28 | ${catkin_INCLUDE_DIRS} 29 | ${roscpp_INCLUDE_DIRS} 30 | ) 31 | 32 | # Each node in the package must be declared like this 33 | add_executable(${PROJECT_NAME} 34 | src/slam_handle.cpp 35 | src/slam.cpp 36 | src/main.cpp 37 | ) 38 | target_link_libraries(${PROJECT_NAME} 39 | ${catkin_LIBRARIES} 40 | ) 41 | -------------------------------------------------------------------------------- /src/1_perception/lidar_cone_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lidar_cone_detector) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | set(PROJECT_DEPS 7 | roscpp 8 | std_msgs 9 | fsd_common_msgs 10 | ) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | fsd_common_msgs 16 | ) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS 20 | LIBRARIES 21 | CATKIN_DEPENDS 22 | DEPENDS 23 | ) 24 | 25 | include_directories( 26 | include 27 | ${catkin_INCLUDE_DIRS} 28 | ${roscpp_INCLUDE_DIRS} 29 | ) 30 | 31 | # Each node in the package must be declared like this 32 | add_executable(${PROJECT_NAME} 33 | src/cone_detector_handle.cpp 34 | src/cone_detector.cpp 35 | src/main.cpp 36 | ) 37 | target_link_libraries(${PROJECT_NAME} 38 | ${catkin_LIBRARIES} 39 | ) 40 | -------------------------------------------------------------------------------- /src/1_perception/vision_cone_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vision_cone_detector) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | set(PROJECT_DEPS 7 | roscpp 8 | std_msgs 9 | fsd_common_msgs 10 | ) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | fsd_common_msgs 16 | ) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS 20 | LIBRARIES 21 | CATKIN_DEPENDS 22 | DEPENDS 23 | ) 24 | 25 | include_directories( 26 | include 27 | ${catkin_INCLUDE_DIRS} 28 | ${roscpp_INCLUDE_DIRS} 29 | ) 30 | 31 | # Each node in the package must be declared like this 32 | add_executable(${PROJECT_NAME} 33 | src/cone_detector_handle.cpp 34 | src/cone_detector.cpp 35 | src/main.cpp 36 | ) 37 | target_link_libraries(${PROJECT_NAME} 38 | ${catkin_LIBRARIES} 39 | ) 40 | -------------------------------------------------------------------------------- /src/2_estimation/velocity_estimator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(velocity_estimator) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | set(PROJECT_DEPS 7 | roscpp 8 | std_msgs 9 | fsd_common_msgs 10 | ) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | fsd_common_msgs 16 | ) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS 20 | LIBRARIES 21 | CATKIN_DEPENDS 22 | DEPENDS 23 | ) 24 | 25 | include_directories( 26 | include 27 | ${catkin_INCLUDE_DIRS} 28 | ${roscpp_INCLUDE_DIRS} 29 | ) 30 | 31 | # Each node in the package must be declared like this 32 | add_executable(${PROJECT_NAME} 33 | src/velocity_estimator_handle.cpp 34 | src/velocity_estimator.cpp 35 | src/main.cpp 36 | ) 37 | target_link_libraries(${PROJECT_NAME} 38 | ${catkin_LIBRARIES} 39 | ) 40 | -------------------------------------------------------------------------------- /src/3_control/control_pure_pursuit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(control_pure_pursuit) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | set(PROJECT_DEPS 7 | roscpp 8 | std_msgs 9 | fsd_common_msgs 10 | ) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | fsd_common_msgs 16 | geometry_msgs 17 | ) 18 | 19 | catkin_package( 20 | INCLUDE_DIRS 21 | LIBRARIES 22 | CATKIN_DEPENDS 23 | DEPENDS 24 | ) 25 | 26 | include_directories( 27 | include 28 | ${catkin_INCLUDE_DIRS} 29 | ${roscpp_INCLUDE_DIRS} 30 | ) 31 | 32 | # Each node in the package must be declared like this 33 | add_executable(${PROJECT_NAME} 34 | src/pure_pursuit_handle.cpp 35 | src/pure_pursuit.cpp 36 | src/main.cpp 37 | ) 38 | target_link_libraries(${PROJECT_NAME} 39 | ${catkin_LIBRARIES} 40 | ) 41 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fsd_common_msgs 4 | 0.0.0 5 | Messages common to many packages 6 | 7 | Sonja Brits 8 | TODO 9 | Sonja Brits 10 | 11 | 12 | catkin 13 | 14 | 15 | message_generation 16 | message_runtime 17 | message_runtime 18 | 19 | 20 | roscpp 21 | rospy 22 | std_msgs 23 | geometry_msgs 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | # TODO: Change name of package 3 | project(fsd_common_msgs) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | message_generation # Needed to generate custom message types 7 | std_msgs 8 | roscpp 9 | rospy 10 | geometry_msgs 11 | ) 12 | 13 | add_message_files( # Tell catkin which custom message files to include 14 | FILES 15 | Cone.msg 16 | ConeDetections.msg 17 | Map.msg 18 | ControlCommand.msg 19 | CarState.msg 20 | CarStateDt.msg 21 | Mission.msg 22 | ) 23 | 24 | generate_messages( 25 | DEPENDENCIES 26 | std_msgs 27 | geometry_msgs 28 | ) 29 | 30 | catkin_package( 31 | CATKIN_DEPENDS message_runtime std_msgs geometry_msgs # Needed to generate custom messages 32 | ) 33 | include_directories( 34 | # include 35 | ${catkin_INCLUDE_DIRS} 36 | ${roscpp_INCLUDE_DIRS} 37 | ${rospy_INCLUDE_DIRS} 38 | ) 39 | 40 | -------------------------------------------------------------------------------- /fsd_aliases: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | FSD_ROOT=$( cd "$(dirname "${BASH_SOURCE[0]}" )" && pwd) 4 | 5 | alias FSD_source="source ${FSD_ROOT}/fsd_environment.sh" 6 | alias FSD_build="catkin clean --yes && catkin build" 7 | alias FSD_cd="cd ${FSD_ROOT}" 8 | 9 | # Missions 10 | alias FSD_launch_trackdrive="roslaunch fsd_common_meta trackdrive.launch" 11 | alias FSD_launch_skidpad="roslaunch fsd_common_meta skidpad.launch" 12 | alias FSD_launch_acceleration="roslaunch fsd_common_meta acceleration.launch" 13 | alias FSD_launch_autox="roslaunch fsd_common_meta autox.launch" 14 | alias FSD_launch_scruti="roslaunch fsd_common_meta scruti.launch" 15 | 16 | # RVIZ Launching 17 | alias FSD_rviz_trackdrive="roslaunch fsd_common_meta rviz_trackdrive.launch" 18 | alias FSD_rviz_skidpad="roslaunch fsd_common_meta rviz_skidpad.launch" 19 | alias FSD_rviz_acceleration="roslaunch fsd_common_meta rviz_acceleration.launch" 20 | alias FSD_rviz_autox="roslaunch fsd_common_meta rviz_autox.launch" 21 | alias FSD_rviz_scruti="roslaunch fsd_common_meta rviz_scruti.launch" 22 | 23 | # FSSIM 24 | alias FSD_ATS="FSD_source; ${FSD_ROOT}/src/fssim/fssim/scripts/launch.py --config ${FSD_ROOT}/src/fssim_interface/fssim_config/ats_simulation.yaml --output ${HOME}/sim_output/" 25 | -------------------------------------------------------------------------------- /src/2_estimation/slam_slam/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "slam_handle.hpp" 22 | 23 | typedef ns_slam::SlamHandle SlamHandle; 24 | 25 | int main(int argc, char **argv) { 26 | ros::init(argc, argv, "slam"); 27 | ros::NodeHandle nodeHandle("~"); 28 | SlamHandle mySlamHandle(nodeHandle); 29 | ros::Rate loop_rate(mySlamHandle.getNodeRate()); 30 | while (ros::ok()) { 31 | 32 | mySlamHandle.run(); 33 | 34 | ros::spinOnce(); // Keeps node alive basically 35 | loop_rate.sleep(); // Sleep for loop_rate 36 | } 37 | return 0; 38 | } 39 | 40 | -------------------------------------------------------------------------------- /src/3_control/control_pure_pursuit/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "pure_pursuit_handle.hpp" 22 | 23 | typedef ns_pure_pursuit::PurePursuitHandle PurePursuitHandle; 24 | 25 | int main(int argc, char **argv) { 26 | ros::init(argc, argv, "purePursuit"); 27 | ros::NodeHandle nodeHandle("~"); 28 | PurePursuitHandle purePursuitHandle(nodeHandle); 29 | ros::Rate loop_rate(purePursuitHandle.getNodeRate()); 30 | while (ros::ok()) { 31 | 32 | purePursuitHandle.run(); 33 | 34 | ros::spinOnce(); // Keeps node alive basically 35 | loop_rate.sleep(); // Sleep for loop_rate 36 | } 37 | return 0; 38 | } 39 | 40 | -------------------------------------------------------------------------------- /src/1_perception/lidar_cone_detector/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "cone_detector_handle.hpp" 22 | 23 | typedef ns_cone_detector::ConeDetectorHandle ConeDetectorHandle; 24 | 25 | int main(int argc, char **argv) { 26 | ros::init(argc, argv, "cone_detector"); 27 | ros::NodeHandle nodeHandle("~"); 28 | ConeDetectorHandle myConeDetectorHandle(nodeHandle); 29 | ros::Rate loop_rate(myConeDetectorHandle.getNodeRate()); 30 | while (ros::ok()) { 31 | 32 | myConeDetectorHandle.run(); 33 | 34 | ros::spinOnce(); // Keeps node alive basically 35 | loop_rate.sleep(); // Sleep for loop_rate 36 | } 37 | return 0; 38 | } 39 | 40 | -------------------------------------------------------------------------------- /src/1_perception/vision_cone_detector/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "cone_detector_handle.hpp" 22 | 23 | typedef ns_cone_detector::ConeDetectorHandle ConeDetectorHandle; 24 | 25 | int main(int argc, char **argv) { 26 | ros::init(argc, argv, "cone_detector"); 27 | ros::NodeHandle nodeHandle("~"); 28 | ConeDetectorHandle myConeDetectorHandle(nodeHandle); 29 | ros::Rate loop_rate(myConeDetectorHandle.getNodeRate()); 30 | while (ros::ok()) { 31 | 32 | myConeDetectorHandle.run(); 33 | 34 | ros::spinOnce(); // Keeps node alive basically 35 | loop_rate.sleep(); // Sleep for loop_rate 36 | } 37 | return 0; 38 | } 39 | 40 | -------------------------------------------------------------------------------- /src/2_estimation/velocity_estimator/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "velocity_estimator_handle.hpp" 22 | 23 | typedef ns_velocity_estimation::VelocityEstimatorHandle VelocityEstimatorHandle; 24 | 25 | int main(int argc, char **argv) { 26 | ros::init(argc, argv, "velocity_estimation"); 27 | ros::NodeHandle nodeHandle("~"); 28 | VelocityEstimatorHandle myVelocityEstimatorHandle(nodeHandle); 29 | ros::Rate loop_rate(myVelocityEstimatorHandle.getNodeRate()); 30 | while (ros::ok()) { 31 | 32 | myVelocityEstimatorHandle.run(); 33 | 34 | ros::spinOnce(); // Keeps node alive basically 35 | loop_rate.sleep(); // Sleep for loop_rate 36 | } 37 | return 0; 38 | } 39 | 40 | -------------------------------------------------------------------------------- /src/2_estimation/velocity_estimator/src/velocity_estimator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "velocity_estimator.hpp" 22 | #include 23 | 24 | namespace ns_velocity_estimation { 25 | // Constructor 26 | VelocityEstimator::VelocityEstimator() { 27 | }; 28 | 29 | // Getters 30 | geometry_msgs::Pose2D VelocityEstimator::getVelocityEstimate() const { return velocity_estimate_;} 31 | 32 | void VelocityEstimator::runAlgorithm() { 33 | createVelocityEstimate(); 34 | } 35 | 36 | void VelocityEstimator::createVelocityEstimate() { 37 | // Generate random numbers as velocity estimate 38 | velocity_estimate_.x = 3* ((double) rand() / (RAND_MAX)); 39 | velocity_estimate_.y = 0.5* ((double) rand() / (RAND_MAX)); 40 | velocity_estimate_.theta = 0.1* ((double) rand() / (RAND_MAX)); 41 | } 42 | 43 | } 44 | -------------------------------------------------------------------------------- /src/fssim_interface/README.md: -------------------------------------------------------------------------------- 1 | # Interface for FSSIM 2 | 3 | In order to create a universal simulator, FSSIM comunicated with your own framework through this interface. 4 | This package does not contain any logic, it only translated messages from FSSIM into your messages. 5 | In addition to that it contains launch files which are able to launch only smaller part of your framework (e.g. control since in 6 | this case FSSIM simulates SLAM and localization as it directly streams pose a map) 7 | 8 | * `fssim_config`: contains configurations for FSSIM 9 | * `config`: contains definitions of topics which are translated 10 | * `mission_launch`: contains launch file for the sub-section of your autonomous-pipeline 11 | 12 | # How to run FSSIM 13 | 14 | Detailed description can be found in ..... 15 | 16 | ### Run FSSIM parallely to your code 17 | If you want to run simulator in one terminal window and be able to test your code in parallel execute 18 | 1. `roslaunch fssim_interface fssim.launch` 19 | 2. Your pipeline, e.g. `roslaunch control_meta trackdrive.launch` 20 | 21 | ### Run FSSIM and all pipeline automatically 22 | If you want to run everything with one command. Either add your pipeline launch file to `local_simulation.yaml` under `autonomous_stack:` 23 | and then only execute 24 | 1. `roslaunch fssim_interface fssim.launch` 25 | 26 | In this way you can also start multiple repetitions: add another line under `repetitions:` in `local_simualtion.yaml` 27 | 28 | ### Run Automatic Test 29 | If you want to run whole bunch of tests without controlling their progress (no RVIZ), call only `FSD_ATS` what will launch `ats_simulation.yaml` config and stores the loged 30 | files and report to `~/sim_output/` folder. 31 | -------------------------------------------------------------------------------- /src/1_perception/lidar_cone_detector/include/cone_detector.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #ifndef PERCEPTION_LIDAR_CONE_DETECTOR_HPP 21 | #define PERCEPTION_LIDAR_CONE_DETECTOR_HPP 22 | 23 | #include "fsd_common_msgs/ConeDetections.h" 24 | #include "geometry_msgs/Point.h" 25 | #include "std_msgs/String.h" 26 | 27 | namespace ns_cone_detector { 28 | 29 | class ConeDetector { 30 | 31 | public: 32 | // Constructor 33 | ConeDetector(); 34 | 35 | // Getters 36 | fsd_common_msgs::ConeDetections getConeDetections() const; 37 | 38 | /** 39 | * creates the cone detections 40 | */ 41 | void createConeDetections(); 42 | 43 | /** 44 | * calls the other functions in the right order 45 | */ 46 | void runAlgorithm(); 47 | 48 | private: 49 | 50 | fsd_common_msgs::ConeDetections coneDetections_; 51 | }; 52 | } 53 | 54 | #endif //PERCEPTION_LIDAR_CONE_DETECTOR_HPP 55 | -------------------------------------------------------------------------------- /src/1_perception/vision_cone_detector/include/cone_detector.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #ifndef PERCEPTION_VISION_CONE_DETECTOR_HPP 21 | #define PERCEPTION_VISION_CONE_DETECTOR_HPP 22 | 23 | #include "fsd_common_msgs/ConeDetections.h" 24 | #include "geometry_msgs/Point.h" 25 | #include "std_msgs/String.h" 26 | 27 | namespace ns_cone_detector { 28 | 29 | class ConeDetector { 30 | 31 | public: 32 | // Constructor 33 | ConeDetector(); 34 | 35 | // Getters 36 | fsd_common_msgs::ConeDetections getConeDetections() const; 37 | 38 | /** 39 | * creates the cone detections 40 | */ 41 | void createConeDetections(); 42 | 43 | /** 44 | * calls the other functions in the right order 45 | */ 46 | void runAlgorithm(); 47 | 48 | private: 49 | 50 | fsd_common_msgs::ConeDetections coneDetections_; 51 | }; 52 | } 53 | 54 | #endif //PERCEPTION_VISION_CONE_DETECTOR_HPP 55 | -------------------------------------------------------------------------------- /src/2_estimation/velocity_estimator/include/velocity_estimator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #ifndef ESTIMATION_VELOCITY_ESTIMATION_VELOCITY_ESTIMATOR_HPP 21 | #define ESTIMATION_VELOCITY_ESTIMATION_VELOCITY_ESTIMATOR_HPP 22 | 23 | #include "geometry_msgs/Pose2D.h" 24 | #include "std_msgs/String.h" 25 | 26 | namespace ns_velocity_estimation { 27 | 28 | class VelocityEstimator { 29 | 30 | public: 31 | // Constructor 32 | VelocityEstimator(); 33 | 34 | // Getters 35 | geometry_msgs::Pose2D getVelocityEstimate() const; 36 | 37 | /** 38 | * creates the cone detections 39 | */ 40 | void createVelocityEstimate(); 41 | 42 | /** 43 | * calls the other functions in the right order 44 | */ 45 | void runAlgorithm(); 46 | 47 | private: 48 | 49 | geometry_msgs::Pose2D velocity_estimate_; 50 | }; 51 | } 52 | 53 | #endif //ESTIMATION_VELOCITY_ESTIMATION_VELOCITY_ESTIMATOR_HPP 54 | -------------------------------------------------------------------------------- /src/1_perception/vision_cone_detector/src/cone_detector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "cone_detector.hpp" 22 | #include 23 | 24 | namespace ns_cone_detector { 25 | // Constructor 26 | ConeDetector::ConeDetector() { 27 | }; 28 | 29 | // Getters 30 | fsd_common_msgs::ConeDetections ConeDetector::getConeDetections() const { return coneDetections_;} 31 | 32 | void ConeDetector::runAlgorithm() { 33 | createConeDetections(); 34 | } 35 | 36 | void ConeDetector::createConeDetections() { 37 | std::vector cones; 38 | fsd_common_msgs::Cone cone; 39 | // Create 3 random blue cones 40 | for (int i = 0; i < 3; i++) { 41 | cone.position.x = 10 * ((double) rand() / (RAND_MAX)); 42 | cone.position.y = 10 * ((double) rand() / (RAND_MAX)); 43 | cone.color.data = "b"; 44 | cones.push_back(cone); 45 | } 46 | coneDetections_.cone_detections = cones; 47 | } 48 | 49 | } 50 | -------------------------------------------------------------------------------- /src/1_perception/lidar_cone_detector/src/cone_detector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "cone_detector.hpp" 22 | #include 23 | 24 | namespace ns_cone_detector { 25 | // Constructor 26 | ConeDetector::ConeDetector() { 27 | }; 28 | 29 | // Getters 30 | fsd_common_msgs::ConeDetections ConeDetector::getConeDetections() const { return coneDetections_;} 31 | 32 | void ConeDetector::runAlgorithm() { 33 | createConeDetections(); 34 | } 35 | 36 | void ConeDetector::createConeDetections() { 37 | std::vector cones; 38 | fsd_common_msgs::Cone cone; 39 | // Create 3 random yellow cones 40 | for (int i = 0; i < 3; i++) 41 | { 42 | cone.position.x = 15 * ((double) rand() / (RAND_MAX)); 43 | cone.position.y = 15 * ((double) rand() / (RAND_MAX)); 44 | cone.color.data = "y"; 45 | cones.push_back(cone); 46 | } 47 | coneDetections_.cone_detections = cones; 48 | } 49 | 50 | } 51 | -------------------------------------------------------------------------------- /src/fssim_interface/config/config.yaml: -------------------------------------------------------------------------------- 1 | # AMZ-Driverless 2 | # Copyright (c) 2018 Authors: 3 | # - Juraj Kabzan 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | fssim: 24 | topic_odom: "/fssim/base_pose_ground_truth" 25 | track: "/fssim/track" 26 | cmd: "/fssim/cmd" 27 | res: "/fssim/res_state" 28 | mission_finnished: "/fssim/mission_finished" 29 | 30 | fsd: 31 | vel: "/estimation/velocity_estimation/velocity_estimation" 32 | cmd: "/control/pure_pursuit/control_command" 33 | res: "/res/state" 34 | state: "/estimation/slam/state" 35 | map: "/estimation/slam/map" 36 | mission_finnished: "/control/mission_finished" 37 | tf: 38 | origin: "/map" 39 | fsd_car_base_link: "/gotthard/base_link" 40 | publish_car_base_link: true 41 | -------------------------------------------------------------------------------- /src/1_perception/lidar_cone_detector/include/cone_detector_handle.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #ifndef PERCEPTION_LIDAR_CONE_DETECTOR_HANDLE_HPP 21 | #define PERCEPTION_LIDAR_CONE_DETECTOR_HANDLE_HPP 22 | 23 | #include "fsd_common_msgs/ConeDetections.h" 24 | #include "cone_detector.hpp" 25 | 26 | namespace ns_cone_detector { 27 | 28 | class ConeDetectorHandle { 29 | 30 | public: 31 | // Constructor 32 | ConeDetectorHandle(ros::NodeHandle &nodeHandle); 33 | 34 | // // Getters 35 | int getNodeRate() const; 36 | 37 | // Methods 38 | void loadParameters(); 39 | void subscribeToTopics(); 40 | void publishToTopics(); 41 | void run(); 42 | void sendConeDetections(); 43 | // void sendVisualization(); 44 | 45 | private: 46 | ros::NodeHandle nodeHandle_; 47 | ros::Publisher coneDetectionsPublisher; 48 | 49 | std::string cone_detections_topic_name_; 50 | int node_rate_; 51 | 52 | ConeDetector coneDetector_; 53 | fsd_common_msgs::ConeDetections cone_detections_; 54 | }; 55 | } 56 | 57 | #endif //PERCEPTION_LIDAR_CONE_DETECTOR_HANDLE_HPP 58 | -------------------------------------------------------------------------------- /src/1_perception/vision_cone_detector/include/cone_detector_handle.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #ifndef PERCEPTION_VISION_CONE_DETECTOR_HANDLE_HPP 21 | #define PERCEPTION_VISION_CONE_DETECTOR_HANDLE_HPP 22 | 23 | #include "fsd_common_msgs/ConeDetections.h" 24 | #include "cone_detector.hpp" 25 | 26 | namespace ns_cone_detector { 27 | 28 | class ConeDetectorHandle { 29 | 30 | public: 31 | // Constructor 32 | ConeDetectorHandle(ros::NodeHandle &nodeHandle); 33 | 34 | // // Getters 35 | int getNodeRate() const; 36 | 37 | // Methods 38 | void loadParameters(); 39 | void subscribeToTopics(); 40 | void publishToTopics(); 41 | void run(); 42 | void sendConeDetections(); 43 | // void sendVisualization(); 44 | 45 | private: 46 | ros::NodeHandle nodeHandle_; 47 | ros::Publisher coneDetectionsPublisher; 48 | 49 | std::string cone_detections_topic_name_; 50 | int node_rate_; 51 | 52 | ConeDetector coneDetector_; 53 | fsd_common_msgs::ConeDetections cone_detections_; 54 | }; 55 | } 56 | 57 | #endif //PERCEPTION_VISION_CONE_DETECTOR_HANDLE_HPP 58 | -------------------------------------------------------------------------------- /src/2_estimation/velocity_estimator/include/velocity_estimator_handle.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #ifndef ESTIMATION_VELOCITY_ESTIMATION_CONE_DETECTOR_HANDLE_HPP 21 | #define ESTIMATION_VELOCITY_ESTIMATION_CONE_DETECTOR_HANDLE_HPP 22 | 23 | #include "fsd_common_msgs/CarStateDt.h" 24 | #include "velocity_estimator.hpp" 25 | 26 | namespace ns_velocity_estimation { 27 | 28 | class VelocityEstimatorHandle { 29 | 30 | public: 31 | // Constructor 32 | VelocityEstimatorHandle(ros::NodeHandle &nodeHandle); 33 | 34 | // // Getters 35 | int getNodeRate() const; 36 | 37 | // Methods 38 | void loadParameters(); 39 | void subscribeToTopics(); 40 | void publishToTopics(); 41 | void run(); 42 | void sendVelocityEstimate(); 43 | // void sendVisualization(); 44 | 45 | private: 46 | ros::NodeHandle nodeHandle_; 47 | ros::Publisher velocityEstimationPublisher; 48 | 49 | std::string velocity_estimation_topic_name_; 50 | int node_rate_; 51 | 52 | VelocityEstimator velocityEstimator_; 53 | fsd_common_msgs::CarStateDt velocity_estimate_; 54 | }; 55 | } 56 | 57 | #endif //ESTIMATION_VELOCITY_ESTIMATION_CONE_DETECTOR_HANDLE_HPP 58 | -------------------------------------------------------------------------------- /src/fssim_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # AMZ-Driverless 2 | # Copyright (c) 2018 Authors: 3 | # - Juraj Kabzan 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | cmake_minimum_required(VERSION 2.8.3) 24 | 25 | project(fssim_interface) 26 | 27 | ## Compile as C++11, supported in ROS Kinetic and newer 28 | add_compile_options(-std=c++11) 29 | 30 | find_package(catkin REQUIRED COMPONENTS 31 | roscpp 32 | rospy 33 | std_msgs 34 | nav_msgs 35 | tf 36 | fsd_common_msgs 37 | fssim_common) 38 | 39 | catkin_package() 40 | 41 | include_directories( 42 | include 43 | ${catkin_INCLUDE_DIRS} 44 | ${roscpp_INCLUDE_DIRS} 45 | ${rospy_INCLUDE_DIRS} 46 | ) 47 | 48 | add_executable(fssim_interface_node src/fssim_interface_node.cpp) 49 | target_link_libraries(fssim_interface_node ${catkin_LIBRARIES}) -------------------------------------------------------------------------------- /src/fssim_interface/fssim_config/local_simulation.yaml: -------------------------------------------------------------------------------- 1 | # AMZ-Driverless 2 | # Copyright (c) 2018 Authors: 3 | # - Juraj Kabzan 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | simulation_name: "Testing what gets wrong first" 24 | 25 | robot_name: gotthard # Name of the robot [string] 26 | kill_after: 6000 # After this time, repetition is stopped [s] 27 | 28 | pkg_config_storage: fssim_interface # If relative path is given in following config files, the path will be taken 29 | # wrt to this package 30 | # Config files for repetitions 31 | repetitions: 32 | - {sensors_config_file: fssim_config/sensors/sensors_1.yaml, track_name: FSG.sdf, autonomous_stack: } 33 | 34 | # Checks which must be true in order to automatically press RES GO 35 | res: 36 | checks: 37 | is_in_track: true 38 | 39 | # Launch files for autonomous missions 40 | launch_file: 41 | rosbag_record: 42 | 43 | #################################### 44 | ######## Possible Tracks ########### 45 | #################################### 46 | # FSG.sdf 47 | # FSI.sdf 48 | # thin.sdf 49 | # skidpad.sdf 50 | # acceleration.sdf 51 | -------------------------------------------------------------------------------- /src/2_estimation/slam_slam/include/slam.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #ifndef ESTIMATION_SLAM_SLAM_HPP 21 | #define ESTIMATION_SLAM_SLAM_HPP 22 | 23 | #include "fsd_common_msgs/ConeDetections.h" 24 | #include "fsd_common_msgs/CarStateDt.h" 25 | #include "fsd_common_msgs/Map.h" 26 | #include "geometry_msgs/Point.h" 27 | #include "geometry_msgs/Pose2D.h" 28 | #include "std_msgs/String.h" 29 | 30 | namespace ns_slam { 31 | 32 | class Slam { 33 | 34 | public: 35 | // Constructor 36 | Slam(); 37 | 38 | // Getters 39 | std::vector getMap() const; 40 | geometry_msgs::Pose2D getState() const; 41 | 42 | // Setters 43 | void setMaxMapSize(int size); 44 | 45 | /** 46 | * initializes the cone map 47 | */ 48 | void initializeMap(); 49 | 50 | /** 51 | * initializes the state 52 | */ 53 | void initializeState(); 54 | 55 | /** 56 | * updates the cone map 57 | */ 58 | void updateMap(const fsd_common_msgs::ConeDetections &cones); 59 | 60 | /** 61 | * calculates the new car state 62 | */ 63 | void calculateState(const fsd_common_msgs::CarStateDt &velocity); 64 | 65 | /** 66 | * calls the other functions in the right order 67 | */ 68 | void runAlgorithm(); 69 | 70 | private: 71 | 72 | std::vector cone_map_; 73 | geometry_msgs::Pose2D slam_state_; 74 | int max_map_size_; 75 | }; 76 | } 77 | 78 | #endif //ESTIMATION_SLAM_SLAM_HPP 79 | -------------------------------------------------------------------------------- /src/fssim_interface/fssim_config/ats_simulation.yaml: -------------------------------------------------------------------------------- 1 | # AMZ-Driverless 2 | # Copyright (c) 2018 Authors: 3 | # - Juraj Kabzan 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | simulation_name: "ATS Simulation" 24 | 25 | robot_name: gotthard # Name of the robot [string] 26 | kill_after: 100 # After this time, repetition is stopped [s] 27 | 28 | pkg_config_storage: fssim_interface # If relative path is given in following config files, the path will be taken 29 | # wrt to this package 30 | # Config files for repetitions 31 | repetitions: 32 | - {sensors_config_file: fssim_config/sensors/sensors_1.yaml, track_name: FSG.sdf, autonomous_stack: roslaunch fssim_interface trackdrive.launch } 33 | - {sensors_config_file: fssim_config/sensors/sensors_1.yaml, track_name: FSI.sdf, autonomous_stack: roslaunch fssim_interface trackdrive.launch } 34 | 35 | 36 | # Checks which must be true in order to automatically press RES GO 37 | res: 38 | checks: 39 | is_in_track: true 40 | 41 | # Launch files for autonomous missions 42 | launch_file: 43 | rosbag_record: roslaunch fssim_interface record_sim.launch bag_save_path:={} 44 | -------------------------------------------------------------------------------- /src/2_estimation/slam_slam/src/slam.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "slam.hpp" 22 | #include 23 | 24 | namespace ns_slam { 25 | // Constructor 26 | Slam::Slam() { 27 | initializeState(); 28 | }; 29 | 30 | // Getters 31 | std::vector Slam::getMap() const { return cone_map_; } 32 | geometry_msgs::Pose2D Slam::getState() const { return slam_state_; } 33 | 34 | // Setters 35 | void Slam::setMaxMapSize(int size) { max_map_size_ = size; } 36 | 37 | void Slam::initializeState() { 38 | slam_state_.x = 0; 39 | slam_state_.y = 0; 40 | slam_state_.theta = 0; 41 | } 42 | 43 | void Slam::updateMap(const fsd_common_msgs::ConeDetections &cones) { 44 | ROS_INFO("Map size is %d", (int) cone_map_.size()); 45 | int excess_cone_length = cone_map_.size() + cones.cone_detections.size() - max_map_size_; 46 | if (cone_map_.size() + cones.cone_detections.size() > max_map_size_) { 47 | ROS_INFO("Removing %d elements to accomodate new elements", (int) excess_cone_length); 48 | cone_map_.erase(cone_map_.begin(), cone_map_.begin() + excess_cone_length); 49 | } 50 | for (int i = 0; i < cones.cone_detections.size(); i++) { 51 | ROS_INFO("cone add to map"); 52 | cone_map_.push_back(cones.cone_detections[i]); 53 | } 54 | } 55 | 56 | void Slam::calculateState(const fsd_common_msgs::CarStateDt &velocity) { 57 | slam_state_.x += velocity.car_state_dt.x; 58 | slam_state_.y += velocity.car_state_dt.y; 59 | slam_state_.theta += velocity.car_state_dt.theta; 60 | } 61 | 62 | } 63 | -------------------------------------------------------------------------------- /src/3_control/control_pure_pursuit/include/pure_pursuit_handle.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #ifndef CONTROL_PURE_PURSUIT_HANDLE_HPP 21 | #define CONTROL_PURE_PURSUIT_HANDLE_HPP 22 | 23 | #include "fsd_common_msgs/ControlCommand.h" 24 | #include "fsd_common_msgs/Map.h" 25 | #include "fsd_common_msgs/CarState.h" 26 | #include "fsd_common_msgs/CarStateDt.h" 27 | #include "pure_pursuit.hpp" 28 | 29 | namespace ns_pure_pursuit { 30 | 31 | class PurePursuitHandle { 32 | 33 | public: 34 | // Constructor 35 | explicit PurePursuitHandle(ros::NodeHandle &nodeHandle); 36 | 37 | // // Getters 38 | int getNodeRate() const; 39 | 40 | // Methods 41 | void loadParameters(); 42 | void subscribeToTopics(); 43 | void publishToTopics(); 44 | void run(); 45 | void sendControlCommand(); 46 | // void sendVisualization(); 47 | 48 | private: 49 | ros::NodeHandle nodeHandle_; 50 | 51 | ros::Publisher controlCommandPublisher_; 52 | ros::Publisher centerLinePublisher_; 53 | 54 | ros::Subscriber slamMapSubscriber_; 55 | ros::Subscriber slamStateSubscriber_; 56 | ros::Subscriber velocityEstimateSubscriber_; 57 | 58 | void velocityEstimateCallback(const fsd_common_msgs::CarStateDt &velocity); 59 | void slamMapCallback(const fsd_common_msgs::Map &map); 60 | void slamStateCallback(const fsd_common_msgs::CarState &state); 61 | 62 | std::string velocity_estimate_topic_name_; 63 | std::string slam_map_topic_name_; 64 | std::string slam_state_topic_name_; 65 | std::string control_command_topic_name_; 66 | std::string center_line_topic_name_; 67 | 68 | int node_rate_; 69 | double max_speed_; 70 | 71 | PurePursuit pure_pursuit_; 72 | fsd_common_msgs::ControlCommand control_command_; 73 | }; 74 | } 75 | 76 | #endif //CONTROL_PURE_PURSUIT_HANDLE_HPP 77 | -------------------------------------------------------------------------------- /src/1_perception/lidar_cone_detector/src/cone_detector_handle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "cone_detector_handle.hpp" 22 | 23 | namespace ns_cone_detector { 24 | 25 | // Constructor 26 | ConeDetectorHandle::ConeDetectorHandle(ros::NodeHandle &nodeHandle) : 27 | nodeHandle_(nodeHandle) { 28 | ROS_INFO("Constructing Handle"); 29 | loadParameters(); 30 | publishToTopics(); 31 | } 32 | 33 | // Getters 34 | int ConeDetectorHandle::getNodeRate() const { return node_rate_; } 35 | 36 | // Methods 37 | void ConeDetectorHandle::loadParameters() { 38 | ROS_INFO("loading handle parameters"); 39 | if (!nodeHandle_.param("cone_detections_topic_name", 40 | cone_detections_topic_name_, 41 | "/perception/cone_detections")) { 42 | ROS_WARN_STREAM("Did not load cone_detections_topic_name. Standard value is: " << cone_detections_topic_name_); 43 | } 44 | if (!nodeHandle_.param("node_rate", node_rate_, 1)) { 45 | ROS_WARN_STREAM("Did not load node_rate. Standard value is: " << node_rate_); 46 | } 47 | } 48 | 49 | void ConeDetectorHandle::publishToTopics() { 50 | ROS_INFO("publish to topics"); 51 | coneDetectionsPublisher = nodeHandle_.advertise(cone_detections_topic_name_, 1); 52 | } 53 | 54 | void ConeDetectorHandle::run() { 55 | coneDetector_.runAlgorithm(); 56 | sendConeDetections(); 57 | } 58 | 59 | void ConeDetectorHandle::sendConeDetections() { 60 | cone_detections_.cone_detections = coneDetector_.getConeDetections().cone_detections; 61 | cone_detections_.header.stamp = ros::Time::now(); 62 | coneDetectionsPublisher.publish(cone_detections_); 63 | ROS_INFO("cone detections sent"); 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /src/1_perception/vision_cone_detector/src/cone_detector_handle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "cone_detector_handle.hpp" 22 | 23 | namespace ns_cone_detector { 24 | 25 | // Constructor 26 | ConeDetectorHandle::ConeDetectorHandle(ros::NodeHandle &nodeHandle) : 27 | nodeHandle_(nodeHandle) { 28 | ROS_INFO("Constructing Handle"); 29 | loadParameters(); 30 | publishToTopics(); 31 | } 32 | 33 | // Getters 34 | int ConeDetectorHandle::getNodeRate() const { return node_rate_; } 35 | 36 | // Methods 37 | void ConeDetectorHandle::loadParameters() { 38 | ROS_INFO("loading handle parameters"); 39 | if (!nodeHandle_.param("cone_detections_topic_name", 40 | cone_detections_topic_name_, 41 | "/perception/cone_detections")) { 42 | ROS_WARN_STREAM("Did not load cone_detections_topic_name. Standard value is: " << cone_detections_topic_name_); 43 | } 44 | if (!nodeHandle_.param("node_rate", node_rate_, 1)) { 45 | ROS_WARN_STREAM("Did not load node_rate. Standard value is: " << node_rate_); 46 | } 47 | } 48 | 49 | void ConeDetectorHandle::publishToTopics() { 50 | ROS_INFO("publish to topics"); 51 | coneDetectionsPublisher = nodeHandle_.advertise(cone_detections_topic_name_, 1); 52 | } 53 | 54 | void ConeDetectorHandle::run() { 55 | coneDetector_.runAlgorithm(); 56 | sendConeDetections(); 57 | } 58 | 59 | void ConeDetectorHandle::sendConeDetections() { 60 | cone_detections_.cone_detections = coneDetector_.getConeDetections().cone_detections; 61 | cone_detections_.header.stamp = ros::Time::now(); 62 | coneDetectionsPublisher.publish(cone_detections_); 63 | ROS_INFO("cone detections sent"); 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /src/3_control/control_pure_pursuit/include/pure_pursuit.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | - Juraj Kabzan 6 | 7 | FSD-Project is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | FSD-Project is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with FSD-Project. If not, see . 19 | */ 20 | 21 | #ifndef CONTROL_PURE_PURSUIT_HPP 22 | #define CONTROL_PURE_PURSUIT_HPP 23 | #include "ros/ros.h" 24 | 25 | #include 26 | #include "fsd_common_msgs/ControlCommand.h" 27 | #include "fsd_common_msgs/Map.h" 28 | #include "fsd_common_msgs/CarState.h" 29 | #include "fsd_common_msgs/CarStateDt.h" 30 | #include "geometry_msgs/Point.h" 31 | #include "std_msgs/String.h" 32 | 33 | namespace ns_pure_pursuit { 34 | 35 | class PurePursuit { 36 | 37 | public: 38 | // Constructor 39 | PurePursuit(ros::NodeHandle& nh); 40 | 41 | // Getters 42 | fsd_common_msgs::ControlCommand getControlCommand() const; 43 | 44 | // Setters 45 | void setMaxSpeed(double &max_speed); 46 | void setCenterLine(const geometry_msgs::Polygon ¢er_line); 47 | void setState(const fsd_common_msgs::CarState &state); 48 | void setVelocity(const fsd_common_msgs::CarStateDt &velocity); 49 | 50 | /** 51 | * creates the cone detections 52 | */ 53 | void createControlCommand(); 54 | 55 | /** 56 | * calls the other functions in the right order 57 | */ 58 | void runAlgorithm(); 59 | 60 | private: 61 | 62 | /** 63 | * Visualize 64 | */ 65 | void publishMarkers(double x_pos, double y_pos, double x_next, double y_next) const; 66 | 67 | ros::NodeHandle& nh_; 68 | 69 | ros::Publisher pub_closest_point_; 70 | 71 | double speed_p; 72 | double steering_p; 73 | 74 | geometry_msgs::Polygon center_line_; 75 | fsd_common_msgs::CarState state_; 76 | fsd_common_msgs::CarStateDt velocity_; 77 | fsd_common_msgs::ControlCommand control_command_; 78 | 79 | double max_speed_; 80 | }; 81 | } 82 | 83 | #endif //CONTROL_PURE_PURSUIT_HPP 84 | -------------------------------------------------------------------------------- /src/2_estimation/slam_slam/include/slam_handle.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #ifndef ESTIMATION_SLAM_SLAM_HANDLE_HPP 21 | #define ESTIMATION_SLAM_SLAM_HANDLE_HPP 22 | 23 | #include "fsd_common_msgs/ConeDetections.h" 24 | #include "fsd_common_msgs/CarStateDt.h" 25 | #include "fsd_common_msgs/Map.h" 26 | #include "fsd_common_msgs/CarState.h" 27 | #include "geometry_msgs/Pose2D.h" 28 | #include "slam.hpp" 29 | 30 | namespace ns_slam { 31 | 32 | class SlamHandle { 33 | 34 | public: 35 | // Constructor 36 | SlamHandle(ros::NodeHandle &nodeHandle); 37 | 38 | // // Getters 39 | int getNodeRate() const; 40 | 41 | // Methods 42 | void loadParameters(); 43 | void subscribeToTopics(); 44 | void publishToTopics(); 45 | void run(); 46 | void sendMap(); 47 | void sendState(); 48 | // void sendVisualization(); 49 | 50 | private: 51 | ros::NodeHandle nodeHandle_; 52 | ros::Subscriber visionConeDetectionsSubscriber_; 53 | ros::Subscriber lidarConeDetectionsSubscriber_; 54 | ros::Subscriber velocityEstimateSubscriber_; 55 | ros::Publisher slamMapPublisher_; 56 | ros::Publisher slamStatePublisher_; 57 | 58 | void lidarConeDetectionsCallback(const fsd_common_msgs::ConeDetections &cones); 59 | void visionConeDetectionsCallback(const fsd_common_msgs::ConeDetections &cones); 60 | void velocityEstimateCallback(const fsd_common_msgs::CarStateDt &velocity); 61 | 62 | std::string vision_cone_detections_topic_name_; 63 | std::string velocity_estimate_topic_name_; 64 | std::string lidar_cone_detections_topic_name_; 65 | std::string slam_map_topic_name_; 66 | std::string slam_state_topic_name_; 67 | int node_rate_; 68 | 69 | Slam slam_; 70 | fsd_common_msgs::ConeDetections cone_detections_; 71 | fsd_common_msgs::CarStateDt velocity_estimate_; 72 | fsd_common_msgs::Map slam_map_; 73 | fsd_common_msgs::CarState slam_state_; 74 | 75 | int max_map_size_; 76 | }; 77 | } 78 | 79 | #endif //ESTIMATION_SLAM_SLAM_HANDLE_HPP 80 | -------------------------------------------------------------------------------- /src/2_estimation/velocity_estimator/src/velocity_estimator_handle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "velocity_estimator_handle.hpp" 22 | 23 | namespace ns_velocity_estimation { 24 | 25 | // Constructor 26 | VelocityEstimatorHandle::VelocityEstimatorHandle(ros::NodeHandle &nodeHandle) : 27 | nodeHandle_(nodeHandle) { 28 | ROS_INFO("Constructing Handle"); 29 | loadParameters(); 30 | publishToTopics(); 31 | } 32 | 33 | // Getters 34 | int VelocityEstimatorHandle::getNodeRate() const { return node_rate_; } 35 | 36 | // Methods 37 | void VelocityEstimatorHandle::loadParameters() { 38 | ROS_INFO("loading handle parameters"); 39 | if (!nodeHandle_.param("velocity_estimation_topic_name", 40 | velocity_estimation_topic_name_, 41 | "/estimation/velocity_estimation/velocity_estimate")) { 42 | ROS_WARN_STREAM("Did not load velocity_estimation_topic_name. Standard value is: " << velocity_estimation_topic_name_); 43 | } 44 | if (!nodeHandle_.param("node_rate", node_rate_, 1)) { 45 | ROS_WARN_STREAM("Did not load node_rate. Standard value is: " << node_rate_); 46 | } 47 | } 48 | 49 | void VelocityEstimatorHandle::publishToTopics() { 50 | ROS_INFO("publish to topics"); 51 | velocityEstimationPublisher = nodeHandle_.advertise(velocity_estimation_topic_name_, 1); 52 | } 53 | 54 | void VelocityEstimatorHandle::run() { 55 | velocityEstimator_.runAlgorithm(); 56 | sendVelocityEstimate(); 57 | } 58 | 59 | void VelocityEstimatorHandle::sendVelocityEstimate() { 60 | velocity_estimate_.car_state_dt.x = velocityEstimator_.getVelocityEstimate().x; 61 | velocity_estimate_.car_state_dt.y = velocityEstimator_.getVelocityEstimate().y; 62 | velocity_estimate_.car_state_dt.theta = velocityEstimator_.getVelocityEstimate().theta; 63 | velocity_estimate_.header.stamp = ros::Time::now(); 64 | velocityEstimationPublisher.publish(velocity_estimate_); 65 | ROS_INFO("Velocity estimate sent"); 66 | } 67 | } 68 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/visualization/rviz/autox.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 1107 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | Enabled: true 52 | Global Options: 53 | Background Color: 48; 48; 48 54 | Default Light: true 55 | Fixed Frame: map 56 | Frame Rate: 30 57 | Name: root 58 | Tools: 59 | - Class: rviz/Interact 60 | Hide Inactive Objects: true 61 | - Class: rviz/MoveCamera 62 | - Class: rviz/Select 63 | - Class: rviz/FocusCamera 64 | - Class: rviz/Measure 65 | - Class: rviz/SetInitialPose 66 | Topic: /initialpose 67 | - Class: rviz/SetGoal 68 | Topic: /move_base_simple/goal 69 | - Class: rviz/PublishPoint 70 | Single click: true 71 | Topic: /clicked_point 72 | Value: true 73 | Views: 74 | Current: 75 | Class: rviz/Orbit 76 | Distance: 10 77 | Enable Stereo Rendering: 78 | Stereo Eye Separation: 0.0599999987 79 | Stereo Focal Distance: 1 80 | Swap Stereo Eyes: false 81 | Value: false 82 | Focal Point: 83 | X: 0 84 | Y: 0 85 | Z: 0 86 | Focal Shape Fixed Size: true 87 | Focal Shape Size: 0.0500000007 88 | Invert Z Axis: false 89 | Name: Current View 90 | Near Clip Distance: 0.00999999978 91 | Pitch: 0.785398006 92 | Target Frame: 93 | Value: Orbit (rviz) 94 | Yaw: 0.785398006 95 | Saved: ~ 96 | Window Geometry: 97 | Displays: 98 | collapsed: false 99 | Height: 1388 100 | Hide Left Dock: false 101 | Hide Right Dock: false 102 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000004e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000004e2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000004e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e00000003efc0100000002fb0000000800540069006d00650100000000000004e00000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000025b000004e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 103 | Selection: 104 | collapsed: false 105 | Time: 106 | collapsed: false 107 | Tool Properties: 108 | collapsed: false 109 | Views: 110 | collapsed: false 111 | Width: 1248 112 | X: 65 113 | Y: 24 114 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/visualization/rviz/skidpad.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 1107 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | Enabled: true 52 | Global Options: 53 | Background Color: 48; 48; 48 54 | Default Light: true 55 | Fixed Frame: map 56 | Frame Rate: 30 57 | Name: root 58 | Tools: 59 | - Class: rviz/Interact 60 | Hide Inactive Objects: true 61 | - Class: rviz/MoveCamera 62 | - Class: rviz/Select 63 | - Class: rviz/FocusCamera 64 | - Class: rviz/Measure 65 | - Class: rviz/SetInitialPose 66 | Topic: /initialpose 67 | - Class: rviz/SetGoal 68 | Topic: /move_base_simple/goal 69 | - Class: rviz/PublishPoint 70 | Single click: true 71 | Topic: /clicked_point 72 | Value: true 73 | Views: 74 | Current: 75 | Class: rviz/Orbit 76 | Distance: 10 77 | Enable Stereo Rendering: 78 | Stereo Eye Separation: 0.0599999987 79 | Stereo Focal Distance: 1 80 | Swap Stereo Eyes: false 81 | Value: false 82 | Focal Point: 83 | X: 0 84 | Y: 0 85 | Z: 0 86 | Focal Shape Fixed Size: true 87 | Focal Shape Size: 0.0500000007 88 | Invert Z Axis: false 89 | Name: Current View 90 | Near Clip Distance: 0.00999999978 91 | Pitch: 0.785398006 92 | Target Frame: 93 | Value: Orbit (rviz) 94 | Yaw: 0.785398006 95 | Saved: ~ 96 | Window Geometry: 97 | Displays: 98 | collapsed: false 99 | Height: 1388 100 | Hide Left Dock: false 101 | Hide Right Dock: false 102 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000004e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000004e2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000004e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e00000003efc0100000002fb0000000800540069006d00650100000000000004e00000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000025b000004e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 103 | Selection: 104 | collapsed: false 105 | Time: 106 | collapsed: false 107 | Tool Properties: 108 | collapsed: false 109 | Views: 110 | collapsed: false 111 | Width: 1248 112 | X: 65 113 | Y: 24 114 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/visualization/rviz/acceleration.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 1107 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | Enabled: true 52 | Global Options: 53 | Background Color: 48; 48; 48 54 | Default Light: true 55 | Fixed Frame: map 56 | Frame Rate: 30 57 | Name: root 58 | Tools: 59 | - Class: rviz/Interact 60 | Hide Inactive Objects: true 61 | - Class: rviz/MoveCamera 62 | - Class: rviz/Select 63 | - Class: rviz/FocusCamera 64 | - Class: rviz/Measure 65 | - Class: rviz/SetInitialPose 66 | Topic: /initialpose 67 | - Class: rviz/SetGoal 68 | Topic: /move_base_simple/goal 69 | - Class: rviz/PublishPoint 70 | Single click: true 71 | Topic: /clicked_point 72 | Value: true 73 | Views: 74 | Current: 75 | Class: rviz/Orbit 76 | Distance: 10 77 | Enable Stereo Rendering: 78 | Stereo Eye Separation: 0.0599999987 79 | Stereo Focal Distance: 1 80 | Swap Stereo Eyes: false 81 | Value: false 82 | Focal Point: 83 | X: 0 84 | Y: 0 85 | Z: 0 86 | Focal Shape Fixed Size: true 87 | Focal Shape Size: 0.0500000007 88 | Invert Z Axis: false 89 | Name: Current View 90 | Near Clip Distance: 0.00999999978 91 | Pitch: 0.785398006 92 | Target Frame: 93 | Value: Orbit (rviz) 94 | Yaw: 0.785398006 95 | Saved: ~ 96 | Window Geometry: 97 | Displays: 98 | collapsed: false 99 | Height: 1388 100 | Hide Left Dock: false 101 | Hide Right Dock: false 102 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000004e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000004e2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000004e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e00000003efc0100000002fb0000000800540069006d00650100000000000004e00000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000025b000004e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 103 | Selection: 104 | collapsed: false 105 | Time: 106 | collapsed: false 107 | Tool Properties: 108 | collapsed: false 109 | Views: 110 | collapsed: false 111 | Width: 1248 112 | X: 65 113 | Y: 24 114 | -------------------------------------------------------------------------------- /src/0_fsd_common/fsd_common_meta/visualization/rviz/trackdrive.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 1107 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | Enabled: true 52 | Global Options: 53 | Background Color: 48; 48; 48 54 | Default Light: true 55 | Fixed Frame: map 56 | Frame Rate: 30 57 | Name: root 58 | Tools: 59 | - Class: rviz/Interact 60 | Hide Inactive Objects: true 61 | - Class: rviz/MoveCamera 62 | - Class: rviz/Select 63 | - Class: rviz/FocusCamera 64 | - Class: rviz/Measure 65 | - Class: rviz/SetInitialPose 66 | Topic: /initialpose 67 | - Class: rviz/SetGoal 68 | Topic: /move_base_simple/goal 69 | - Class: rviz/PublishPoint 70 | Single click: true 71 | Topic: /clicked_point 72 | Value: true 73 | Views: 74 | Current: 75 | Class: rviz/Orbit 76 | Distance: 10 77 | Enable Stereo Rendering: 78 | Stereo Eye Separation: 0.0599999987 79 | Stereo Focal Distance: 1 80 | Swap Stereo Eyes: false 81 | Value: false 82 | Focal Point: 83 | X: 0 84 | Y: 0 85 | Z: 0 86 | Focal Shape Fixed Size: true 87 | Focal Shape Size: 0.0500000007 88 | Invert Z Axis: false 89 | Name: Current View 90 | Near Clip Distance: 0.00999999978 91 | Pitch: 0.785398006 92 | Target Frame: 93 | Value: Orbit (rviz) 94 | Yaw: 0.785398006 95 | Saved: ~ 96 | Window Geometry: 97 | Displays: 98 | collapsed: false 99 | Height: 1388 100 | Hide Left Dock: false 101 | Hide Right Dock: false 102 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000004e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000004e2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000004e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e00000003efc0100000002fb0000000800540069006d00650100000000000004e00000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000025b000004e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 103 | Selection: 104 | collapsed: false 105 | Time: 106 | collapsed: false 107 | Tool Properties: 108 | collapsed: false 109 | Views: 110 | collapsed: false 111 | Width: 1248 112 | X: 65 113 | Y: 24 114 | -------------------------------------------------------------------------------- /src/fssim_interface/include/interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AMZ-Driverless 3 | * Copyright (c) 2018 Authors: 4 | * - Juraj Kabzan 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | 25 | #ifndef FSSIM_INTERFACE_INTERFACE_HPP 26 | #define FSSIM_INTERFACE_INTERFACE_HPP 27 | 28 | // TF Includes 29 | #include 30 | 31 | // ROS Messages 32 | #include "nav_msgs/Odometry.h" 33 | 34 | // FSD_CAR MSGS 35 | #include "fsd_common_msgs/CarState.h" 36 | #include "fsd_common_msgs/CarStateDt.h" 37 | #include "fsd_common_msgs/Map.h" 38 | #include "fsd_common_msgs/ControlCommand.h" 39 | #include "fsd_common_msgs/Mission.h" 40 | 41 | // FSSIM Messages 42 | #include "fssim_common/Cmd.h" 43 | #include "fssim_common/State.h" 44 | #include "fssim_common/Track.h" 45 | #include "fssim_common/Mission.h" 46 | 47 | namespace fssim { 48 | fssim_common::Cmd getFssimCmd(const fsd_common_msgs::ControlCommand &msg) { 49 | fssim_common::Cmd cmd; 50 | cmd.dc = msg.throttle.data; 51 | cmd.delta = msg.steering_angle.data; 52 | return cmd; 53 | } 54 | 55 | fssim_common::Mission getFssimMissionFinnished(const fsd_common_msgs::Mission &msg) { 56 | fssim_common::Mission mis; 57 | mis.finished = msg.finished; 58 | mis.mission = msg.mission; 59 | return mis; 60 | } 61 | 62 | } // namespace fssim 63 | 64 | namespace gotthard { 65 | 66 | fsd_common_msgs::CarStateDt getStateDt(const nav_msgs::Odometry &odom) { 67 | fsd_common_msgs::CarStateDt msg; 68 | msg.header = odom.header; 69 | msg.car_state_dt.x = odom.twist.twist.linear.x; 70 | msg.car_state_dt.y = odom.twist.twist.linear.y; 71 | msg.car_state_dt.theta = odom.twist.twist.angular.z; 72 | return msg; 73 | } 74 | 75 | fsd_common_msgs::CarStateDt getStateDt(const fssim_common::State &odom) { 76 | fsd_common_msgs::CarStateDt msg; 77 | msg.header = odom.header; 78 | msg.car_state_dt.x = odom.vx; 79 | msg.car_state_dt.y = odom.vy; 80 | msg.car_state_dt.theta = odom.r; 81 | return msg; 82 | } 83 | 84 | fsd_common_msgs::CarState getState(const fssim_common::State &odom) { 85 | fsd_common_msgs::CarState msg; 86 | msg.header = odom.header; 87 | msg.car_state.x = odom.x; 88 | msg.car_state.y = odom.y; 89 | msg.car_state.theta = odom.yaw; 90 | msg.car_state_dt = getStateDt(odom); 91 | return msg; 92 | } 93 | 94 | fsd_common_msgs::Cone getConeFromPoint(const geometry_msgs::Point &p, const std::string &color) { 95 | fsd_common_msgs::Cone cone; 96 | cone.color.data = color; 97 | cone.position.x = p.x; 98 | cone.position.y = p.y; 99 | cone.position.z = p.z; 100 | return cone; 101 | } 102 | 103 | fsd_common_msgs::Map getMap(const fssim_common::Track &track) { 104 | fsd_common_msgs::Map msg; 105 | msg.header = track.header; 106 | msg.cone_yellow.clear(); 107 | for (const geometry_msgs::Point &c : track.cones_left) { 108 | msg.cone_yellow.push_back(getConeFromPoint(c, "yellow")); 109 | } 110 | 111 | msg.cone_blue.clear(); 112 | for (const geometry_msgs::Point &c : track.cones_right) { 113 | msg.cone_blue.push_back(getConeFromPoint(c, "blue")); 114 | } 115 | return msg; 116 | } 117 | 118 | } // namespace gotthard 119 | 120 | #endif //FSSIM_INTERFACE_INTERFACE_HPP 121 | -------------------------------------------------------------------------------- /src/fssim_interface/src/fssim_interface_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * AMZ-Driverless 3 | * Copyright (c) 2018 Authors: 4 | * - Juraj Kabzan 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | */ 24 | 25 | // ROS Includes 26 | #include "ros/ros.h" 27 | 28 | // TF 29 | #include "tf/transform_broadcaster.h" 30 | 31 | #include "interface.hpp" 32 | 33 | // FSSIM 34 | static ros::Subscriber sub_fssim_odom; 35 | static ros::Subscriber sub_fssim_res; 36 | static ros::Subscriber sub_fssim_track; 37 | 38 | static ros::Publisher pub_fssim_cmd; 39 | static ros::Publisher pub_fssim_mission_finnished; 40 | 41 | // FSD_CAR 42 | static ros::Subscriber sub_fsd_car_command; 43 | static ros::Subscriber sub_fsd_mission_finnished; 44 | 45 | static ros::Publisher pub_fsd_vel; 46 | static ros::Publisher pub_fsd_state; 47 | static ros::Publisher pub_fsd_map; 48 | static ros::Publisher pub_gotthard_res; 49 | 50 | static bool tf_base_link = false; 51 | static std::string fsd_vehicle; 52 | static std::string origin; 53 | static tf::TransformBroadcaster *br = nullptr; 54 | 55 | void callbackFssimOdom(const fssim_common::State::ConstPtr &msg) { 56 | pub_fsd_vel.publish(gotthard::getStateDt(*msg)); 57 | pub_fsd_state.publish(gotthard::getState(*msg)); 58 | 59 | if (tf_base_link) { 60 | tf::Transform transform; 61 | transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0)); 62 | tf::Quaternion q; 63 | q.setRPY(0, 0, msg->yaw); 64 | transform.setRotation(q); 65 | br->sendTransform(tf::StampedTransform(transform, msg->header.stamp, origin, fsd_vehicle)); 66 | } 67 | } 68 | 69 | void callbackFssimTrack(const fssim_common::Track::ConstPtr &msg) { 70 | pub_fsd_map.publish(gotthard::getMap(*msg)); 71 | } 72 | 73 | void callbackFsdCmd(const fsd_common_msgs::ControlCommand::ConstPtr &msg) { 74 | pub_fssim_cmd.publish(fssim::getFssimCmd(*msg)); 75 | } 76 | 77 | void callbackFsdMissionFinnished(const fsd_common_msgs::Mission::ConstPtr &msg) { 78 | pub_fssim_mission_finnished.publish(fssim::getFssimMissionFinnished(*msg)); 79 | } 80 | 81 | template 82 | Type getParam(const ros::NodeHandle &nh, const std::string &name) { 83 | Type val; 84 | const bool success = nh.getParam(name, val); 85 | assert(success && "PARAMETER DOES NOT EXIST"); 86 | return val; 87 | } 88 | 89 | int main(int argc, char **argv) { 90 | ros::init(argc, argv, "fssim_interface"); 91 | ros::NodeHandle n("~"); 92 | 93 | br = new tf::TransformBroadcaster(); 94 | 95 | sub_fsd_car_command = n.subscribe(getParam(n, "fsd/cmd"), 1, callbackFsdCmd); 96 | sub_fsd_mission_finnished = 97 | n.subscribe(getParam(n, "fsd/mission_finnished"), 1, callbackFsdMissionFinnished); 98 | 99 | pub_fsd_vel = n.advertise(getParam(n, "fsd/vel"), 1); 100 | pub_fsd_state = n.advertise(getParam(n, "fsd/state"), 1); 101 | pub_fsd_map = n.advertise(getParam(n, "fsd/map"), 1, true); 102 | 103 | sub_fssim_odom = n.subscribe(getParam(n, "fssim/topic_odom"), 1, callbackFssimOdom); 104 | sub_fssim_track = n.subscribe(getParam(n, "fssim/track"), 1, callbackFssimTrack); 105 | 106 | pub_fssim_cmd = n.advertise(getParam(n, "fssim/cmd"), 1); 107 | pub_fssim_mission_finnished = 108 | n.advertise(getParam(n, "fssim/mission_finnished"), 1); 109 | 110 | tf_base_link = getParam(n, "fsd/tf/publish_car_base_link"); 111 | origin = getParam(n, "fsd/tf/origin"); 112 | fsd_vehicle = getParam(n, "fsd/tf/fsd_car_base_link"); 113 | 114 | ros::spin(); 115 | return 0; 116 | } 117 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # FS Driverless 2019 2 | 3 | This is a skeleton repository for the autonomous code of a driverless FS race car. This README contains some information to get you started. 4 | 5 | The use of **Ubuntu 16.04 and ROS Kinetic** is assumed everywhere on this repository. 6 | 7 | This repository contains a framework for the architecture of an autonomous FS race car, with basic dummy nodes to illustrate how to organise code. Some helpful tools are also included. 8 | 9 | # Repository organisation 10 | 11 | The code is organised in several top level packages/directories. The top level should adhere to the following subdivision of functionality (a more detailed description can be found in the folders themselves): 12 | 13 | **fsd_common/** - top-level launchfiles, and other files used by many packages 14 | 15 | **perception/** - folder for perception packages 16 | 17 | **estimation/** - folder for estimation packages 18 | 19 | **control/** - folder for control packages 20 | 21 | - - - - 22 | 23 | # Placement of ROS packages 24 | ROS Packages should be added in one of the top level work-package folders. The top level work-package folders themselves should not be used to store ros package information. The directory tree for lidar would look like: 25 | 26 | ``` 27 | ~/fsd_skeleton 28 | |__ environment.sh 29 | |__ update_dependencies.sh 30 | |__ src 31 | |__ 0_fsd_common 32 | | |__ fsd_common_meta 33 | | | |__ missions 34 | | |__ fsd_common_msgs 35 | |__ 1_perception 36 | | |__ perception_meta 37 | | |__ lidar_cone_detection 38 | | | |__ package.xml 39 | | | |__ CMakeLists.txt 40 | | | |__ ... 41 | | 42 | |__ 2_estimation 43 | | |__ estimation_meta 44 | | |__ velocity_estimator 45 | | 46 | |__ 3_control 47 | ``` 48 | - - - - 49 | 50 | # Aliases (useful commands) 51 | Once running `update_dependencies`, some aliases for commands will be added. Restart the terminal and the following commands will be available: 52 | * `FSD_source`: sources environment from anywhere 53 | * `FSD_cd`: change directory to root directory of skeleton_repo 54 | * `FSD_build`: clean and build project (catkin clean and catkin build) 55 | * `FSD_launch_acceleration`: launch mission, e.g. acceleration, trackdrive, autox etc 56 | * `FSD_rviz_acceleration`: launch RVIZ with custom config for mission, e.g. acceleration, trackdrive, autox etc 57 | * `FSD_ATS`: run automated test 58 | 59 | Look at`fsd_aliases` to see full list, or add more custom aliases. 60 | - - - - 61 | 62 | # Setting up the Workspace 63 | **1 Clone the repository:** 64 | 65 | ``` 66 | cd ~ 67 | git clone git@github.com:AMZ-Driverless/fsd_skeleton.git 68 | ``` 69 | **2 Install dependencies** 70 | ``` 71 | cd ~/fsd_skeleton 72 | ./update_dependencies.sh 73 | ``` 74 | 75 | **3 Build workspace** 76 | ``` 77 | cd ~/fsd_skeleton 78 | catkin build 79 | ``` 80 | 81 | **4 Source environment** 82 | 83 | Assuming you've run `./update_dependencies.sh` succesfully and restarted the terminal. 84 | ``` 85 | FSD_source 86 | ``` 87 | Else, 88 | ``` 89 | cd ~/fsd_skeleton 90 | source fsd_environment.sh 91 | ``` 92 | 93 | **5 Test setup** 94 | ``` 95 | roslaunch fsd_common_meta trackdrive.launch 96 | ``` 97 | in new terminal 98 | ``` 99 | rqt_graph 100 | ``` 101 | You should see all the nodes int the pipeline running 102 | - - - - 103 | 104 | # Run the workspace with FSSIM 105 | * see https://github.com/AMZ-Driverless/fssim#combine-it-with-simple-fsd-skeleton-framework-and-drive-a-lap 106 | 107 | # Conventions 108 | - - - - 109 | ## ROS naming conventions 110 | We use the naming conventions defined at http://wiki.ros.org/ROS/Patterns/Conventions 111 | ### Work packages: 112 | `work_package`, lowercase and `_` as separator, e.g. `lidar`. 113 | ### ROS packages: 114 | `workpackage_somename`, lowercase and `_` as separator, e.g. `lidar_trimmer`, as to make it clear what the package is used for. 115 | ### ROS nodes 116 | `node_name`, lowercase and `_` as separator. Can be short. 117 | ### ROS topics 118 | `topic_name`, lowercase and `_` as separator. 119 | ### ROS messages 120 | `CamelCased.msg` for message filenames. Message types are always CamelCase, whereas message fields are lowercase and `_` as separator, e.g. 121 | ``` 122 | MyMessage.msg: 123 | Header header 124 | Float64 my_float 125 | geometry_msgs/Point my_point 126 | ``` 127 | 128 | ## Style guides 129 | ### ROS C++: 130 | Google Style (http://wiki.ros.org/CppStyleGuide) 131 | 132 | * Files: `under_scored`, exception for `.msg` files, `CMakeLists.txt`. 133 | * Classes/types: `CamelCase` 134 | * Functions/methods: `camelCase` 135 | * Variables: `under_scored` and DESCRIPTIVE. 136 | * Constants: `ALL_CAPITALS`. 137 | * Global variables: AVOID except special cases. Rather have parameters defined in `config.yaml`. 138 | 139 | ### ROS Python 140 | PEP-8 style (http://wiki.ros.org/PyStyleGuide) 141 | 142 | ### README files 143 | Markdown syntax (https://github.com/adam-p/markdown-here/wiki/Markdown-Cheatsheet) 144 | 145 | # Future improvements 146 | 147 | * Better dependency management example and documentation 148 | * Jenkins setup 149 | * RVIZ setup 150 | * ... 151 | -------------------------------------------------------------------------------- /src/2_estimation/slam_slam/src/slam_handle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | 6 | FSD-Project is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | FSD-Project is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with FSD-Project. If not, see . 18 | */ 19 | 20 | #include 21 | #include "slam_handle.hpp" 22 | 23 | namespace ns_slam { 24 | 25 | // Constructor 26 | SlamHandle::SlamHandle(ros::NodeHandle &nodeHandle) : 27 | nodeHandle_(nodeHandle) { 28 | ROS_INFO("Constructing Handle"); 29 | loadParameters(); 30 | slam_.setMaxMapSize(max_map_size_); 31 | subscribeToTopics(); 32 | publishToTopics(); 33 | } 34 | 35 | // Getters 36 | int SlamHandle::getNodeRate() const { return node_rate_; } 37 | 38 | // Methods 39 | void SlamHandle::loadParameters() { 40 | ROS_INFO("loading handle parameters"); 41 | if (!nodeHandle_.param("vision_cone_detections_topic_name", 42 | vision_cone_detections_topic_name_, 43 | "/perception/vision/cone_detections")) { 44 | ROS_WARN_STREAM("Did not load vision_cone_detections_topic_name. Standard value is: " << vision_cone_detections_topic_name_); 45 | } 46 | if (!nodeHandle_.param("lidar_cone_detections_topic_name", 47 | lidar_cone_detections_topic_name_, 48 | "/perception/lidar/cone_detections")) { 49 | ROS_WARN_STREAM("Did not load lidar_cone_detections_topic_name. Standard value is: " << lidar_cone_detections_topic_name_); 50 | } 51 | if (!nodeHandle_.param("velocity_estimate_topic_name", 52 | velocity_estimate_topic_name_, 53 | "/estimation/velocity_estimation/velocity_estimate")) { 54 | ROS_WARN_STREAM("Did not load velocity_estimate_topic_name. Standard value is: " << velocity_estimate_topic_name_); 55 | } 56 | if (!nodeHandle_.param("slam_map_topic_name", 57 | slam_map_topic_name_, 58 | "/estimation/slam/map")) { 59 | ROS_WARN_STREAM("Did not load slam_map_topic_name. Standard value is: " << slam_map_topic_name_); 60 | } 61 | if (!nodeHandle_.param("slam_state_topic_name", 62 | slam_state_topic_name_, 63 | "/estimation/slam/state")) { 64 | ROS_WARN_STREAM("Did not load slam_state_topic_name. Standard value is: " << slam_state_topic_name_); 65 | } 66 | if (!nodeHandle_.param("max_map_size", max_map_size_, 30)) { 67 | ROS_WARN_STREAM("Did not load max_map_size. Standard value is: " << max_map_size_); 68 | } 69 | if (!nodeHandle_.param("node_rate", node_rate_, 1)) { 70 | ROS_WARN_STREAM("Did not load node_rate. Standard value is: " << node_rate_); 71 | } 72 | } 73 | 74 | void SlamHandle::subscribeToTopics() { 75 | ROS_INFO("subscribe to topics"); 76 | visionConeDetectionsSubscriber_ = nodeHandle_.subscribe(vision_cone_detections_topic_name_, 1, &SlamHandle::visionConeDetectionsCallback, this); 77 | lidarConeDetectionsSubscriber_ = nodeHandle_.subscribe(lidar_cone_detections_topic_name_, 1, &SlamHandle::lidarConeDetectionsCallback, this); 78 | velocityEstimateSubscriber_ = nodeHandle_.subscribe(velocity_estimate_topic_name_, 1, &SlamHandle::velocityEstimateCallback, this); 79 | } 80 | 81 | void SlamHandle::publishToTopics() { 82 | ROS_INFO("publish to topics"); 83 | slamMapPublisher_ = nodeHandle_.advertise(slam_map_topic_name_, 1); 84 | slamStatePublisher_ = nodeHandle_.advertise(slam_state_topic_name_, 1); 85 | } 86 | 87 | void SlamHandle::run() { 88 | sendMap(); 89 | sendState(); 90 | } 91 | 92 | void SlamHandle::sendMap() { 93 | slam_map_.cone_blue = slam_.getMap(); 94 | slam_map_.header.stamp = ros::Time::now(); 95 | slamMapPublisher_.publish(slam_map_); 96 | ROS_INFO("SLAM map sent"); 97 | } 98 | 99 | void SlamHandle::sendState() { 100 | slam_state_.car_state.x = slam_.getState().x; 101 | slam_state_.car_state.y = slam_.getState().y; 102 | slam_state_.car_state.theta = slam_.getState().theta; 103 | slam_state_.header.stamp = ros::Time::now(); 104 | slamStatePublisher_.publish(slam_state_); 105 | ROS_INFO("SLAM state sent"); 106 | } 107 | 108 | void SlamHandle::lidarConeDetectionsCallback(const fsd_common_msgs::ConeDetections &cones) { 109 | ROS_INFO("Updating map with LiDAR cones"); 110 | slam_.updateMap(cones); 111 | } 112 | 113 | void SlamHandle::visionConeDetectionsCallback(const fsd_common_msgs::ConeDetections &cones) { 114 | ROS_INFO("Updating map with vision cones"); 115 | slam_.updateMap(cones); 116 | sendMap(); 117 | } 118 | 119 | void SlamHandle::velocityEstimateCallback(const fsd_common_msgs::CarStateDt &velocity) { 120 | slam_.calculateState(velocity); 121 | } 122 | } 123 | -------------------------------------------------------------------------------- /src/3_control/control_pure_pursuit/src/pure_pursuit.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | - Juraj Kabzan 6 | 7 | FSD-Project is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | FSD-Project is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with FSD-Project. If not, see . 19 | */ 20 | 21 | #include 22 | #include "pure_pursuit.hpp" 23 | #include 24 | 25 | // ROS Msgs 26 | #include "visualization_msgs/MarkerArray.h" 27 | 28 | namespace ns_pure_pursuit { 29 | // Constructor 30 | PurePursuit::PurePursuit(ros::NodeHandle &nh) : nh_(nh) { 31 | pub_closest_point_ = nh.advertise("/control/pure_pursuit/marker", 1); 32 | 33 | if (!nh.param("controller/speed/p", speed_p, 0.01)) { 34 | ROS_WARN_STREAM("Did not load controller/speed/p. Standard value is: " << 0.01); 35 | } 36 | if (!nh.param("controller/steering/p", steering_p, 0.01)) { 37 | ROS_WARN_STREAM("Did not load controller/steering/p. Standard value is: " << 0.01); 38 | } 39 | }; 40 | 41 | // Getters 42 | fsd_common_msgs::ControlCommand PurePursuit::getControlCommand() const { return control_command_; } 43 | 44 | // Setters 45 | void PurePursuit::setMaxSpeed(double &max_speed) { 46 | max_speed_ = max_speed; 47 | } 48 | 49 | void PurePursuit::setCenterLine(const geometry_msgs::Polygon ¢er_line) { 50 | center_line_ = center_line; 51 | } 52 | 53 | void PurePursuit::setState(const fsd_common_msgs::CarState &state) { 54 | state_ = state; 55 | } 56 | 57 | void PurePursuit::setVelocity(const fsd_common_msgs::CarStateDt &velocity) { 58 | velocity_ = velocity; 59 | } 60 | 61 | void PurePursuit::runAlgorithm() { 62 | createControlCommand(); 63 | } 64 | 65 | void PurePursuit::createControlCommand() { 66 | 67 | if (center_line_.points.empty()) { 68 | control_command_.throttle.data = static_cast(-1.0); 69 | control_command_.steering_angle.data = 0.0; 70 | return; 71 | } 72 | 73 | const auto it_center_line = std::min_element(center_line_.points.begin(), center_line_.points.end(), 74 | [&](const geometry_msgs::Point32 &a, 75 | const geometry_msgs::Point32 &b) { 76 | const double da = std::hypot(state_.car_state.x - a.x, 77 | state_.car_state.y - a.y); 78 | const double db = std::hypot(state_.car_state.x - b.x, 79 | state_.car_state.y - b.y); 80 | 81 | return da < db; 82 | }); 83 | const auto i_center_line = std::distance(center_line_.points.begin(), it_center_line); 84 | const auto size = center_line_.points.size(); 85 | const auto i_next = (i_center_line + 10) % size; 86 | geometry_msgs::Point32 next_point = center_line_.points[i_next]; 87 | 88 | { // Steering Control 89 | const double beta_est = control_command_.steering_angle.data * 0.5; 90 | const double eta = std::atan2(next_point.y - state_.car_state.y, next_point.x - state_.car_state.x) 91 | - (state_.car_state.theta + beta_est); 92 | const double length = std::hypot(next_point.y - state_.car_state.y, next_point.x - state_.car_state.x); 93 | control_command_.steering_angle.data = static_cast(steering_p * std::atan(2.0 / length * std::sin(eta))); 94 | 95 | } 96 | { // Speed Controller 97 | const double vel = std::hypot(state_.car_state_dt.car_state_dt.x, state_.car_state_dt.car_state_dt.y); 98 | control_command_.throttle.data = static_cast(speed_p * (max_speed_ - vel)); 99 | } 100 | 101 | // Visualize 102 | publishMarkers(it_center_line->x, it_center_line->y, next_point.x, next_point.y); 103 | 104 | } 105 | 106 | void PurePursuit::publishMarkers(double x_pos, double y_pos, double x_next, double y_next) const{ 107 | visualization_msgs::MarkerArray markers; 108 | 109 | visualization_msgs::Marker marker; 110 | marker.color.r = 1.0; 111 | marker.color.a = 1.0; 112 | marker.pose.position.x = x_pos; 113 | marker.pose.position.y = y_pos; 114 | marker.pose.orientation.w = 1.0; 115 | marker.type = visualization_msgs::Marker::SPHERE; 116 | marker.action = visualization_msgs::Marker::ADD; 117 | marker.id = 0; 118 | marker.scale.x = 0.5; 119 | marker.scale.y = 0.5; 120 | marker.scale.z = 0.5; 121 | marker.header.stamp = ros::Time::now(); 122 | marker.header.frame_id = "map"; 123 | markers.markers.push_back(marker); 124 | 125 | marker.pose.position.x = x_next; 126 | marker.pose.position.y = y_next; 127 | marker.color.b = 1.0; 128 | marker.id = 1; 129 | markers.markers.push_back(marker); 130 | 131 | pub_closest_point_.publish(markers); 132 | } 133 | 134 | } 135 | -------------------------------------------------------------------------------- /src/fssim_interface/fssim_config/sensors/sensors_1.yaml: -------------------------------------------------------------------------------- 1 | # AMZ-Driverless 2 | # Copyright (c) 2018 Authors: 3 | # - Juraj Kabzan 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | # All positions (pos) are with respect to chassis origin 24 | 25 | stream_track: 26 | enabled: true 27 | markers: "/fssim/track" 28 | 29 | 30 | sensors: 31 | lidar: 32 | enabled: false 33 | topic: lidar/raw 34 | pos: 35 | x: 0.75 36 | y: 0.0 37 | z: 0.15 38 | 39 | velocity: 40 | noise_vx_sigma: 0.01 # Sigma of gaussian noise on respectible velocity estimate 41 | noise_vy_sigma: 0.01 # Sigma of gaussian noise on respectible velocity estimate 42 | noise_r_sigma: 0.01 # Sigma of gaussian noise on respectible velocity estimate 43 | 44 | lidar_pipeline: 45 | enabled: true 46 | node_name: lidar_pipeline # This name MUST BE the SAME to this section yaml file 47 | topic_name: /lidar/cones # Where should the cones be published 48 | overwrite: 49 | enabled: true 50 | frame: gotthard/base_link # Frame the cones are represented in 51 | rate: 10 # Publishing frequency [Hz] 52 | delay: 53 | time: 0.2 # Delay [s] 54 | noise_sigma: 0.002 # Additive gassian noise on delay 55 | cut_cones_below_x: -1 # Cut cones that are further than this distance in x car axis 56 | 57 | observation_radius: 15.0 # Below this threshold we can observe cones [m] 58 | distance_dependent_observation: 100.0 # The probability of observation decreases with factor = -1/distance_dependent_misclass * r + 1 59 | observation_likelihood_left: 1.0 # Likelihood of observing left [0..1] 60 | observation_likelihood_right: 1.0 # Likelihood of observing right [0..1] 61 | observation_likelihood_orange: 1.0 # Likelihood of observing right [0..1] 62 | 63 | color_observation_radius: 10.0 # We can obser color only below this threshild [m] 64 | distance_dependent_misclass: 200.0 # The probability of color observation decreases with factor = -1/distance_dependent_misclass * r + 1 65 | likelihood_yellow: 0.8 # Likelihood of observic color of cones left [0..1] 66 | likelihood_blue: 0.8 # Likelihood of observic color of cones right [0..1] 67 | likelihood_orange: 0.8 # Likelihood of observic color of cones right [0..1] 68 | 69 | gaussian_noise_xy_mu: 0.0 # Mean of Gaussian noise on [x,y] cone position 70 | gaussian_noise_xy_sigma: 0.0 # Variance of Gaussian noise on [x,y] cone position 71 | 72 | gaussian_noise_mu_radial: 0.0 # Mean of Gaussian noise on r (cone position is described with [r,\theta]) cone position (radial coordinates) 73 | gaussian_noise_sigma_radial: 0.05 # Variance of Gaussian noise on r (cone position is described with [r,\theta]) cone position (radial coordinates) 74 | 75 | gaussian_noise_mu_angular: 0.00 # Mean of Gaussian noise on \theta (cone position is described with [r,\theta]) cone position (radial coordinates) 76 | gaussian_noise_sigma_angular: 0.007 # Variance of Gaussian noise on r (cone position is described with [r,\theta]) cone position (radial coordinates) 77 | 78 | camera_pipeline: 79 | enabled: true 80 | node_name: camera_pipeline 81 | topic_name: /camera/cones 82 | overwrite: 83 | enabled: true 84 | frame: gotthard/base_link # Frame the cones are represented in 85 | rate: 10 # Publishing frequency [Hz] 86 | delay: 87 | time: 0.2 # Delay [s] 88 | noise_sigma: 0.002 # Additive gassian noise on delay 89 | cut_cones_below_x: -1 # Cut cones that are further than this distance in x car axis 90 | observation_radius: 10.0 # Below this threshold we can observe cones [m] 91 | distance_dependent_observation: 100.0 # The probability of observation decreases with factor = -1/distance_dependent_misclass * r + 1 92 | observation_likelihood_left: 1.0 # Likelihood of observing left [0..1] 93 | observation_likelihood_right: 1.0 # Likelihood of observing right [0..1] 94 | observation_likelihood_orange: 1.0 # Likelihood of observing right [0..1] 95 | 96 | color_observation_radius: 10.0 # We can obser color only below this threshild [m] 97 | distance_dependent_misclass: 200.0 # The probability of color observation decreases with factor = -1/distance_dependent_misclass * r + 1 98 | likelihood_yellow: 0.99 # Likelihood of observic color of cones left [0..1] 99 | likelihood_blue: 0.99 # Likelihood of observic color of cones right [0..1] 100 | likelihood_orange: 0.99 # Likelihood of observic color of cones right [0..1] 101 | 102 | gaussian_noise_xy_mu: 0.0 # Mean of Gaussian noise on [x,y] cone position 103 | gaussian_noise_xy_sigma: 0.0 # Variance of Gaussian noise on [x,y] cone position 104 | 105 | gaussian_noise_mu_radial: 0.0 # Mean of Gaussian noise on r (cone position is described with [r,\theta]) cone position (radial coordinates) 106 | gaussian_noise_sigma_radial: 0.2 # Variance of Gaussian noise on r (cone position is described with [r,\theta]) cone position (radial coordinates) 107 | 108 | gaussian_noise_mu_angular: 0.00 # Mean of Gaussian noise on \theta (cone position is described with [r,\theta]) cone position (radial coordinates) 109 | gaussian_noise_sigma_angular: 0.007 # Variance of Gaussian noise on r (cone position is described with [r,\theta]) cone position (radial coordinates) 110 | -------------------------------------------------------------------------------- /update_dependencies.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Sonja Brits - FSD Driverless 2019 4 | # Adapted from work by Huub Hendrikx 5 | 6 | # Apt packages 7 | 8 | WSTOOL_PACKAGE="python-wstool" 9 | 10 | CHECKINSTALL_PACKAGE="checkinstall" 11 | 12 | ROS_PACKAGE=( 13 | "ros-kinetic-desktop-full" 14 | "ros-kinetic-ros-base" 15 | ) 16 | 17 | FSD_WORKPACKAGES=( 18 | '1_perception' 19 | '2_estimation' 20 | '3_control' 21 | ) 22 | 23 | BLACKLIST_PACKAGES='' 24 | 25 | ################################### 26 | # Decide whether to include fssim # 27 | ################################### 28 | case $1 in 29 | -f|--fssim) 30 | FSSIM="TRUE" 31 | ;; 32 | esac 33 | 34 | printf "FSSIM is..." 35 | if [ -z $FSSIM ]; then 36 | echo "DISABLED" 37 | BLACKLIST_PACKAGES='fssim_interface fssim' 38 | else 39 | echo "ENABLED" 40 | FSD_WORKPACKAGES=("${FSD_WORKPACKAGES[@]}" 'fssim_interface') 41 | fi 42 | 43 | ##################################### 44 | # Set paths # 45 | ##################################### 46 | FSD_DEPENDENCY_FILE="dependencies.rosinstall" 47 | 48 | # TODO: check whether cpplint is installed 49 | CPPLINT_PACKAGE="cpplint" 50 | 51 | ABSOLUTE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" 52 | TOP_LEVEL_DIR_NAME="$(basename "${ABSOLUTE_PATH}")" 53 | FSD_ROOT_VAR=$( cd "$(dirname "${BASH_SOURCE[0]}" )" && pwd) 54 | FSD_ROSDEP_DIR="${ABSOLUTE_PATH}/src/4_continuous_integration/rosdep" 55 | ROSDEP_SOURCES_TARGET="/etc/ros/rosdep/sources.list.d" 56 | 57 | red=$'\e[1;31m' 58 | green=$'\e[1;32m' 59 | blue=$'\e[1;34m' 60 | end=$'\e[0m' 61 | 62 | printf "\n" 63 | printf "Formula Student Driverless dependency updater\n" 64 | printf "Version 2.0 - 2019" 65 | printf "\n\n" 66 | 67 | printf "First checking some things...\n" 68 | 69 | ######################################### 70 | # Check installed packages # 71 | ######################################### 72 | printf "Checking if ROS is installed... " 73 | package_OK=$(dpkg-query -W --showformat='${Status}\n' "${ROS_PACKAGE[0]}" 2> /dev/null|grep "install ok installed") 74 | 75 | if [ "" == "$package_OK" ]; then 76 | package_OK=$(dpkg-query -W --showformat='${Status}\n' "${ROS_PACKAGE[1]}" 2> /dev/null|grep "install ok installed") 77 | fi 78 | if [ "" == "$package_OK" ]; then 79 | printf "[${red}NO${end}]\n" 80 | printf "Please install the ROS using: 'sudo apt install ros-kinetic-desktop-full' or 'sudo apt install ros-kinetic-ros-base'\n" 81 | exit 1 82 | fi 83 | printf "[${green}YES${end}]\n" 84 | 85 | printf "Checking if checkinstall is installed... " 86 | package_OK=$(dpkg-query -W --showformat='${Status}\n' "${CHECKINSTALL_PACKAGE}" 2> /dev/null|grep "install ok installed") 87 | 88 | if [ "" == "$package_OK" ]; then 89 | printf "[${red}NO${end}]\n" 90 | printf "Please install the checkinstall package using: 'sudo apt install checkinstall'\n" 91 | exit 1 92 | fi 93 | 94 | printf "[${green}YES${end}]\n" 95 | 96 | ######################################### 97 | # Add nice aliases to ~/.bashrc # 98 | ######################################### 99 | if [ -z "$FSD_ROOT" ]; then 100 | printf "Setting Aliases and Variables..." 101 | 102 | if ! grep -Fxq "export FSD_ROOT=$FSD_ROOT_VAR" ~/.bashrc; then 103 | printf "\nAdding export FSD_ROOT to ~/.bashrc... " 104 | echo "" >> ~/.bashrc 105 | echo "# FSD 2019 Environment variables" >> ~/.bashrc 106 | echo "export FSD_ROOT=$FSD_ROOT_VAR" >> ~/.bashrc 107 | fi 108 | 109 | if ! grep -Fxq "# FSD aliases source" ~/.bashrc; then 110 | FSD_ROOT=${FSD_ROOT_VAR} 111 | printf "\nAdding alias commands to ~/.bashrc... " 112 | echo "" >> ~/.bashrc 113 | echo "source ${FSD_ROOT}/fsd_environment.sh" >> ~/.bashrc 114 | fi 115 | else 116 | printf "Aliases and Environment variables has been set... " 117 | fi 118 | printf "[${green}YES${end}]\n" 119 | printf "ALL CHECKS PASSED [${green}YES${end}]\n" 120 | 121 | ############################################ 122 | # Update the git repositories using wstool # 123 | ############################################ 124 | read -p "Do you want to get/update the repositories (Y/n)? " -n 1 -r 125 | echo 126 | if [[ $REPLY =~ ^[Yy]$ ]] || [ -z $REPLY ]; then 127 | 128 | if [ -e "${ABSOLUTE_PATH}/.rosinstall" ]; then 129 | printf "Rosinstall file found in top level directory! So we are removing it!\n" 130 | rm ${ABSOLUTE_PATH}/.rosinstall 131 | fi 132 | 133 | printf "We detected that your are running update_dependencies for the first time!\n" 134 | INIT_RESULT="$(cd "${ABSOLUTE_PATH}" && wstool init)" 135 | printf "${INIT_RESULT}\n" 136 | 137 | printf "Merging each workpackage into toplevel dependency list...\n" 138 | 139 | for i in ${FSD_WORKPACKAGES[@]}; do 140 | FSD_WP_DEPENDENCY_FILE="${ABSOLUTE_PATH}/src/${i}/${FSD_DEPENDENCY_FILE}" 141 | printf "${blue}\n${i}${end}\n" 142 | printf "Using dependencies in ${FSD_WP_DEPENDENCY_FILE}\n " 143 | WS_MERGE_RESULT="$(cd "${ABSOLUTE_PATH}" && wstool merge "${FSD_WP_DEPENDENCY_FILE}")" 144 | printf "${WS_MERGE_RESULT}" 145 | printf "\n" 146 | done 147 | 148 | printf "\n Updating all repositories now...\n" 149 | cd ${ABSOLUTE_PATH} 150 | wstool update 151 | 152 | fi 153 | 154 | ################################################### 155 | # Update the binary dependencies using rosdep # 156 | ################################################### 157 | read -p "Do you want to install/update the projects dependencies (Y/n)? " -n 1 -r 158 | echo 159 | if [[ $REPLY =~ ^[Yy]$ ]] || [ -z $REPLY ]; then 160 | 161 | # Place our FSD dependencies in the rosdep sources folder 162 | printf "\n\nCopying FSD's ROS dependency list to rosdep sources folder\n" 163 | printf "Target folder: ${ROSDEP_SOURCES_TARGET}\n" 164 | printf "Sudo might ask for your password now!\n" 165 | COPY_FAILED_MESSAGE="[${red}COPY FAILED: The dependency update might fail now!${end}]\n" 166 | sudo cp "${FSD_ROSDEP_DIR}/fsd-dependencies.yaml" "${ROSDEP_SOURCES_TARGET}/" || { printf "$COPY_FAILED_MESSAGE" ; } 167 | sudo cp ${FSD_ROSDEP_DIR}/*.rdmanifest "${ROSDEP_SOURCES_TARGET}/" || { printf "$COPY_FAILED_MESSAGE" ; } 168 | sudo cp "${FSD_ROSDEP_DIR}/10-fsd-dependencies.list" "${ROSDEP_SOURCES_TARGET}/" || { printf "$COPY_FAILED_MESSAGE" ; } 169 | 170 | sudo apt-get update 171 | # Update rosdep 172 | rosdep update 173 | 174 | # Install the dependencies 175 | rosdep install --from-paths "${ABSOLUTE_PATH}/" -r -i -y || { printf "[${red}ROSDEP FAILED${end}]\n" ;} 176 | 177 | fi 178 | 179 | ################################################# 180 | # Configure catkin workpace # 181 | ################################################# 182 | echo "Blacklisting packages: " ${BLACKLIST_PACKAGES} 183 | catkin init 184 | if [ -n "${BLACKLIST_PACKAGES}" ]; then 185 | catkin config --blacklist ${BLACKLIST_PACKAGES} --cmake-args -DCMAKE_BUILD_TYPE=Release 186 | else 187 | catkin config --no-blacklist --cmake-args -DCMAKE_BUILD_TYPE=Release 188 | fi 189 | 190 | ################################################# 191 | # If fssim enabled update deps # 192 | ################################################# 193 | if [ ! -z $FSSIM ]; then 194 | printf "Updating FSSIM dependencies..." 195 | cd src/fssim/ 196 | git pull 197 | ./update_dependencies.sh 198 | git lfs pull 199 | cd ../../ 200 | fi 201 | -------------------------------------------------------------------------------- /src/3_control/control_pure_pursuit/src/pure_pursuit_handle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Formula Student Driverless Project (FSD-Project). 3 | Copyright (c) 2018: 4 | - Sonja Brits 5 | - Juraj Kabzan 6 | 7 | FSD-Project is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | FSD-Project is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with FSD-Project. If not, see . 19 | */ 20 | 21 | #include 22 | #include "pure_pursuit_handle.hpp" 23 | 24 | // ROS Msgs 25 | #include "geometry_msgs/PolygonStamped.h" 26 | 27 | namespace ns_pure_pursuit { 28 | 29 | // Constructor 30 | PurePursuitHandle::PurePursuitHandle(ros::NodeHandle &nodeHandle) : 31 | nodeHandle_(nodeHandle), 32 | pure_pursuit_(nodeHandle) { 33 | ROS_INFO("Constructing Handle"); 34 | loadParameters(); 35 | subscribeToTopics(); 36 | publishToTopics(); 37 | pure_pursuit_.setMaxSpeed(max_speed_); 38 | } 39 | 40 | // Getters 41 | int PurePursuitHandle::getNodeRate() const { return node_rate_; } 42 | 43 | // Methods 44 | void PurePursuitHandle::loadParameters() { 45 | ROS_INFO("loading handle parameters"); 46 | if (!nodeHandle_.param("max_speed", 47 | max_speed_, 48 | 3.0)) { 49 | ROS_WARN_STREAM("Did not load max_speed. Standard value is: " << max_speed_); 50 | } 51 | if (!nodeHandle_.param("slam_map_topic_name", 52 | slam_map_topic_name_, 53 | "/estimation/slam/map")) { 54 | ROS_WARN_STREAM("Did not load slam_map_topic_name. Standard value is: " << slam_map_topic_name_); 55 | } 56 | if (!nodeHandle_.param("slam_state_topic_name", 57 | slam_state_topic_name_, 58 | "/estimation/slam/state")) { 59 | ROS_WARN_STREAM("Did not load slam_state_topic_name. Standard value is: " << slam_state_topic_name_); 60 | } 61 | if (!nodeHandle_.param("velocity_estimate_topic_name", 62 | velocity_estimate_topic_name_, 63 | "/estimation/velocity_estimation/velocity_estimate")) { 64 | ROS_WARN_STREAM( 65 | "Did not load velocity_estimate_topic_name. Standard value is: " << velocity_estimate_topic_name_); 66 | } 67 | if (!nodeHandle_.param("control_command_topic_name", 68 | control_command_topic_name_, 69 | "/control/pure_pursuit/car_command")) { 70 | ROS_WARN_STREAM("Did not load control_command_topic_name. Standard value is: " << control_command_topic_name_); 71 | } 72 | if (!nodeHandle_.param("center_line_topic_name", 73 | center_line_topic_name_, 74 | "/control/pure_pursuit/center_line")) { 75 | ROS_WARN_STREAM("Did not load center_line_topic_name. Standard value is: " << center_line_topic_name_); 76 | } 77 | if (!nodeHandle_.param("node_rate", node_rate_, 1)) { 78 | ROS_WARN_STREAM("Did not load node_rate. Standard value is: " << node_rate_); 79 | } 80 | } 81 | 82 | void PurePursuitHandle::subscribeToTopics() { 83 | ROS_INFO("subscribe to topics"); 84 | slamMapSubscriber_ = 85 | nodeHandle_.subscribe(slam_map_topic_name_, 1, &PurePursuitHandle::slamMapCallback, this); 86 | slamStateSubscriber_ = 87 | nodeHandle_.subscribe(slam_state_topic_name_, 1, &PurePursuitHandle::slamStateCallback, this); 88 | velocityEstimateSubscriber_ = 89 | nodeHandle_.subscribe(velocity_estimate_topic_name_, 1, &PurePursuitHandle::velocityEstimateCallback, this); 90 | } 91 | 92 | void PurePursuitHandle::publishToTopics() { 93 | ROS_INFO("publish to topics"); 94 | controlCommandPublisher_ = nodeHandle_.advertise(control_command_topic_name_, 1); 95 | centerLinePublisher_ = nodeHandle_.advertise(center_line_topic_name_, 1, true); 96 | } 97 | 98 | void PurePursuitHandle::run() { 99 | pure_pursuit_.runAlgorithm(); 100 | sendControlCommand(); 101 | } 102 | 103 | void PurePursuitHandle::sendControlCommand() { 104 | control_command_.throttle = pure_pursuit_.getControlCommand().throttle; 105 | control_command_.steering_angle = pure_pursuit_.getControlCommand().steering_angle; 106 | control_command_.header.stamp = ros::Time::now(); 107 | controlCommandPublisher_.publish(control_command_); 108 | } 109 | 110 | void PurePursuitHandle::slamMapCallback(const fsd_common_msgs::Map &map) { 111 | geometry_msgs::PolygonStamped center_line; 112 | { // Find Center Line 113 | center_line.polygon.points.clear(); 114 | for (const auto &yellow: map.cone_yellow) { 115 | 116 | const auto it_blue = std::min_element(map.cone_blue.begin(), map.cone_blue.end(), 117 | [&](const fsd_common_msgs::Cone &a, 118 | const fsd_common_msgs::Cone &b) { 119 | const double da = std::hypot(yellow.position.x - a.position.x, 120 | yellow.position.y - a.position.y); 121 | const double db = std::hypot(yellow.position.x - b.position.x, 122 | yellow.position.y - b.position.y); 123 | 124 | return da < db; 125 | }); 126 | 127 | geometry_msgs::Point32 p; 128 | p.x = static_cast((yellow.position.x + it_blue->position.x) / 2.0); 129 | p.y = static_cast((yellow.position.y + it_blue->position.y) / 2.0); 130 | p.z = 0.0; 131 | center_line.polygon.points.push_back(p); 132 | } 133 | } 134 | 135 | geometry_msgs::Polygon dense_center_line; 136 | { // Densify the center line 137 | const double precision = 0.2; 138 | for (unsigned int i = 1; i < center_line.polygon.points.size(); i++) { 139 | const double dx = center_line.polygon.points[i].x - center_line.polygon.points[i - 1].x; 140 | const double dy = center_line.polygon.points[i].y - center_line.polygon.points[i - 1].y; 141 | const double d = std::hypot(dx, dy); 142 | 143 | const int nm_add_points = d / precision; 144 | for (unsigned int j = 0; j < nm_add_points; ++j) { 145 | geometry_msgs::Point32 new_p = center_line.polygon.points[i - 1]; 146 | new_p.x += precision * j * dx / d; 147 | new_p.y += precision * j * dy / d; 148 | dense_center_line.points.push_back(new_p); 149 | } 150 | } 151 | } 152 | 153 | center_line.polygon = dense_center_line; 154 | center_line.header.frame_id = "map"; 155 | center_line.header.stamp = ros::Time::now(); 156 | centerLinePublisher_.publish(center_line); 157 | 158 | pure_pursuit_.setCenterLine(dense_center_line); 159 | } 160 | 161 | void PurePursuitHandle::slamStateCallback(const fsd_common_msgs::CarState &state) { 162 | pure_pursuit_.setState(state); 163 | } 164 | 165 | void PurePursuitHandle::velocityEstimateCallback(const fsd_common_msgs::CarStateDt &velocity) { 166 | pure_pursuit_.setVelocity(velocity); 167 | } 168 | } 169 | -------------------------------------------------------------------------------- /src/fssim_interface/rviz/trackdrive.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 93 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.663888872 8 | Tree Height: 732 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679016 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.492592603 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: Lidar Observations 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 3 33 | Class: rviz/Grid 34 | Color: 177; 177; 177 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.0299999993 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 100 47 | Reference Frame: 48 | Value: true 49 | - Class: rviz/Axes 50 | Enabled: true 51 | Length: 2 52 | Name: Map 53 | Radius: 0.25 54 | Reference Frame: map 55 | Value: true 56 | - Class: rviz/Axes 57 | Enabled: true 58 | Length: 1 59 | Name: FSSIM Map 60 | Radius: 0.100000001 61 | Reference Frame: fssim_map 62 | Value: true 63 | - Alpha: 1 64 | Class: rviz/RobotModel 65 | Collision Enabled: false 66 | Enabled: true 67 | Links: 68 | All Links Enabled: true 69 | Expand Joint Details: false 70 | Expand Link Details: false 71 | Expand Tree: false 72 | Link Tree Style: Links in Alphabetic Order 73 | base_link: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | chassis: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | cog: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | front_center_lidar_link: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | left_front_wheel: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | left_rear_wheel: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | left_steering_hinge: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | right_front_wheel: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | right_rear_wheel: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | right_steering_hinge: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | Name: RobotModel 121 | Robot Description: robot_description 122 | TF Prefix: fssim/vehicle 123 | Update Interval: 0 124 | Value: true 125 | Visual Enabled: true 126 | - Class: rviz/TF 127 | Enabled: true 128 | Frame Timeout: 15 129 | Frames: 130 | All Enabled: true 131 | fssim/vehicle/base_link: 132 | Value: true 133 | fssim/vehicle/chassis: 134 | Value: true 135 | fssim/vehicle/cog: 136 | Value: true 137 | fssim/vehicle/front_center_lidar_link: 138 | Value: true 139 | fssim/vehicle/left_front_wheel: 140 | Value: true 141 | fssim/vehicle/left_rear_wheel: 142 | Value: true 143 | fssim/vehicle/left_steering_hinge: 144 | Value: true 145 | fssim/vehicle/right_front_wheel: 146 | Value: true 147 | fssim/vehicle/right_rear_wheel: 148 | Value: true 149 | fssim/vehicle/right_steering_hinge: 150 | Value: true 151 | fssim_map: 152 | Value: true 153 | gotthard/base_link: 154 | Value: true 155 | map: 156 | Value: true 157 | Marker Scale: 0.5 158 | Name: TF 159 | Show Arrows: true 160 | Show Axes: true 161 | Show Names: true 162 | Tree: 163 | fssim_map: 164 | fssim/vehicle/base_link: 165 | fssim/vehicle/cog: 166 | fssim/vehicle/chassis: 167 | fssim/vehicle/front_center_lidar_link: 168 | {} 169 | fssim/vehicle/left_rear_wheel: 170 | {} 171 | fssim/vehicle/left_steering_hinge: 172 | fssim/vehicle/left_front_wheel: 173 | {} 174 | fssim/vehicle/right_rear_wheel: 175 | {} 176 | fssim/vehicle/right_steering_hinge: 177 | fssim/vehicle/right_front_wheel: 178 | {} 179 | map: 180 | gotthard/base_link: 181 | {} 182 | Update Interval: 0 183 | Value: true 184 | - Class: rviz/MarkerArray 185 | Enabled: false 186 | Marker Topic: /fssim/track/markers 187 | Name: FSSIM Track 188 | Namespaces: 189 | {} 190 | Queue Size: 100 191 | Value: false 192 | - Alpha: 0.5 193 | Autocompute Intensity Bounds: false 194 | Autocompute Value Bounds: 195 | Max Value: 10 196 | Min Value: -10 197 | Value: true 198 | Axis: Z 199 | Channel Name: intensity 200 | Class: rviz/PointCloud2 201 | Color: 255; 255; 255 202 | Color Transformer: Intensity 203 | Decay Time: 0 204 | Enabled: true 205 | Invert Rainbow: false 206 | Max Color: 255; 255; 0 207 | Max Intensity: 1 208 | Min Color: 0; 0; 255 209 | Min Intensity: 0 210 | Name: Camera Observations 211 | Position Transformer: XYZ 212 | Queue Size: 1 213 | Selectable: true 214 | Size (Pixels): 3 215 | Size (m): 0.5 216 | Style: Spheres 217 | Topic: /camera/cones 218 | Unreliable: false 219 | Use Fixed Frame: true 220 | Use rainbow: false 221 | Value: true 222 | - Alpha: 0.5 223 | Autocompute Intensity Bounds: false 224 | Autocompute Value Bounds: 225 | Max Value: 10 226 | Min Value: -10 227 | Value: true 228 | Axis: Z 229 | Channel Name: intensity 230 | Class: rviz/PointCloud2 231 | Color: 11; 9; 9 232 | Color Transformer: Intensity 233 | Decay Time: 0 234 | Enabled: true 235 | Invert Rainbow: true 236 | Max Color: 255; 255; 0 237 | Max Intensity: 1 238 | Min Color: 0; 0; 255 239 | Min Intensity: 0 240 | Name: Lidar Observations 241 | Position Transformer: XYZ 242 | Queue Size: 1 243 | Selectable: true 244 | Size (Pixels): 3 245 | Size (m): 0.5 246 | Style: Flat Squares 247 | Topic: /lidar/cones 248 | Unreliable: false 249 | Use Fixed Frame: true 250 | Use rainbow: false 251 | Value: true 252 | - Class: rviz/MarkerArray 253 | Enabled: true 254 | Marker Topic: /boundary_estimation/visual 255 | Name: Boundary Estimation 256 | Namespaces: 257 | {} 258 | Queue Size: 100 259 | Value: true 260 | - Class: rviz/MarkerArray 261 | Enabled: true 262 | Marker Topic: /slam/marker_map 263 | Name: Map 264 | Namespaces: 265 | {} 266 | Queue Size: 100 267 | Value: true 268 | - Alpha: 1 269 | Class: rviz/Polygon 270 | Color: 25; 255; 0 271 | Enabled: true 272 | Name: Center Line 273 | Topic: /control/pure_pursuit/center_line 274 | Unreliable: false 275 | Value: true 276 | - Class: rviz/MarkerArray 277 | Enabled: false 278 | Marker Topic: /control/pure_pursuit/marker 279 | Name: Control Marker 280 | Namespaces: 281 | {} 282 | Queue Size: 100 283 | Value: false 284 | Enabled: true 285 | Global Options: 286 | Background Color: 255; 255; 255 287 | Default Light: true 288 | Fixed Frame: map 289 | Frame Rate: 30 290 | Name: root 291 | Tools: 292 | - Class: rviz/Interact 293 | Hide Inactive Objects: true 294 | - Class: rviz/MoveCamera 295 | - Class: rviz/Select 296 | - Class: rviz/FocusCamera 297 | - Class: rviz/Measure 298 | - Class: rviz/SetInitialPose 299 | Topic: /initialpose 300 | - Class: rviz/SetGoal 301 | Topic: /move_base_simple/goal 302 | - Class: rviz/PublishPoint 303 | Single click: true 304 | Topic: /clicked_point 305 | Value: true 306 | Views: 307 | Current: 308 | Class: rviz/ThirdPersonFollower 309 | Distance: 15.6628675 310 | Enable Stereo Rendering: 311 | Stereo Eye Separation: 0.0599999987 312 | Stereo Focal Distance: 1 313 | Swap Stereo Eyes: false 314 | Value: false 315 | Focal Point: 316 | X: 0.370205879 317 | Y: 0.905825377 318 | Z: -1.90734863e-06 319 | Focal Shape Fixed Size: true 320 | Focal Shape Size: 0.0500000007 321 | Invert Z Axis: false 322 | Name: Current View 323 | Near Clip Distance: 0.00999999978 324 | Pitch: 0.352739751 325 | Target Frame: fssim/vehicle/base_link 326 | Value: ThirdPersonFollower (rviz) 327 | Yaw: 3.20366788 328 | Saved: ~ 329 | Window Geometry: 330 | Displays: 331 | collapsed: true 332 | Height: 1028 333 | Hide Left Dock: true 334 | Hide Right Dock: true 335 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000037afc0200000008fb0000001200530065006c0065006300740069006f006e0000000028000001c50000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000037a000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001100000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c00000003efc0100000002fb0000000800540069006d00650100000000000003c00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003c00000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 336 | Selection: 337 | collapsed: false 338 | Time: 339 | collapsed: false 340 | Tool Properties: 341 | collapsed: false 342 | Views: 343 | collapsed: true 344 | Width: 960 345 | X: 960 346 | Y: 24 347 | -------------------------------------------------------------------------------- /src/fssim_interface/rviz/fssim.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | #ffffff 5 | #000000 6 | false 7 | false 8 | 9 | 10 | 11 | 12 | 13 | 14 | Untitled Axis 15 | 0 16 | true 17 | 18 | 19 | Untitled Axis 20 | 0 21 | true 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | header/stamp 30 | 0 31 | 32 | 1000 33 | 0 34 | 0 35 | -1000 36 | 0 37 | 38 | /control/pure_pursuit/control_command 39 | fsd_common_msgs/ControlCommand 40 | 41 | 42 | steering_angle/data 43 | 0 44 | 45 | 0.5 46 | -0.5 47 | 0 48 | -1000 49 | 1 50 | 51 | /control/pure_pursuit/control_command 52 | fsd_common_msgs/ControlCommand 53 | 54 | 55 | 56 | #000000 57 | 0 58 | 59 | 60 | 1000 61 | 20 62 | 3 63 | 64 | 74 | 100 75 | Steering command 76 | 77 | 78 | 79 | true 80 | 81 | 30 82 | Car command 83 | 84 | 85 | 86 | 87 | 88 | Untitled Axis 89 | 0 90 | true 91 | 92 | 93 | Untitled Axis 94 | 0 95 | true 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | header/stamp 104 | 0 105 | 106 | 1000 107 | 0 108 | 0 109 | -1000 110 | 0 111 | 112 | /estimation/slam/state 113 | fsd_common_msgs/CarState 114 | 115 | 116 | car_state_dt/car_state_dt/x 117 | 0 118 | 119 | 1000 120 | 0 121 | 0 122 | -1000 123 | 0 124 | 125 | /estimation/slam/state 126 | fsd_common_msgs/CarState 127 | 128 | 129 | 130 | #000000 131 | 0 132 | 133 | 134 | 1000 135 | 10 136 | 3 137 | 138 | 148 | 100 149 | vx 150 | 151 | 152 | 153 | 154 | header/stamp 155 | 0 156 | 157 | 1000 158 | 0 159 | 0 160 | -1000 161 | 0 162 | 163 | /estimation/slam/state 164 | fsd_common_msgs/CarState 165 | 166 | 167 | car_state_dt/car_state_dt/y 168 | 0 169 | 170 | 1000 171 | 0 172 | 0 173 | -1000 174 | 0 175 | 176 | /estimation/slam/state 177 | fsd_common_msgs/CarState 178 | 179 | 180 | 181 | #000000 182 | 0 183 | 184 | 185 | 1000 186 | 10 187 | 3 188 | 189 | 199 | 100 200 | vy 201 | 202 | 203 | 204 | 205 | header/stamp 206 | 0 207 | 208 | 1000 209 | 0 210 | 0 211 | -1000 212 | 0 213 | 214 | /estimation/slam/state 215 | fsd_common_msgs/CarState 216 | 217 | 218 | car_state_dt/car_state_dt/theta 219 | 0 220 | 221 | 1000 222 | 0 223 | 0 224 | -1000 225 | 0 226 | 227 | /estimation/slam/state 228 | fsd_common_msgs/CarState 229 | 230 | 231 | 232 | #000000 233 | 0 234 | 235 | 236 | 1000 237 | 10 238 | 3 239 | 240 | 250 | 100 251 | r 252 | 253 | 254 | 255 | true 256 | 257 | 30 258 | Velocities 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | Untitled Axis 267 | 0 268 | true 269 | 270 | 271 | Untitled Axis 272 | 0 273 | true 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | header/stamp 282 | 0 283 | 284 | 1000 285 | 0 286 | 0 287 | -1000 288 | 0 289 | 290 | /control/pure_pursuit/control_command 291 | fsd_common_msgs/ControlCommand 292 | 293 | 294 | throttle/data 295 | 0 296 | 297 | 1 298 | -1 299 | 0 300 | -1000 301 | 0 302 | 303 | /control/pure_pursuit/control_command 304 | fsd_common_msgs/ControlCommand 305 | 306 | 307 | 308 | #000000 309 | 0 310 | 311 | 312 | 1000 313 | 10 314 | 3 315 | 316 | 326 | 100 327 | Driver Command 328 | 329 | 330 | 331 | true 332 | 333 | 30 334 | Car Command 335 | 336 | 337 | 338 | 339 | 340 | Untitled Axis 341 | 0 342 | true 343 | 344 | 345 | Untitled Axis 346 | 0 347 | true 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | alpha_f_l 356 | 0 357 | 358 | 50 359 | -50 360 | 0 361 | -1000 362 | 0 363 | 364 | /fssim/car_info 365 | fssim_common/CarInfo 366 | 367 | 368 | Fy_f_l 369 | 0 370 | 371 | 50 372 | -50 373 | 0 374 | -1000 375 | 0 376 | 377 | /fssim/car_info 378 | fssim_common/CarInfo 379 | 380 | 381 | 382 | #000000 383 | 0 384 | 385 | 386 | 10 387 | 1 388 | 1 389 | 390 | 400 | 100 401 | FL 402 | 403 | 404 | 405 | 406 | alpha_f_r 407 | 0 408 | 409 | 50 410 | -50 411 | 0 412 | -1000 413 | 0 414 | 415 | /fssim/car_info 416 | fssim_common/CarInfo 417 | 418 | 419 | Fy_f_r 420 | 0 421 | 422 | 50 423 | -50 424 | 0 425 | -1000 426 | 0 427 | 428 | /fssim/car_info 429 | fssim_common/CarInfo 430 | 431 | 432 | 433 | #000000 434 | 0 435 | 436 | 437 | 10 438 | 1 439 | 1 440 | 441 | 451 | 100 452 | FR 453 | 454 | 455 | 456 | 457 | alpha_r_l 458 | 0 459 | 460 | 50 461 | -50 462 | 0 463 | -1000 464 | 0 465 | 466 | /fssim/car_info 467 | fssim_common/CarInfo 468 | 469 | 470 | Fy_r_l 471 | 0 472 | 473 | 50 474 | -50 475 | 0 476 | -1000 477 | 0 478 | 479 | /fssim/car_info 480 | fssim_common/CarInfo 481 | 482 | 483 | 484 | #000000 485 | 0 486 | 487 | 488 | 10 489 | 1 490 | 1 491 | 492 | 502 | 100 503 | RL 504 | 505 | 506 | 507 | 508 | alpha_r_r 509 | 0 510 | 511 | 50 512 | -50 513 | 0 514 | -1000 515 | 0 516 | 517 | /fssim/car_info 518 | fssim_common/CarInfo 519 | 520 | 521 | Fy_r_r 522 | 0 523 | 524 | 50 525 | -50 526 | 0 527 | -1000 528 | 0 529 | 530 | /fssim/car_info 531 | fssim_common/CarInfo 532 | 533 | 534 | 535 | #000000 536 | 0 537 | 538 | 539 | 10 540 | 1 541 | 1 542 | 543 | 553 | 100 554 | RR 555 | 556 | 557 | 558 | true 559 | 560 | 30 561 | Tires 562 | 563 | 564 | 565 | false 566 |
567 | 568 | --------------------------------------------------------------------------------