├── .gitignore
├── .gitlab-ci.yml
├── DoxygenMainpage.hpp
├── Doxygen_software_files.hpp
├── LICENSE
├── Readme.md
├── build_all.bash
├── cpm.png
├── cpm_lib
├── CMakeLists.txt
├── Doxygen_cpm_bash_descriptions.hpp
├── QOS_LOCAL_COMMUNICATION.xml.template
├── Toolchain.cmake
├── build.bash
├── build_arm.bash
├── dds_idl
│ ├── Color.idl
│ ├── CommonroadDDSGoalState.idl
│ ├── CommonroadDDSShape.idl
│ ├── CommonroadObstacle.idl
│ ├── CommonroadObstacleList.idl
│ ├── HLCHello.idl
│ ├── Header.idl
│ ├── HlcCommunication.idl
│ ├── LedPoints.idl
│ ├── Log.idl
│ ├── LogLevel.idl
│ ├── Parameter.idl
│ ├── ParameterRequest.idl
│ ├── Point2D.idl
│ ├── Pose2D.idl
│ ├── ReadyStatus.idl
│ ├── RoundTripTime.idl
│ ├── StopRequest.idl
│ ├── SystemTrigger.idl
│ ├── TimeStamp.idl
│ ├── VehicleCommandDirect.idl
│ ├── VehicleCommandPathTracking.idl
│ ├── VehicleCommandSpeedCurvature.idl
│ ├── VehicleCommandTrajectory.idl
│ ├── VehicleObservation.idl
│ ├── VehicleState.idl
│ ├── VehicleStateList.idl
│ └── Visualization.idl
├── include
│ └── cpm
│ │ ├── AsyncReader.hpp
│ │ ├── CommandLineReader.hpp
│ │ ├── HLCCommunicator.hpp
│ │ ├── Logging.hpp
│ │ ├── MultiVehicleReader.hpp
│ │ ├── Parameter.hpp
│ │ ├── ParameterReceiver.hpp
│ │ ├── Participant.hpp
│ │ ├── ParticipantSingleton.hpp
│ │ ├── RTTTool.hpp
│ │ ├── Reader.hpp
│ │ ├── ReaderAbstract.hpp
│ │ ├── SimpleTimer.hpp
│ │ ├── TimeMeasurement.hpp
│ │ ├── Timer.hpp
│ │ ├── TimerFD.hpp
│ │ ├── VehicleIDFilteredTopic.hpp
│ │ ├── Writer.hpp
│ │ ├── exceptions.hpp
│ │ ├── get_time_ns.hpp
│ │ ├── get_topic.hpp
│ │ ├── init.hpp
│ │ └── stamp_message.hpp
├── rtigen.bash
├── rtigen_matlab.m
├── src
│ ├── CommandLineReader.cpp
│ ├── HLCCommunicator.cpp
│ ├── InternalConfiguration.cpp
│ ├── InternalConfiguration.hpp
│ ├── Logging.cpp
│ ├── ParameterReceiver.cpp
│ ├── ParticipantSingleton.cpp
│ ├── RTTTool.cpp
│ ├── SimpleTimer.cpp
│ ├── TimeMeasurement.cpp
│ ├── Timer.cpp
│ ├── TimerFD.cpp
│ ├── TimerSimulated.cpp
│ ├── TimerSimulated.hpp
│ ├── exceptions.cpp
│ └── get_time_ns.cpp
└── test
│ ├── QOS_TEST.xml
│ ├── catch.cpp
│ ├── catch.hpp
│ ├── test_AsyncReader.cpp
│ ├── test_CommandLineReader.cpp
│ ├── test_HLCCommunicator.cpp
│ ├── test_InternalConfiguration.cpp
│ ├── test_MultiVehicleReader.cpp
│ ├── test_Participant.cpp
│ ├── test_Reader.cpp
│ ├── test_ReaderAbstract.cpp
│ ├── test_VehicleIDFilteredTopic.cpp
│ ├── test_Writer.cpp
│ ├── test_logging.cpp
│ ├── test_parameter.cpp
│ ├── test_rtt.cpp
│ ├── test_simple_timer.cpp
│ ├── test_simple_timer_stop.cpp
│ ├── test_timer.cpp
│ ├── test_timer_simulated.cpp
│ ├── test_timer_start_again.cpp
│ ├── test_timer_stop.cpp
│ └── test_timer_stop_running.cpp
├── doxygen_config
├── doxygen_notes.md
├── high_level_controller
├── .gitignore
├── autostart
│ ├── CMakeLists.txt
│ ├── build.bash
│ ├── create_nuc_package.bash
│ ├── dummy_program_check.bash
│ ├── lab_autostart.bash
│ ├── local_test.bash
│ ├── simple_copy.bash
│ ├── src
│ │ ├── main.cpp
│ │ └── test_receiver.cpp
│ ├── src_error_logger
│ │ └── main.cpp
│ ├── test_receiver.bash
│ └── time_sync_test.bash
└── examples
│ ├── cpp
│ ├── basic_circle
│ │ ├── CMakeLists.txt
│ │ ├── QOS_LOCAL_COMMUNICATION.xml.template
│ │ ├── build.bash
│ │ ├── run.bash
│ │ └── src
│ │ │ └── main.cpp
│ ├── basic_line
│ │ ├── CMakeLists.txt
│ │ ├── build.bash
│ │ ├── run.bash
│ │ └── src
│ │ │ └── main.cpp
│ ├── central_routing
│ │ ├── CMakeLists.txt
│ │ ├── QOS_LOCAL_COMMUNICATION.xml.template
│ │ ├── build.bash
│ │ ├── include
│ │ │ ├── 150_jahre_RWTH_szenario
│ │ │ │ └── lane_graph.hpp
│ │ │ ├── lane_graph_full
│ │ │ │ └── lane_graph.hpp
│ │ │ ├── lane_graph_one_lane
│ │ │ │ └── lane_graph.hpp
│ │ │ └── lane_graph_outer_circle
│ │ │ │ └── lane_graph.hpp
│ │ ├── run.bash
│ │ └── src
│ │ │ ├── MultiVehicleTrajectoryPlanner.cpp
│ │ │ ├── MultiVehicleTrajectoryPlanner.hpp
│ │ │ ├── VehicleTrajectoryPlanningState.cpp
│ │ │ ├── VehicleTrajectoryPlanningState.hpp
│ │ │ ├── geometry.hpp
│ │ │ ├── lane_graph_tools.cpp
│ │ │ ├── lane_graph_tools.hpp
│ │ │ ├── main.cpp
│ │ │ └── tests.cpp
│ ├── controller_test_loop
│ │ ├── CMakeLists.txt
│ │ ├── build.bash
│ │ ├── run.bash
│ │ ├── run_platoon.bash
│ │ └── src
│ │ │ ├── main.cpp
│ │ │ ├── main_platoon.cpp
│ │ │ └── test_loop_trajectory.hpp
│ ├── diagonal_figure_eight
│ │ ├── CMakeLists.txt
│ │ ├── build.bash
│ │ ├── run.bash
│ │ └── src
│ │ │ └── main.cpp
│ ├── distributed_routing
│ │ ├── CMakeLists.txt
│ │ ├── QOS_LOCAL_COMMUNICATION.xml.template
│ │ ├── build.bash
│ │ ├── include
│ │ │ ├── 150_jahre_RWTH_szenario
│ │ │ │ └── lane_graph.hpp
│ │ │ ├── lane_graph_full
│ │ │ │ └── lane_graph.hpp
│ │ │ ├── lane_graph_one_lane
│ │ │ │ └── lane_graph.hpp
│ │ │ └── lane_graph_outer_circle
│ │ │ │ └── lane_graph.hpp
│ │ ├── run.bash
│ │ ├── run_distributed.bash
│ │ └── src
│ │ │ ├── CouplingGraph.cpp
│ │ │ ├── CouplingGraph.hpp
│ │ │ ├── VehicleTrajectoryPlanner.cpp
│ │ │ ├── VehicleTrajectoryPlanner.hpp
│ │ │ ├── VehicleTrajectoryPlanningState.cpp
│ │ │ ├── VehicleTrajectoryPlanningState.hpp
│ │ │ ├── geometry.hpp
│ │ │ ├── lane_graph_tools.cpp
│ │ │ ├── lane_graph_tools.hpp
│ │ │ ├── main.cpp
│ │ │ └── tests.cpp
│ ├── dynamic_priorities
│ │ ├── .gitignore
│ │ ├── CMakeLists.txt
│ │ ├── QOS_LOCAL_COMMUNICATION.xml.template
│ │ ├── Readme.md
│ │ ├── build.bash
│ │ ├── eval.sh
│ │ ├── include
│ │ │ ├── lane_graph_full
│ │ │ │ └── lane_graph.hpp
│ │ │ ├── lane_graph_one_lane
│ │ │ │ └── lane_graph.hpp
│ │ │ ├── lane_graph_outer_circle
│ │ │ │ └── lane_graph.hpp
│ │ │ └── lane_graph_two_ovals
│ │ │ │ └── lane_graph.hpp
│ │ ├── rtigen.bash
│ │ ├── run.bash
│ │ ├── run_simulation.bash
│ │ ├── src
│ │ │ ├── CouplingGraph.cpp
│ │ │ ├── CouplingGraph.hpp
│ │ │ ├── QOS_LOCAL_COMMUNICATION.xml
│ │ │ ├── VehicleTrajectoryPlanner.cpp
│ │ │ ├── VehicleTrajectoryPlanner.hpp
│ │ │ ├── VehicleTrajectoryPlanningState.cpp
│ │ │ ├── VehicleTrajectoryPlanningState.hpp
│ │ │ ├── dds_idl
│ │ │ │ ├── FallbackSync.idl
│ │ │ │ ├── FutureCollisionAssessment.idl
│ │ │ │ ├── Trajectory.idl
│ │ │ │ └── VehiclePath.idl
│ │ │ ├── geometry.hpp
│ │ │ ├── get_edge_indices.py
│ │ │ ├── lane_graph_tools.cpp
│ │ │ ├── lane_graph_tools.hpp
│ │ │ ├── main.cpp
│ │ │ ├── ovals_example.hpp
│ │ │ ├── simulation.cpp
│ │ │ ├── simulation.hpp
│ │ │ └── tests.cpp
│ │ └── visualization
│ │ │ ├── .gitignore
│ │ │ ├── car_position.m
│ │ │ ├── pgf_to_pdf.tex
│ │ │ ├── search_fca_index.m
│ │ │ ├── search_trajectory_message_index.m
│ │ │ ├── set_figure_properties.m
│ │ │ ├── transformedRectangle.m
│ │ │ ├── vehColor.m
│ │ │ ├── visualisation.py
│ │ │ ├── visualise_graph.py
│ │ │ └── visualize.m
│ ├── eight_zero
│ │ ├── CMakeLists.txt
│ │ ├── build.bash
│ │ ├── run.bash
│ │ └── src
│ │ │ ├── Eight.cpp
│ │ │ ├── Eight.hpp
│ │ │ └── main.cpp
│ └── two_vehicles_drive
│ │ ├── CMakeLists.txt
│ │ ├── build.bash
│ │ ├── run.bash
│ │ └── src
│ │ └── main.cpp
│ └── matlab
│ ├── 01_direct_control
│ └── main.m
│ ├── 02_path_tracking
│ └── main.m
│ ├── 03_trajectory
│ └── main.m
│ ├── Doxygen_matlab_description.hpp
│ ├── QOS_READY_TRIGGER.xml
│ ├── QP_central_routing
│ ├── ControllerFiles
│ │ ├── IntegratorModel.m
│ │ ├── MPC_init.m
│ │ ├── Priority_QP.m
│ │ ├── QP_controller.m
│ │ ├── QP_evaluate.m
│ │ ├── QP_optimizer.m
│ │ ├── blk.m
│ │ ├── config.m
│ │ ├── convert_to_QP.m
│ │ ├── decode_deltaU.m
│ │ ├── discretize.m
│ │ ├── generate_mpc_matrices.m
│ │ ├── local2global.m
│ │ ├── mpc_cost_function_matrices.m
│ │ ├── prediction_matrices.m
│ │ └── sampleReferenceTrajectory.m
│ ├── Maps
│ │ └── LabMapCommonRoad.xml
│ ├── PathPlanner.m
│ ├── QOS_READY_TRIGGER.xml
│ ├── README.txt
│ ├── RTree
│ │ ├── Lanelet.m
│ │ ├── LoadXML.m
│ │ ├── MBRforLanelet.m
│ │ ├── MyList.m
│ │ ├── Point2D_RTree.m
│ │ ├── RBTElement.m
│ │ ├── RNode.m
│ │ ├── RTree.m
│ │ ├── Rectangle.m
│ │ ├── RedBlackTree.m
│ │ ├── drawCenterline.m
│ │ ├── drawLaneletMap.m
│ │ └── parseXML.m
│ ├── configVehicle.m
│ ├── main_vehicle_ids.m
│ └── vehicle.m
│ ├── QP_commonroad_target
│ ├── CommonRoadImport
│ │ ├── Obstacle.m
│ │ ├── PlanningProblem.m
│ │ ├── goalstateTostruct.m
│ │ ├── obstacle2trajectory.m
│ │ ├── planning_extraction.m
│ │ └── stateTostruct.m
│ ├── ControllerFiles
│ │ ├── IntegratorModel.m
│ │ ├── MPC_init.m
│ │ ├── Priority_QP.m
│ │ ├── QP_controller.m
│ │ ├── QP_evaluate.m
│ │ ├── QP_optimizer.m
│ │ ├── blk.m
│ │ ├── config.m
│ │ ├── convert_to_QP.m
│ │ ├── decode_deltaU.m
│ │ ├── discretize.m
│ │ ├── generate_mpc_matrices.m
│ │ ├── local2global.m
│ │ ├── mpc_cost_function_matrices.m
│ │ ├── prediction_matrices.m
│ │ └── sampleReferenceTrajectory.m
│ ├── Maps
│ │ ├── LabMapCommonRoad.xml
│ │ ├── LabMapCommonRoadPlanning.xml
│ │ ├── LabMapCommonRoadPlanning2Vehicles.xml
│ │ ├── LabMapCommonRoadPlanningObstacle.xml
│ │ ├── RUS_Bicycle-4_1_T-1.xml
│ │ ├── ZAM_Tjunction-1_144_T-1.xml
│ │ ├── scenarios_cooperative_C-DEU_B471-1_1_T-1.xml
│ │ └── scenarios_cooperative_C-USA_Lanker-2_2_T-1.xml
│ ├── PathPlanner.m
│ ├── QOS_READY_TRIGGER.xml
│ ├── README.txt
│ ├── RTree
│ │ ├── Lanelet.m
│ │ ├── LoadXML.m
│ │ ├── MBRforLanelet.m
│ │ ├── MyList.m
│ │ ├── Point2D_RTree.m
│ │ ├── RBTElement.m
│ │ ├── RNode.m
│ │ ├── RTree.m
│ │ ├── Rectangle.m
│ │ ├── RedBlackTree.m
│ │ ├── drawCenterline.m
│ │ ├── drawLaneletMap.m
│ │ └── parseXML.m
│ ├── configVehicle.m
│ ├── debug_in_matlab.m
│ ├── main_vehicle_ids.m
│ └── vehicle.m
│ ├── init_script.m
│ ├── leader_follower
│ ├── followers.m
│ ├── leader.m
│ ├── leader_trajectory.m
│ └── main.m
│ ├── platoon
│ ├── leader.m
│ ├── leader_trajectory.m
│ ├── main_vehicle_amount.m
│ ├── main_vehicle_ids.m
│ └── test_received_ids.m
│ └── read_system_trigger.m
├── indoor_positioning_system
├── CMakeLists.txt
├── build.bash
├── cmake
│ └── FindPylon.cmake
├── recordings
│ ├── convert_to_csv.bash
│ ├── record.bash
│ ├── recording_config.xml
│ ├── recording_multiple_vehicles_manual_driving.dat_0_0
│ ├── recording_single_vehicle_manual_driving.dat_0_0
│ ├── recording_unreliable_ID.dat_0_0
│ └── replay_config.xml
├── src
│ ├── DetectVehicleID.cpp
│ ├── DetectVehicleID.hpp
│ ├── DetectVehicles.cpp
│ ├── DetectVehicles.hpp
│ ├── IpsPipeline.cpp
│ ├── IpsPipeline.hpp
│ ├── PoseCalculation.cpp
│ ├── PoseCalculation.hpp
│ ├── ThreadSafeQueue.hpp
│ ├── UndistortPoints.cpp
│ ├── UndistortPoints.hpp
│ ├── main_ips_pipeline.cpp
│ ├── main_led_detection.cpp
│ ├── pseudocode.txt
│ └── types.hpp
└── test
│ ├── catch.cpp
│ ├── catch.hpp
│ ├── test_DetectVehicleID.cpp
│ └── test_DetectVehicles.cpp
├── install.sh
├── lab_control_center
├── CMakeLists.txt
├── Doxygen_lcc_file_descriptions.hpp
├── bash
│ ├── copy_to_remote.bash
│ ├── environment_variables.bash
│ ├── environment_variables_local.bash
│ ├── reboot_raspberry.bash
│ ├── remote_kill.bash
│ ├── remote_start.bash
│ ├── tmux_middleware.bash
│ └── tmux_script.bash
├── build.bash
├── cmake
│ └── FindLibXML++.cmake
├── commonroad_HomePoses.xml
├── gdb_run.bash
├── icon.png
├── lab-control-center.desktop
├── labcam
│ ├── .gitignore
│ ├── CMakeLists.txt
│ ├── Readme.md
│ ├── camera_config
│ │ └── acA1920-50gc.pfs
│ ├── cmake
│ │ ├── FindGLIB.cmake
│ │ ├── FindGStreamer.cmake
│ │ └── FindPylon.cmake
│ ├── include
│ │ └── labcam
│ │ │ ├── CInstantCameraAppSrc.h
│ │ │ ├── LabCam.hpp
│ │ │ └── LabCamIface.hpp
│ └── src
│ │ ├── CInstantCameraAppSrc.cpp
│ │ ├── LabCam.cpp
│ │ ├── LabCamIface.cpp
│ │ └── main.cpp
├── parameters.yaml
├── recording
│ ├── record.bash
│ ├── rti_recording_config_template.xml
│ └── visualization
│ │ ├── dbConnection.m
│ │ ├── main.m
│ │ └── preprocessing.m
├── run.bash
├── src
│ ├── GoToPlanner.cpp
│ ├── GoToPlanner.hpp
│ ├── HLCReadyAggregator.cpp
│ ├── HLCReadyAggregator.hpp
│ ├── Joystick.cpp
│ ├── Joystick.hpp
│ ├── LCCErrorLogger.cpp
│ ├── LCCErrorLogger.hpp
│ ├── LogLevelSetter.cpp
│ ├── LogLevelSetter.hpp
│ ├── LogStorage.cpp
│ ├── LogStorage.hpp
│ ├── ObstacleAggregator.cpp
│ ├── ObstacleAggregator.hpp
│ ├── ObstacleSimulation.cpp
│ ├── ObstacleSimulation.hpp
│ ├── ObstacleSimulationManager.cpp
│ ├── ObstacleSimulationManager.hpp
│ ├── ParameterServer.cpp
│ ├── ParameterServer.hpp
│ ├── ParameterStorage.cpp
│ ├── ParameterStorage.hpp
│ ├── ParameterWithDescription.hpp
│ ├── ProgramExecutor.cpp
│ ├── ProgramExecutor.hpp
│ ├── RTTAggregator.cpp
│ ├── RTTAggregator.hpp
│ ├── TimeSeries.cpp
│ ├── TimeSeries.hpp
│ ├── TimeSeriesAggregator.cpp
│ ├── TimeSeriesAggregator.hpp
│ ├── TimerTrigger.cpp
│ ├── TimerTrigger.hpp
│ ├── TrajectoryCommand.cpp
│ ├── TrajectoryCommand.hpp
│ ├── VehicleAutomatedControl.cpp
│ ├── VehicleAutomatedControl.hpp
│ ├── VehicleManualControl.cpp
│ ├── VehicleManualControl.hpp
│ ├── VisualizationCommandsAggregator.cpp
│ ├── VisualizationCommandsAggregator.hpp
│ ├── commonroad_classes
│ │ ├── CommonRoadScenario.cpp
│ │ ├── CommonRoadScenario.hpp
│ │ ├── CommonRoadTransformation.cpp
│ │ ├── CommonRoadTransformation.hpp
│ │ ├── CommonroadDrawConfiguration.hpp
│ │ ├── DynamicObstacle.cpp
│ │ ├── DynamicObstacle.hpp
│ │ ├── EnvironmentObstacle.cpp
│ │ ├── EnvironmentObstacle.hpp
│ │ ├── InterfaceDraw.hpp
│ │ ├── InterfaceGeometry.hpp
│ │ ├── InterfaceTransform.hpp
│ │ ├── InterfaceTransformTime.hpp
│ │ ├── Intersection.cpp
│ │ ├── Intersection.hpp
│ │ ├── Lanelet.cpp
│ │ ├── Lanelet.hpp
│ │ ├── ObstacleSimulationData.hpp
│ │ ├── PlanningProblem.cpp
│ │ ├── PlanningProblem.hpp
│ │ ├── SpecificationError.cpp
│ │ ├── SpecificationError.hpp
│ │ ├── StaticObstacle.cpp
│ │ ├── StaticObstacle.hpp
│ │ ├── TrafficLight.cpp
│ │ ├── TrafficLight.hpp
│ │ ├── TrafficSign.cpp
│ │ ├── TrafficSign.hpp
│ │ ├── XMLTranslation.cpp
│ │ ├── XMLTranslation.hpp
│ │ ├── datatypes
│ │ │ ├── Interval.hpp
│ │ │ └── IntervalOrExact.hpp
│ │ ├── geometry
│ │ │ ├── Circle.cpp
│ │ │ ├── Circle.hpp
│ │ │ ├── Point.cpp
│ │ │ ├── Point.hpp
│ │ │ ├── Polygon.cpp
│ │ │ ├── Polygon.hpp
│ │ │ ├── Position.cpp
│ │ │ ├── Position.hpp
│ │ │ ├── Rectangle.cpp
│ │ │ ├── Rectangle.hpp
│ │ │ ├── Shape.cpp
│ │ │ └── Shape.hpp
│ │ └── states
│ │ │ ├── GoalState.cpp
│ │ │ ├── GoalState.hpp
│ │ │ ├── Occupancy.cpp
│ │ │ ├── Occupancy.hpp
│ │ │ ├── SignalState.cpp
│ │ │ ├── SignalState.hpp
│ │ │ ├── State.cpp
│ │ │ ├── State.hpp
│ │ │ ├── StateExact.cpp
│ │ │ └── StateExact.hpp
│ ├── defaults.cpp
│ ├── defaults.hpp
│ └── main.cpp
├── test
│ ├── ForkCommunicationTest.cpp
│ ├── LICENSE.txt
│ ├── TimerTestRealtime.cpp
│ ├── TimerTestSimulated.cpp
│ └── VisualizationTest.cpp
├── ui
│ ├── MainWindow.cpp
│ ├── MainWindow.hpp
│ ├── commonroad
│ │ ├── CommonroadViewUI.cpp
│ │ ├── CommonroadViewUI.hpp
│ │ ├── LaneletModelRecord.hpp
│ │ ├── ObstacleToggle.cpp
│ │ ├── ObstacleToggle.hpp
│ │ ├── ProblemModelRecord.hpp
│ │ ├── commonroad.glade
│ │ └── obstacle_toggle.glade
│ ├── file_chooser
│ │ ├── FileChooserDialog.glade
│ │ ├── FileChooserUI.cpp
│ │ ├── FileChooserUI.hpp
│ │ ├── FileDialogPaths.cpp
│ │ ├── FileDialogPaths.hpp
│ │ ├── FileSaverDialog.glade
│ │ ├── FileSaverUI.cpp
│ │ └── FileSaverUI.hpp
│ ├── lcc_errors
│ │ ├── LCCErrorModelRecord.hpp
│ │ ├── LCCErrorViewUI.cpp
│ │ ├── LCCErrorViewUI.hpp
│ │ └── lcc_errors.glade
│ ├── logger
│ │ ├── LoggerModelRecord.hpp
│ │ ├── LoggerViewUI.cpp
│ │ ├── LoggerViewUI.hpp
│ │ └── logger.glade
│ ├── manual_control
│ │ ├── VehicleManualControlUi.cpp
│ │ ├── VehicleManualControlUi.hpp
│ │ └── manual_control_ui2.glade
│ ├── map_view
│ │ ├── LabMapCommonRoad.xml
│ │ ├── MapViewUi.cpp
│ │ ├── MapViewUi.hpp
│ │ ├── car.png
│ │ ├── car_small.png
│ │ ├── labcam.png
│ │ ├── map.png
│ │ └── object_small.png
│ ├── master_layout.glade
│ ├── monitoring
│ │ ├── MonitoringUi.cpp
│ │ ├── MonitoringUi.hpp
│ │ └── monitoring_ui.glade
│ ├── params
│ │ ├── ParamModelRecord.hpp
│ │ ├── ParamViewUI.cpp
│ │ ├── ParamViewUI.hpp
│ │ ├── ParamsCreateView.cpp
│ │ ├── ParamsCreateView.hpp
│ │ ├── params.glade
│ │ ├── params_create.glade
│ │ └── params_show.glade
│ ├── right_tabs
│ │ ├── TabsViewUI.cpp
│ │ ├── TabsViewUI.hpp
│ │ └── right_tabs.glade
│ ├── setup
│ │ ├── CrashChecker.cpp
│ │ ├── CrashChecker.hpp
│ │ ├── Deploy.cpp
│ │ ├── Deploy.hpp
│ │ ├── SetupViewUI.cpp
│ │ ├── SetupViewUI.hpp
│ │ ├── Upload.cpp
│ │ ├── Upload.hpp
│ │ ├── UploadWindow.cpp
│ │ ├── UploadWindow.hpp
│ │ ├── VehicleToggle.cpp
│ │ ├── VehicleToggle.hpp
│ │ ├── setup.glade
│ │ ├── upload_window.glade
│ │ └── vehicle_toggle.glade
│ ├── style.css
│ └── timer
│ │ ├── TimerModelRecord.hpp
│ │ ├── TimerViewUI.cpp
│ │ ├── TimerViewUI.hpp
│ │ └── timer.glade
└── valgrind_run.bash
├── low_level_controller
├── vehicle_atmega2560_firmware.atsln
└── vehicle_atmega2560_firmware
│ ├── adc.c
│ ├── adc.h
│ ├── crc.c
│ ├── crc.h
│ ├── imu.c
│ ├── imu.h
│ ├── led.c
│ ├── led.h
│ ├── main.c
│ ├── motor.c
│ ├── motor.h
│ ├── odometer.c
│ ├── odometer.h
│ ├── servo_timer.c
│ ├── servo_timer.h
│ ├── spi.c
│ ├── spi.h
│ ├── spi_packets.h
│ ├── tests.c
│ ├── tests.h
│ ├── twi.c
│ ├── twi.h
│ ├── util.h
│ ├── vehicle_atmega2560_firmware.componentinfo.xml
│ ├── vehicle_atmega2560_firmware.cproj
│ ├── watchdog.c
│ └── watchdog.h
├── mid_level_controller
├── CMakeLists.txt
├── Doxygen_mlc_file_descriptions.hpp
├── Readme
│ ├── HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files.html
│ └── HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files
│ │ ├── analytics.js
│ │ ├── analytics_002.js
│ │ ├── community05_0_transparent.png
│ │ ├── css_2THG1eGiBIizsWFeexsNe1iDifJ00QRS9uSd03rY9co.css
│ │ ├── css_46mJMVJIWJRW0HgByOhet1ulNoT4vBO3KXsAcTnwFcI.css
│ │ ├── css_6U16mkQcGzYUlPVWT5PXgIakg-VsMY-gAzU9NXE-waY.css
│ │ ├── css_TQMq8RYUebyLOf8AgSOB7w8Yt4dK_hLWVSbwFKVBmNE.css
│ │ ├── css_YObmECsqGVmJVCkAad2QAZM9u6qXTBB6W44K52Liwy4.css
│ │ ├── css_h901DvxtMS8t0rgMOKtzA3N9K5oZHAoz8r7Fb9r7Qws.css
│ │ ├── js_4MRGjqSerJEQxet46pdKTifSYprPwyhoaSNKNoRsFw4.js
│ │ ├── js_Oq6ruP-xSadx-QnSRF27cRdi_VPT5Np6lQFTEFQHHnY.js
│ │ ├── js_Rk5YIkK6m2gKTGH-GrMu0pM_PlMKXnI0ktQUgXw1XgA.js
│ │ ├── js_Utlukeo-CtfDmwMvFh2pQ2p3Wb_mMczTU4GrOmzMNUc.js
│ │ ├── js_aYfVMQPe8P2rCf6DJ1LhPCg7gBb0JOOlHqxPPNPgF94.js
│ │ ├── js_vDrW3Ry_4gtSYaLsh77lWhWjIC6ml2QNkcfvfP5CVFs.js
│ │ ├── raspbian-jessie-config_dialog.png
│ │ ├── raspbian_jessie_setup.png
│ │ └── raspi-config.jpg
├── Toolchain.cmake
├── bootloader_raspberry.bash
├── build.bash
├── get_all_logs.bash
├── package
│ └── start.bash
├── reboot_all_raspberry.bash
├── run.bash
└── src
│ ├── Controller.cxx
│ ├── Controller.hpp
│ ├── Localization.cxx
│ ├── Localization.hpp
│ ├── MpcController.cxx
│ ├── MpcController.hpp
│ ├── PathInterpolation.cxx
│ ├── PathInterpolation.hpp
│ ├── PathTrackingController.cxx
│ ├── PathTrackingController.hpp
│ ├── SensorCalibration.cxx
│ ├── SensorCalibration.hpp
│ ├── SimulationIPS.cxx
│ ├── SimulationIPS.hpp
│ ├── SimulationVehicle.cxx
│ ├── SimulationVehicle.hpp
│ ├── TrajectoryInterpolation.cxx
│ ├── TrajectoryInterpolation.hpp
│ ├── VehicleModel.cxx
│ ├── VehicleModel.hpp
│ ├── bcm2835.c
│ ├── bcm2835.h
│ ├── casadi_mpc_fn.c
│ ├── casadi_mpc_fn.h
│ ├── geometry.hpp
│ ├── main.cxx
│ ├── spi.c
│ └── spi.h
├── middleware
├── CMakeLists.txt
├── Doxygen_middleware_file_descriptions.hpp
├── QOS_LOCAL_COMMUNICATION.xml.template
├── build.bash
├── run.bash
├── src
│ ├── Communication.hpp
│ ├── TypedCommunication.cpp
│ ├── TypedCommunication.hpp
│ └── main.cpp
└── test
│ ├── catch.cpp
│ ├── catch.hpp
│ ├── test_goals_to_hlc.cpp
│ ├── test_hlc_to_vehicle.cpp
│ ├── test_middleware_to_hlc.cpp
│ ├── test_vehicle_read.cpp
│ └── test_vehicle_to_middleware.cpp
├── quick_install.sh
├── tools
├── battery_level
│ ├── Doxygen_battery_level.hpp
│ ├── main.m
│ └── u-level.png
├── camera_calibration
│ ├── Doxygen_camera_calibration.hpp
│ ├── calibrationCheckerboardPoints.m
│ ├── calibration_points.csv
│ └── end_to_end_calibration.m
├── go_to_formation
│ ├── plan_path.m
│ └── setCostmap.m
├── ips_pose_calibration
│ ├── Doxygen_ips.hpp
│ ├── pose_calibration.m
│ └── pose_calibration.txt
├── map_print
│ ├── Doxygen_map.hpp
│ ├── WorkflowPrint.txt
│ ├── figure_eight_map_print
│ │ ├── Klothoide.m
│ │ ├── format_path_as_text.m
│ │ ├── make_path.m
│ │ ├── make_test_trajectory.m
│ │ └── prepress.pdf
│ ├── map_calibration_squares
│ │ ├── main.m
│ │ └── prepress.pdf
│ ├── map_print2
│ │ ├── cache.mat
│ │ ├── collocation
│ │ │ ├── Lagrange_differentiation_matrix.m
│ │ │ ├── Lagrange_interpolation.m
│ │ │ ├── Lagrange_interpolation_matrix.m
│ │ │ ├── Lagrange_interpolation_weights.m
│ │ │ ├── collocation_constants.m
│ │ │ ├── quadrature_CGL.m
│ │ │ └── quadrature_LGL.m
│ │ ├── lane_graph.m
│ │ ├── lane_graph_to_cpp.m
│ │ ├── main.m
│ │ ├── map-sketch.jpg
│ │ ├── prepress.pdf
│ │ ├── solve_segment_path.m
│ │ └── startup.m
│ ├── maxxprint_druckdaten_anforderungen.pdf
│ └── sub_lane_graph
│ │ ├── graph_inversion.m
│ │ ├── lane_graph.cpp
│ │ ├── main.m
│ │ ├── sub_graph_deletion.m
│ │ ├── sub_graph_selection.m
│ │ └── visualize_lane_graph.m
├── reference_trajectory_map2_test_loop
│ ├── Doxygen_reference_trajectory.hpp
│ ├── animate_trajectory.m
│ ├── create_reference_path.m
│ ├── export_trajectory.m
│ ├── load_collision_map.m
│ ├── optimization_checkpoint.mat
│ ├── optimize_speed_profile.m
│ └── reference_path.csv
└── vehicle_dynamics_identification_and_mpc
│ ├── Doxygen_vehicle_dynamics.hpp
│ ├── LEGACY_CONVERTER.xml
│ ├── MpcController.m
│ ├── TrajectoryInterpolation.m
│ ├── documentation
│ ├── main.pdf
│ └── main.tex
│ ├── manual_parameter_fit.m
│ ├── mpc_test.m
│ ├── optimize_parameters.m
│ ├── optimize_parameters_delay_grid.m
│ ├── optimize_parameters_delay_grid_eval.m
│ ├── prepare_measurement_data.m
│ ├── read_rti_csv.m
│ ├── readme.md
│ ├── recording_to_mat.bash
│ ├── recording_vehicles_2_3_test_loop.dat_0_0
│ ├── recording_vehicles_2_3_test_loop_b.dat_0_0
│ ├── sql_to_mat.m
│ ├── test_trajectory.csv
│ ├── vehicle_dynamics.m
│ └── vehicle_dynamics_no_bat.m
└── update_nucs.bash
/.gitignore:
--------------------------------------------------------------------------------
1 | *.o
2 | *.a
3 | *.so
4 | *.csv
5 | *.gz
6 | *.log
7 | *.mat
8 | *.asv
9 | *.md.html
10 | build/
11 | build_arm/
12 | build_arm_sim/
13 | build_x64_sim/
14 | build_sim/
15 | autostart_package/
16 | matlab_package/
17 | install/
18 | __pycache__/
19 | devel/
20 | img/
21 | cmake-build-debug/
22 | .idea/
23 | html/
24 | latex/
25 | gmon.out
26 | firmware/
27 | Debug/
28 | .vs/
29 | vehicle_raspberry_pi_zero_w/cross_compile/tools/
30 | rtidds/
31 | *~
32 | tools/map_print2/*.mat
33 | .vscode
34 | ParamServer/yaml-cpp
35 | Log_*.csv
36 | core
37 | indoor_positioning_system/debug*.png
38 | output/
39 | package.tar.gz
40 | mid_level_controller/package/
41 | tools/vehicle_dynamics_identification_and_mpc/casadi_mpc_fn*
42 | core
43 | tools/vehicle_dynamics_identification_and_mpc/documentation
44 | tools/map_print/map_print2/lane_graph.cpp
45 | std*.txt
46 | nuc_apache_package
47 | middleware.txt
48 | high_level_controller/high_level_controller.txt
49 | high_level_controller/hlc_*.txt
50 | middleware/idl_compiled
51 | high_level_controller/matlab/IDL_gen/
52 | high_level_controller/matlab/dds_idl_matlab
53 | command_line_arguments_test.bash
54 | lcc_interface/
55 | cpm_node/
56 | cpm_lib/dds_idl_cpp
57 | cpm_lib/dds_idl_matlab
58 | cpm_lib/include/cpm/dds
59 | cpm_lib/cpm_library_package
60 | build_arm/
61 | lab_control_center/*.config
62 | lab_control_center/vgdump
63 | lab_control_center/commonroad_profiles.yaml
64 | lab_control_center/gtk.suppression
65 | lab_control_center/file_dialog_open_config.yaml
66 |
--------------------------------------------------------------------------------
/.gitlab-ci.yml:
--------------------------------------------------------------------------------
1 | image: ubuntu:18.04
2 |
3 | stages:
4 | - build_sim
5 | - build_nosim
6 |
7 | before_script:
8 | - 'which ssh-agent || ( apt update -y && apt install openssh-client -y )'
9 | - eval $(ssh-agent -s)
10 | - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add -
11 | - mkdir -p ~/.ssh
12 | - chmod 700 ~/.ssh
13 | - ssh-keyscan git.rwth-aachen.de >> ~/.ssh/known_hosts
14 | - chmod 644 ~/.ssh/known_hosts
15 | - pwd
16 | - export DEBIAN_FRONTEND=noninteractive
17 | - apt update
18 | - apt install -y tzdata sudo expect
19 | - adduser --disabled-password --gecos '' docker
20 | - adduser docker sudo
21 | - echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers
22 | - ln -fs /usr/share/zoneinfo/Europe/Berlin /etc/localtime
23 | - dpkg-reconfigure --frontend noninteractive tzdata
24 | - mv ${RTI_INSTALLER_AUTOMATION} ${RTI_INSTALLER_AUTOMATION}.sh
25 | - chmod 700 ${RTI_INSTALLER_AUTOMATION}.sh
26 |
27 | build_sim:
28 | stage: build_sim
29 | script:
30 | - sudo ./install.sh --license_path=$RTI_LICENSE_KEY --domain_id=100 --simulation --ci --rti_installer_automation_path=${RTI_INSTALLER_AUTOMATION}.sh
31 | - source /etc/profile.d/rti_connext_dds.sh && ./build_all.bash --simulation
32 |
33 | build_nosim:
34 | stage: build_nosim
35 | script:
36 | - sudo ./install.sh --license_path=$RTI_LICENSE_KEY --domain_id=100 --ci --rti_installer_automation_path=${RTI_INSTALLER_AUTOMATION}.sh
37 | - source /etc/profile.d/rti_connext_dds.sh && ./build_all.bash
38 |
--------------------------------------------------------------------------------
/Doxygen_software_files.hpp:
--------------------------------------------------------------------------------
1 | //File descriptions only for the top directory
2 |
3 | /**
4 | * \page sw_files Software Files
5 | * \subpage sw_build_all
6 | * \subpage sw_install
7 | * \subpage sw_update_nucs
8 | * \subpage sw_readme
9 | * \subpage sw_license
10 | * \ingroup software_files
11 | */
12 |
13 | /**
14 | * \page sw_build_all build_all.bash
15 | * \brief Build script for all relevant projects, can be run headless or in simulation mode as well
16 | */
17 |
18 | /**
19 | * \page sw_install install.sh
20 | * \brief Install script for additional projects etc. needed in the lab, automates the mandatory steps in order to compile the cpm
21 | */
22 |
23 | /**
24 | * \page sw_update_nucs update_nucs.bash
25 | * \brief Script to perform linux updates on the NUCs / HLCs
26 | */
27 |
28 | /**
29 | * \page sw_readme Readme.md
30 | * \brief Relevant file information about the LCC
31 | */
32 |
33 | /**
34 | * \page sw_license LICENSE
35 | * \brief The license file
36 | */
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2020 Lehrstuhl Informatik 11 - RWTH Aachen University
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
23 | This file is part of cpm_lab.
24 |
25 | Author: i11 - Embedded Software, RWTH Aachen University
--------------------------------------------------------------------------------
/cpm.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/cpm.png
--------------------------------------------------------------------------------
/cpm_lib/Doxygen_cpm_bash_descriptions.hpp:
--------------------------------------------------------------------------------
1 | //This file includes descriptions for the cpm bash scripts and for the test folder
2 |
3 | /**
4 | * \defgroup cpmlib_further_files Further CPM Lib Files
5 | * \brief Additional files for the CPM Lib, e.g. Bash scripts
6 | * \ingroup cpmlib
7 | */
8 |
9 | /**
10 | * \page cpm_files CPM Files
11 | * \subpage cpm_build
12 | * \subpage cpm_build_arm
13 | * \subpage cpm_rtigen
14 | * \subpage cpm_rtigen_matlab
15 | * \subpage cpm_test
16 | * \ingroup cpmlib_further_files
17 | */
18 |
19 | /**
20 | * \page cpm_build build.bash
21 | * \brief x86 build file for the cpm library. Also creates C++ and Matlab files from the .idl DDS type definitions, if the according
22 | * folders do not already exist. If they do, please remove the folders beforehand if the files should be re-generated.
23 | * Runs created tests.
24 | * Also creates one of the packages for the HLC/NUC s.t. it also has access to the library and generated files.
25 | */
26 |
27 | /**
28 | * \page cpm_build_arm build_arm.bash
29 | * \brief ARM build file for the cpm library
30 | */
31 |
32 | /**
33 | * \page cpm_rtigen rtigen.bash
34 | * \brief Creates C++ files from the .idl type definitions for DDS messages
35 | */
36 |
37 | /**
38 | * \page cpm_rtigen_matlab rtigen_matlab.m
39 | * \brief Creates Matlab files from the .idl type definitions for DDS messages
40 | */
41 |
42 | /**
43 | * \page cpm_test cpm_lib/test
44 | * \brief Includes test files
45 | */
--------------------------------------------------------------------------------
/cpm_lib/Toolchain.cmake:
--------------------------------------------------------------------------------
1 | # Define our host system
2 | SET(CMAKE_SYSTEM_NAME Linux)
3 | SET(CMAKE_SYSTEM_VERSION 1)
4 |
5 | # Define the cross compiler locations
6 | SET(CMAKE_C_COMPILER $ENV{RASPBIAN_TOOLCHAIN}/bin/arm-linux-gnueabihf-gcc)
7 | SET(CMAKE_CXX_COMPILER $ENV{RASPBIAN_TOOLCHAIN}/bin/arm-linux-gnueabihf-g++)
8 |
9 | # Define the sysroot path for the RaspberryPi distribution in our tools folder
10 | SET(CMAKE_FIND_ROOT_PATH $ENV{RASPBIAN_TOOLCHAIN}/arm-raspbian-linux-gnueabi/sysroot/)
11 |
12 | # Use our definitions for compiler tools
13 | SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
14 | # Search for libraries and headers in the target directories only
15 | SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
16 | SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
17 |
--------------------------------------------------------------------------------
/cpm_lib/build_arm.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # exit when any command fails
3 | set -e
4 |
5 | if [ ! -d "build_arm" ]; then
6 | mkdir -p build_arm
7 | fi
8 |
9 | if [ ! -d "dds_idl_cpp" ]; then
10 | ./rtigen.bash
11 | fi
12 |
13 |
14 | cd build_arm
15 | cmake .. -DCMAKE_TOOLCHAIN_FILE=../Toolchain.cmake -DBUILD_ARM=ON
16 | make -j$(nproc)
17 | cd ..
18 |
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/Color.idl:
--------------------------------------------------------------------------------
1 | #ifndef COLOR_IDL
2 | #define COLOR_IDL
3 |
4 | /**
5 | * \struct Color
6 | * \brief A color
7 | * \ingroup cpmlib_idl
8 | */
9 | struct Color
10 | {
11 | octet a; //!< Alpha, between 0 and 255
12 | octet r; //!< Red, between 0 and 255
13 | octet g; //!< Green, between 0 and 255
14 | octet b; //!< Blue, between 0 and 255
15 | };
16 | #endif
17 |
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/CommonroadObstacleList.idl:
--------------------------------------------------------------------------------
1 | #include "CommonroadObstacle.idl"
2 |
3 | #ifndef COMMONROAD_OBSTACLE_LIST_IDL
4 | #define COMMONROAD_OBSTACLE_LIST_IDL
5 |
6 | /**
7 | * \struct CommonroadObstacleList
8 | * \brief Provides commonroad obstacle state data for all simulated obstacles
9 | * \ingroup cpmlib_idl
10 | */
11 | struct CommonroadObstacleList {
12 | //! List of commonroad obstacle data
13 | sequence commonroad_obstacle_list;
14 | };
15 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/HLCHello.idl:
--------------------------------------------------------------------------------
1 | #ifndef HLC_HELLO_IDL
2 | #define HLC_HELLO_IDL
3 |
4 | /**
5 | * \struct HLCHello
6 | * \brief These messages are sent by the LCC to check if a program is still running on a remote machine
7 | * \ingroup cpmlib_idl
8 | */
9 | struct HLCHello {
10 | string source_id; //!< To find out where the msg came from and to not answer to own msg
11 |
12 | boolean script_running; //!< If true, a Matlab / C++ session is still running
13 | boolean middleware_running; //!< If true, a Middleware session is still running
14 | };
15 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/Header.idl:
--------------------------------------------------------------------------------
1 | #include "TimeStamp.idl"
2 |
3 | #ifndef HEADER_IDL
4 | #define HEADER_IDL
5 |
6 | /**
7 | * \struct Header
8 | * \brief Provides timing information: Creation and valid_after timestamp
9 | * \ingroup cpmlib_idl
10 | */
11 | struct Header {
12 | //! The time at which this sample was created.
13 | TimeStamp create_stamp;
14 |
15 | //! This sample may be used at or after the valid_after_stamp time, not before.
16 | TimeStamp valid_after_stamp;
17 | };
18 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/HlcCommunication.idl:
--------------------------------------------------------------------------------
1 | #include "Header.idl"
2 |
3 | #ifndef HLCCOMMUNICATION_IDL
4 | #define HLCCOMMUNICATION_IDL
5 |
6 | /**
7 | * \struct LaneGraphPosition
8 | * \brief TODO
9 | * \ingroup cpmlib_idl
10 | */
11 | struct LaneGraphPosition
12 | {
13 | //! TODO
14 | TimeStamp estimated_arrival_time;
15 | //! TODO
16 | unsigned short edge_index;
17 | //! TODO
18 | unsigned short edge_path_index;
19 | };
20 |
21 |
22 | /**
23 | * \enum MessageType
24 | * \brief TODO
25 | * \ingroup cpmlib_idl
26 | */
27 | enum MessageType
28 | {
29 | Iterative,
30 | Final
31 | };
32 |
33 | /**
34 | * \struct HlcCommunication
35 | * \brief TODO
36 | * \ingroup cpmlib_idl
37 | */
38 | struct HlcCommunication
39 | {
40 | //! TODO
41 | octet vehicle_id; //@key
42 | //! TODO
43 | Header header;
44 | //! TODO
45 | MessageType type;
46 | //! TODO
47 | boolean has_collisions;
48 | //! TODO
49 | sequence lane_graph_positions;
50 | };
51 | #endif
52 |
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/LedPoints.idl:
--------------------------------------------------------------------------------
1 | #include "TimeStamp.idl"
2 |
3 | #ifndef LEDPOINTS_IDL
4 | #define LEDPOINTS_IDL
5 |
6 | /**
7 | * \struct ImagePoint
8 | * \brief Image / pixel coordinates
9 | * \ingroup cpmlib_idl
10 | */
11 | struct ImagePoint
12 | {
13 | double x; //!< image coordinates in x direction
14 | double y; //!< image coordinates in y direction
15 | };
16 |
17 | /**
18 | * \struct LedPoints
19 | * \brief LED points that are detected in the IPS camera image.
20 | * Using raw pixel coordiantes, no calibration.
21 | * \ingroup cpmlib_idl
22 | */
23 | struct LedPoints
24 | {
25 | TimeStamp time_stamp; //!< timestamp when the image was recorded
26 | sequence led_points; //!< image coordinates of detected led points
27 | };
28 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/Log.idl:
--------------------------------------------------------------------------------
1 | #include "TimeStamp.idl"
2 |
3 | #ifndef LOG_IDL
4 | #define LOG_IDL
5 |
6 | /**
7 | * \struct Log
8 | * \brief A log message. These can be sent by any participant and are aggregated and shown in the LCC.
9 | * \ingroup cpmlib_idl
10 | */
11 | struct Log {
12 | //! ID of the log message sender, e.g. middleware
13 | string id; //@key
14 |
15 | //! Log content
16 | string content;
17 |
18 | //! When the log was created
19 | TimeStamp stamp;
20 |
21 | /**
22 | * \brief Log level from 0 (log nothing) to e.g. 3 (high verbosity) for filtering after message was received
23 | */
24 | unsigned short log_level;
25 | };
26 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/LogLevel.idl:
--------------------------------------------------------------------------------
1 | #ifndef LOGLEVEL_IDL
2 | #define LOGLEVEL_IDL
3 |
4 | /**
5 | * \struct LogLevel
6 | * \brief Log level from 0 (log nothing) to e.g. 3 (high verbosity)
7 | *
8 | * The Logging class in the cpm lib only prints messages with a log level <= the current one.
9 | * The lowest allowed level for msgs is 1, s.t. with 0 no messages are logged.
10 | * \ingroup cpmlib_idl
11 | */
12 | struct LogLevel {
13 | //! The log level that should be set within the network, usually sent by the LCC to other participants
14 | unsigned short log_level;
15 | };
16 |
17 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/ParameterRequest.idl:
--------------------------------------------------------------------------------
1 | #ifndef PARAMETERREQUEST_IDL
2 | #define PARAMETERREQUEST_IDL
3 |
4 | /**
5 | * \struct ParameterRequest
6 | * \brief Parameter values can be distributed by the LCC. To request a parameter that might have been set in this central instance,
7 | * send a parameter request message and wait for an answer to it (this behaviour is already implemented, e.g. in cpm::parameter_bool to get a boolean parameter)
8 | *
9 | * https://cpm.embedded.rwth-aachen.de/doc/pages/viewpage.action?pageId=11698287
10 | * \ingroup cpmlib_idl
11 | */
12 | struct ParameterRequest {
13 | //! Name of the parameter for which a value is requested
14 | string name; //@key
15 | };
16 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/Point2D.idl:
--------------------------------------------------------------------------------
1 | /**
2 | * \struct Point2D
3 | * \brief 2D point without rotation
4 | * \ingroup cpmlib_idl
5 | */
6 | struct Point2D {
7 | //! x position
8 | double x;
9 | //! y position
10 | double y;
11 | };
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/Pose2D.idl:
--------------------------------------------------------------------------------
1 | #ifndef POSE2D_IDL
2 | #define POSE2D_IDL
3 |
4 | /**
5 | * \struct Pose2D
6 | * \brief 2D position including rotation
7 | * \ingroup cpmlib_idl
8 | */
9 | struct Pose2D {
10 | double x; //!< meter
11 | double y; //!< meter
12 | double yaw; //!< radian
13 | };
14 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/RoundTripTime.idl:
--------------------------------------------------------------------------------
1 | #ifndef ROUNDTRIPTIME_IDL
2 | #define ROUNDTRIPTIME_IDL
3 |
4 | /**
5 | * \struct RoundTripTime
6 | * \brief These messages are sent by the LCC to measure the best current round trip time in the network (and to estimate the worst)
7 | * \ingroup cpmlib_idl
8 | */
9 | struct RoundTripTime {
10 | string source_id; //!< To find out where the msg came from and to not answer to own msg
11 |
12 | boolean is_answer; //!< If true, do not respond to this msg, as it already is an answer to an RTT request
13 | octet count; //!< Counter to not mix up old with new RTT requests
14 | };
15 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/StopRequest.idl:
--------------------------------------------------------------------------------
1 | /*
2 | * StopRequest is sent from a HLC to the LCC, stop request to stop everything,
3 | * e.g. because it failed and there is danger of collision.
4 | */
5 |
6 | #ifndef STOPREQUEST_IDL
7 | #define STOPREQUEST_IDL
8 |
9 | /**
10 | * \struct StopRequest
11 | * \brief TODO
12 | * \ingroup cpmlib_idl
13 | */
14 | struct StopRequest {
15 | //! TODO
16 | octet vehicle_id; //@key
17 | };
18 | #endif
19 |
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/TimeStamp.idl:
--------------------------------------------------------------------------------
1 | #ifndef TIMESTAMP_IDL
2 | #define TIMESTAMP_IDL
3 |
4 | /**
5 | * \struct TimeStamp
6 | * \brief Data type that contains a timestamp in nanoseconds since 1970, UTC
7 | * \ingroup cpmlib_idl
8 | */
9 | struct TimeStamp {
10 | //! Unix timestamp in nanoseconds, i.e. nanoseconds since 1970, UTC.
11 | unsigned long long nanoseconds;
12 | };
13 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/VehicleCommandDirect.idl:
--------------------------------------------------------------------------------
1 | #include "Header.idl"
2 |
3 | #ifndef VEHICLECOMMANDDIRECT_IDL
4 | #define VEHICLECOMMANDDIRECT_IDL
5 |
6 | /**
7 | * \struct VehicleCommandDirect
8 | * \brief Direct control commands that can be sent to the vehicle
9 | *
10 | * Uses dimensionless, uncalibrated inputs and applies them directly to the motor and servo.
11 | *
12 | * (motor_throttle == 1) => Max forward
13 | *
14 | * (motor_throttle == 0) => Brake to standstill
15 | *
16 | * (motor_throttle == -1) => Max reverse
17 | *
18 | * (steering_servo == 1) => Max left
19 | *
20 | * (steering_servo == 0) => Steering roughly centered, but may have an offset
21 | *
22 | * (steering_servo == -1) => Max right
23 | * \ingroup cpmlib_idl
24 | */
25 | struct VehicleCommandDirect
26 | {
27 | //! ID of the vehicle to control
28 | octet vehicle_id; //@key
29 |
30 | //! Header information, e.g. to see if the msg is still valid
31 | Header header;
32 |
33 | double motor_throttle; //!< dimensionless, in [-1, 1]
34 | double steering_servo; //!< dimensionless, in [-1, 1]
35 | };
36 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/VehicleCommandPathTracking.idl:
--------------------------------------------------------------------------------
1 | #include "Header.idl"
2 | #include "Pose2D.idl"
3 |
4 | #ifndef VEHICLECOMMANDPATHTRACKING_IDL
5 | #define VEHICLECOMMANDPATHTRACKING_IDL
6 |
7 | /**
8 | * \struct PathPoint
9 | * \brief TODO
10 | * \ingroup cpmlib_idl
11 | */
12 | struct PathPoint
13 | {
14 | //! TODO
15 | Pose2D pose;
16 | //! TODO
17 | double s; // m
18 | };
19 |
20 | /**
21 | * \struct VehicleCommandPathTracking
22 | * \brief TODO
23 | * \ingroup cpmlib_idl
24 | */
25 | struct VehicleCommandPathTracking
26 | {
27 | //! TODO
28 | octet vehicle_id; //@key
29 | //! TODO
30 | Header header;
31 | //! TODO
32 | sequence path;
33 | //! TODO
34 | double speed; // [m/s]
35 | };
36 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/VehicleCommandSpeedCurvature.idl:
--------------------------------------------------------------------------------
1 | #include "Header.idl"
2 |
3 | #ifndef VEHICLECOMMANDSPEEDCURVATURE_IDL
4 | #define VEHICLECOMMANDSPEEDCURVATURE_IDL
5 |
6 | /**
7 | * \struct VehicleCommandSpeedCurvature
8 | * \brief Used to send speed and curvature data to control a vehicle
9 | *
10 | * SpeedCurvatureMode
11 | *
12 | * Uses a feedback controller to match the given reference speed.
13 | *
14 | * Uses a feedforward controller to steer along a path with the given curvature.
15 | *
16 | * This only works with "normal" driving, results may vary when spinning or sliding.
17 | * \ingroup cpmlib_idl
18 | */
19 | struct VehicleCommandSpeedCurvature
20 | {
21 | //! ID of the vehicle to control
22 | octet vehicle_id; //@key
23 |
24 | //! Header information, e.g. to see if the msg is still valid
25 | Header header;
26 |
27 | double speed; //!< m/s
28 | double curvature; //!< 1/meter
29 | };
30 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/VehicleObservation.idl:
--------------------------------------------------------------------------------
1 | #include "Header.idl"
2 | #include "Pose2D.idl"
3 |
4 | #ifndef VEHICLE_OBSERVATION_IDL
5 | #define VEHICLE_OBSERVATION_IDL
6 |
7 | /**
8 | * \struct VehicleObservation
9 | * \brief Vehicle observation data from the IPS (gives position of a vehicle)
10 | * \ingroup cpmlib_idl
11 | */
12 | struct VehicleObservation
13 | {
14 | //! ID of the vehicle the observation belongs to
15 | octet vehicle_id; //@key
16 |
17 | //! Header data
18 | Header header;
19 |
20 | //! Position of the vehicle
21 | Pose2D pose;
22 | };
23 | #endif
--------------------------------------------------------------------------------
/cpm_lib/dds_idl/VehicleStateList.idl:
--------------------------------------------------------------------------------
1 | #include "VehicleState.idl"
2 | #include "VehicleObservation.idl"
3 |
4 | #ifndef VEHICLESTATELIST_IDL
5 | #define VEHICLESTATELIST_IDL
6 | //TODO rename
7 |
8 | /**
9 | * \struct VehicleStateList
10 | * \brief Messages sent from the Middleware to the HLC, containing relevant vehicle information, the current time and the periodicity of the calls
11 | * Also functions as "wake up" call that means that the HLC can start computation with this data
12 | * \ingroup cpmlib_idl
13 | */
14 | struct VehicleStateList {
15 | //!Current time, should be used by the HLC instead of using its own clock
16 | unsigned long long t_now;
17 |
18 | //!Periodicity of calling the HLC
19 | unsigned long long period_ms;
20 |
21 | //!List of vehicle state information for all vehicles
22 | sequence state_list;
23 |
24 | //!List of vehicle observation information for all vehicles
25 | sequence vehicle_observation_list;
26 | sequence active_vehicle_ids;
27 | };
28 | #endif
29 |
--------------------------------------------------------------------------------
/cpm_lib/include/cpm/VehicleIDFilteredTopic.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | namespace cpm
7 | {
8 | /**
9 | * \class VehicleIDFilteredTopic
10 | * \brief Creates a DDS Topic that filters all incoming
11 | * messages so that only samples with the given
12 | * vehicle_id are processed further.
13 | * \ingroup cpmlib
14 | */
15 | template
16 | struct VehicleIDFilteredTopic : public dds::topic::ContentFilteredTopic
17 | {
18 | /**
19 | * Takes the topic which needs to be filtered. Only
20 | * samples with the id vehicle_id will be considered if the
21 | * filter is applied to the topic and used by e.g. a DDS
22 | * Reader.
23 | * \param topic reference to the topic which is supposed to be used
24 | * \param vehicle_id reference to the vehicle id
25 | * \return a ContentFilteredTopic which filters the vehicle id
26 | */
27 | VehicleIDFilteredTopic(const dds::topic::Topic &topic, const uint8_t &vehicle_id)
28 | :dds::topic::ContentFilteredTopic(
29 | topic,
30 | topic.name() + "_vehicle_id_filtered_" + std::to_string(vehicle_id),
31 | dds::topic::Filter("vehicle_id = " + std::to_string(vehicle_id))
32 | )
33 | {
34 | static_assert(std::is_same().vehicle_id()), uint8_t>::value, "IDL type must have a vehicle_id.");
35 | }
36 | };
37 | }
--------------------------------------------------------------------------------
/cpm_lib/include/cpm/exceptions.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 | namespace cpm {
6 | /**
7 | * \brief Small error class for the timer
8 | * \ingroup cpmlib
9 | */
10 | class ErrorTimerStart: public std::runtime_error {
11 | public:
12 | /**
13 | * \brief Constructor / Error type for timer error
14 | * \param msg Error message
15 | */
16 | ErrorTimerStart(const std::string& msg);
17 | };
18 | }
--------------------------------------------------------------------------------
/cpm_lib/include/cpm/get_time_ns.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include
3 | #include
4 |
5 | namespace cpm {
6 | /**
7 | * \brief Global function to access the current system time in nanoseconds, saves redundant code
8 | * \ingroup cpmlib
9 | */
10 | uint64_t get_time_ns();
11 |
12 | /**
13 | * \brief Same as get_time_ns but allows specifying the clock type
14 | */
15 | uint64_t get_time_ns(clockid_t clockid);
16 | }
--------------------------------------------------------------------------------
/cpm_lib/include/cpm/init.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | namespace cpm
4 | {
5 | /**
6 | * This function can be used to initialize the cpm lib given command line parameters
7 | * \param argc Command line parameter of main()
8 | * \param argv Command line parameter of main()
9 | * \ingroup cpmlib
10 | */
11 | void init(int argc, char *argv[]);
12 | }
--------------------------------------------------------------------------------
/cpm_lib/include/cpm/stamp_message.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | /**
4 | * \file stamp_message.hpp
5 | * \brief This interface provides a method that can be used to
6 | * set a time stamp for a sample (for creation and validity)
7 | */
8 |
9 | #include
10 |
11 | namespace cpm
12 | {
13 | /**
14 | * \brief This function takes a sample reference and
15 | * sets its timestamp for the time of creation (t_now)
16 | * and the timestamp for the time of validity (t_now + expected_delay)
17 | * from which on the sample can be used by the receiver
18 | * \param message the sample whose header needs to be set
19 | * \param t_now the current system time in nanoseconds
20 | * \param expected_delay the amount of nanoseconds before the sample becomes valid (starting at t_now)
21 | * \ingroup cpmlib
22 | */
23 | template
24 | void stamp_message(T& message, uint64_t t_now, uint64_t expected_delay)
25 | {
26 | message.header().create_stamp().nanoseconds(t_now);
27 | message.header().valid_after_stamp().nanoseconds(t_now + expected_delay);
28 | }
29 | }
--------------------------------------------------------------------------------
/cpm_lib/rtigen.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | set -e
3 |
4 | mkdir -p dds_idl_cpp
5 |
6 | # Generate IDL C++
7 | find dds_idl/ -type f | xargs -n 1 rtiddsgen -replace -legacyPlugin -language C++11 -d ./dds_idl_cpp/
8 |
9 | # Copy headers to public inclues
10 | mkdir -p include/cpm/dds
11 | (cd dds_idl_cpp;find -type f) | grep \\.h | xargs -n 1 -I ARG cp dds_idl_cpp/ARG include/cpm/dds/
12 |
--------------------------------------------------------------------------------
/cpm_lib/rtigen_matlab.m:
--------------------------------------------------------------------------------
1 | function rtigen_matlab
2 | setenv("LD_LIBRARY_PATH", ['/opt/rti_connext_dds-6.0.0/lib/x64Linux4gcc7.3.0']);
3 | dds_idl_matlab_dir = 'dds_idl_matlab';
4 |
5 | % IDL files from cpm library
6 | dds_idl_dir = fullfile('dds_idl');
7 | if ~exist(dds_idl_dir, 'dir')
8 | error(['Missing directory "' dds_idl_dir '"']);
9 | end
10 | addpath(dds_idl_dir)
11 |
12 | % import idl files
13 | idl_files = [dir(fullfile(dds_idl_dir, '*.idl'))];
14 | mkdir(dds_idl_matlab_dir);
15 | cd(dds_idl_matlab_dir);
16 | for f = {idl_files.name}
17 | DDS.import(f{1},'matlab', 'f')
18 | end
19 | end
--------------------------------------------------------------------------------
/cpm_lib/src/InternalConfiguration.cpp:
--------------------------------------------------------------------------------
1 | #include "InternalConfiguration.hpp"
2 | #include "cpm/init.hpp"
3 | #include "cpm/CommandLineReader.hpp"
4 | #include "cpm/Logging.hpp"
5 | #include "cpm/RTTTool.hpp"
6 |
7 | /**
8 | * \file InternalConfiguration.cpp
9 | * \ingroup cpmlib
10 | */
11 |
12 | namespace cpm
13 | {
14 | void init(int argc, char *argv[])
15 | {
16 | InternalConfiguration::init(argc, argv);
17 | }
18 |
19 |
20 | void InternalConfiguration::init(int argc, char *argv[])
21 | {
22 | InternalConfiguration::the_instance = InternalConfiguration(
23 | cmd_parameter_int("dds_domain", 0, argc, argv),
24 | cmd_parameter_string("logging_id", "uninitialized", argc, argv),
25 | cmd_parameter_string("dds_initial_peer", "", argc, argv)
26 | );
27 |
28 | // TODO reverse access, i.e. access the config from the logging
29 | cpm::Logging::Instance().set_id(InternalConfiguration::Instance().get_logging_id());
30 | }
31 |
32 |
33 | InternalConfiguration InternalConfiguration::the_instance;
34 | }
--------------------------------------------------------------------------------
/cpm_lib/src/Timer.cpp:
--------------------------------------------------------------------------------
1 | #include "cpm/Timer.hpp"
2 | #include "cpm/Parameter.hpp"
3 | #include "cpm/Logging.hpp"
4 | #include "cpm/TimerFD.hpp"
5 | #include "TimerSimulated.hpp"
6 |
7 | /**
8 | * \file Timer.cpp
9 | * \ingroup cpmlib
10 | */
11 |
12 | namespace cpm {
13 |
14 | std::shared_ptr Timer::create(
15 | std::string node_id,
16 | uint64_t period_nanoseconds,
17 | uint64_t offset_nanoseconds,
18 | bool wait_for_start,
19 | bool simulated_time_allowed,
20 | bool simulated_time
21 | )
22 | {
23 | // Switch between FD and simulated time
24 | if (simulated_time && simulated_time_allowed) {
25 | //Use timer for simulated time
26 | return std::make_shared(node_id, period_nanoseconds, offset_nanoseconds);
27 | }
28 | else if (simulated_time && !simulated_time_allowed) {
29 | Logging::Instance().write(
30 | 1,
31 | "%s",
32 | "Timer Error: simulated time requested but not allowed."
33 | );
34 | fprintf(stderr, "Error: simulated time requested but not allowed.\n");
35 | fflush(stderr);
36 | exit(EXIT_FAILURE);
37 | }
38 | else {
39 | //Use timer for real time
40 | return std::make_shared(node_id, period_nanoseconds, offset_nanoseconds, wait_for_start);
41 | }
42 | }
43 |
44 |
45 | }
--------------------------------------------------------------------------------
/cpm_lib/src/exceptions.cpp:
--------------------------------------------------------------------------------
1 | #include "cpm/exceptions.hpp"
2 |
3 | /**
4 | * \file exceptions.cpp
5 | * \ingroup cpmlib
6 | */
7 |
8 | namespace cpm {
9 | ErrorTimerStart::ErrorTimerStart(const std::string& msg):
10 | std::runtime_error(msg)
11 | {
12 | }
13 | }
--------------------------------------------------------------------------------
/cpm_lib/src/get_time_ns.cpp:
--------------------------------------------------------------------------------
1 | #include "cpm/get_time_ns.hpp"
2 |
3 | /**
4 | * \file get_time_ns.cpp
5 | * \ingroup cpmlib
6 | */
7 |
8 | uint64_t cpm::get_time_ns(clockid_t clockid) {
9 | struct timespec t;
10 | clock_gettime(clockid, &t);
11 | return uint64_t(t.tv_sec) * 1000000000ull + uint64_t(t.tv_nsec);
12 | }
13 |
14 | uint64_t cpm::get_time_ns() {
15 | return cpm::get_time_ns(CLOCK_REALTIME);
16 | }
--------------------------------------------------------------------------------
/cpm_lib/test/QOS_TEST.xml:
--------------------------------------------------------------------------------
1 |
2 |
5 |
9 |
10 |
11 |
16 |
17 |
18 |
19 | TestParticipant
20 | TestParticipantRole
21 |
22 |
23 |
24 |
25 |
26 |
27 |
--------------------------------------------------------------------------------
/cpm_lib/test/catch.cpp:
--------------------------------------------------------------------------------
1 | #define CATCH_CONFIG_MAIN
2 | #include "catch.hpp"
--------------------------------------------------------------------------------
/cpm_lib/test/test_InternalConfiguration.cpp:
--------------------------------------------------------------------------------
1 | #include "catch.hpp"
2 | #include "InternalConfiguration.hpp"
3 | #include "cpm/init.hpp"
4 |
5 | /**
6 | * \test Tests InternalConfiguration by faking some command line input
7 | * \ingroup cpmlib
8 | */
9 | TEST_CASE( "InternalConfiguration" )
10 | {
11 | char program_name[] = "irrelevant_program";
12 | char arg1[] = "--dds_domain=31";
13 | char arg2[] = "--logging_id=hello";
14 | char *argv[] = { program_name, arg1, arg2 };
15 | int argc = 3;
16 |
17 | cpm::init(argc, argv);
18 |
19 | CHECK( cpm::InternalConfiguration::Instance().get_dds_domain() == 31 );
20 | CHECK( cpm::InternalConfiguration::Instance().get_logging_id() == "hello" );
21 | }
--------------------------------------------------------------------------------
/high_level_controller/.gitignore:
--------------------------------------------------------------------------------
1 | # Ignore everything in the current folder
2 | /*
3 |
4 | # But not these files...
5 | !.gitignore
6 |
7 | # and not these folders
8 | !autostart/
9 | !examples/
--------------------------------------------------------------------------------
/high_level_controller/autostart/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.1)
2 | add_definitions(-Wall -Wextra -Werror=return-type)
3 | set (CMAKE_CXX_STANDARD 17)
4 |
5 | # RTI DDS
6 | add_definitions(-DRTI_UNIX -DRTI_LINUX -DRTI_64BIT -DRTI_STATIC)
7 | include_directories(SYSTEM $ENV{NDDSHOME}/include)
8 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds)
9 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds/hpp)
10 | link_libraries(dl nsl m pthread rt)
11 | link_libraries(nddscpp2z nddscz nddscorez)
12 | link_directories($ENV{NDDSHOME}/lib/x64Linux4gcc7.3.0)
13 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -m64 -Wl,--no-as-needed")
14 |
15 | # CPM lib
16 | include_directories(SYSTEM ../../cpm_lib/include/)
17 | include_directories(SYSTEM ../../cpm_lib/include/cpm/dds/)
18 | link_directories(../../cpm_lib/build/)
19 |
20 |
21 | include_directories(src)
22 |
23 | set (SOURCES
24 | )
25 |
26 | add_executable( autostart
27 | src/main.cpp
28 | ${SOURCES}
29 | )
30 |
31 | target_link_libraries(autostart cpm)
32 |
33 | add_executable( test_receiver
34 | src/test_receiver.cpp
35 | ${SOURCES}
36 | )
37 |
38 | target_link_libraries(test_receiver cpm)
39 |
40 | add_executable( download_error_logger
41 | src_error_logger/main.cpp
42 | ${SOURCES}
43 | )
44 |
45 | target_link_libraries(download_error_logger stdc++fs cpm)
--------------------------------------------------------------------------------
/high_level_controller/autostart/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # exit when any command fails
3 | set -e
4 |
5 | # Get directory of bash script
6 | BASH_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
7 |
8 | mkdir -p build
9 | cd build
10 |
11 | cmake ..
12 | make -j$(nproc)
13 | cd ..
14 |
15 | # Publish autostart package via http/apache for the HLCs to download - TODO: Change tmp-version to local version after merge, similar to middleware build
16 | cd /${BASH_DIR}
17 | rm -rf autostart_package
18 | mkdir autostart_package
19 | cp ${BASH_DIR}/build/autostart ./autostart_package
20 | cp ${BASH_DIR}/build/download_error_logger ./autostart_package
21 | cp ${BASH_DIR}/lab_autostart.bash ./autostart_package # This file will be updated by lab_autostart itself, meaning that an update of that file only takes effect after a NUC restart
22 | tar -czf autostart_package.tar.gz autostart_package
23 | rm -f /var/www/html/nuc/autostart_package.tar.gz
24 | cp ./autostart_package.tar.gz /var/www/html/nuc
25 |
--------------------------------------------------------------------------------
/high_level_controller/autostart/create_nuc_package.bash:
--------------------------------------------------------------------------------
1 | # This file is supposed to be used to create a package that is downloaded by the NUC AFTER IT HAS BOOTED
2 | # -> Only files that are probably relevant to all Matlab scripts are uploaded at this points
3 | # Scripts are still supposed to be uploaded using the LCC's UI (or your own upload script)
4 |
5 | # exit when any command fails
6 | set -e
7 |
8 | # Get directory of bash script
9 | BASH_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
10 |
11 | # Publish NUC package via http/apache for the HLCs to download
12 |
13 |
14 | cd /${BASH_DIR}
15 | rm -rf matlab_package
16 | mkdir matlab_package
17 | cp ${BASH_DIR}/../examples/matlab/read_system_trigger.m ./matlab_package
18 | cp ${BASH_DIR}/../examples/matlab/init_script.m ./matlab_package
19 | cp ${BASH_DIR}/../examples/matlab/QOS_READY_TRIGGER.xml ./matlab_package
20 | tar -czf matlab_package.tar.gz matlab_package
21 | rm -f /var/www/html/nuc/matlab_package.tar.gz
22 | cp ./matlab_package.tar.gz /var/www/html/nuc
23 |
--------------------------------------------------------------------------------
/high_level_controller/autostart/dummy_program_check.bash:
--------------------------------------------------------------------------------
1 | #!bin/bash
2 |
3 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib64/:./cpm:/opt/rti_connext_dds-6.0.0/lib/x64Linux4gcc7.3.0
4 | export PATH=$PATH:$HOME/bin:$HOME/.local/bin:/opt/rti_connext_dds-6.0.0:/opt/rti_connext_dds-6.0.0/bin:/opt/rti_connext_dds-6.0.0/lib/x64Linux4gcc7.3.0:/opt/rti_connext_dds-6.0.0/bin:/opt/raspbian-toolchain-gcc-4.7.2-linux64/bin
5 | export NDDSHOME=/opt/rti_connext_dds-6.0.0
6 | export RASPBIAN_TOOLCHAIN=/opt/raspbian-toolchain-gcc-4.7.2-linux64
7 | export RTI_LICENSE_FILE=/opt/rti_connext_dds-6.0.0/rti_license.dat
8 |
9 | export DDS_INITIAL_PEER=rtps@udpv4://192.168.1.249:25598
10 |
11 | exit_script() {
12 | tmux kill-session -t "rticlouddiscoveryservice"
13 | }
14 |
15 | trap exit_script SIGINT SIGTERM
16 |
17 | # Start local software (WARNING: domain hardcoded right now)
18 | # tmux new-session -d -s "rticlouddiscoveryservice" "rticlouddiscoveryservice -transport 25598 >stdout_rticlouddiscoveryservice.txt 2>stderr_rticlouddiscoveryservice.txt"
19 |
20 | # Default domain is 21, just like the vehicle default domain (-> domain for real lab tests)
21 | ./build/dummy_program_check --dds_domain=21 --dds_initial_peer=$DDS_INITIAL_PEER
--------------------------------------------------------------------------------
/high_level_controller/autostart/local_test.bash:
--------------------------------------------------------------------------------
1 | # Load environment Variables
2 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
3 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
4 |
5 | ./build/autostart --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --number_of_vehicles=20
--------------------------------------------------------------------------------
/high_level_controller/autostart/simple_copy.bash:
--------------------------------------------------------------------------------
1 | # scp /home/cpm/dev/software/high_level_controller/autostart/lab_autostart.bash controller@192.168.1.215:/home/controller/dev/autostart
2 | # scp /home/cpm/dev/software/high_level_controller/autostart/build/autostart controller@192.168.1.215:/home/controller/dev/autostart
3 | # scp /home/cpm/dev/software/cpm_lib/build/libcpm.so controller@192.168.1.215:/home/controller/dev/autostart/cpm
4 |
5 | # Get autostart folder directory relative to this script's directory
6 | DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )/"
7 | ABSOLUTE_AUTOSTART_DIR="$(realpath "${DIR}")"
8 |
9 | scp ${ABSOLUTE_AUTOSTART_DIR}/lab_autostart.bash guest@192.168.1.202:/home/guest/autostart
10 | scp ${ABSOLUTE_AUTOSTART_DIR}/lab_autostart.bash guest@192.168.1.219:/home/guest/autostart
11 | scp ${ABSOLUTE_AUTOSTART_DIR}/lab_autostart.bash guest@192.168.1.201:/home/guest/autostart
12 | scp ${ABSOLUTE_AUTOSTART_DIR}/lab_autostart.bash guest@192.168.1.203:/home/guest/autostart
13 |
14 |
15 | # @reboot ~/dev/autostart/lab_autostart.bash
--------------------------------------------------------------------------------
/high_level_controller/autostart/src/test_receiver.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * \file main.cpp
3 | * \brief This file includes a reader that receives NUC messages
4 | */
5 |
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 |
12 | #include "HLCHello.hpp"
13 |
14 | #include "cpm/ParticipantSingleton.hpp"
15 | #include "cpm/get_topic.hpp"
16 | #include "cpm/Logging.hpp"
17 | #include "cpm/CommandLineReader.hpp"
18 | #include "cpm/init.hpp"
19 | #include "cpm/AsyncReader.hpp"
20 |
21 | int main (int argc, char *argv[]) {
22 | //Initialize the cpm logger, set domain id etc
23 | cpm::init(argc, argv);
24 | cpm::Logging::Instance().set_id("hlc_hello");
25 |
26 | //Create DataReader that reads NUC ready messages
27 | cpm::AsyncReader reader(
28 | [](std::vector& samples){
29 | for (auto& data : samples)
30 | {
31 | std::cout << "Received: " << data << std::endl;
32 | }
33 | },
34 | "hlc_hello",
35 | true
36 | );
37 |
38 | std::cout << "Press Enter to stop the program" << std::endl;
39 | std::cin.get();
40 |
41 | return 0;
42 | }
--------------------------------------------------------------------------------
/high_level_controller/autostart/test_receiver.bash:
--------------------------------------------------------------------------------
1 | #!bin/bash
2 |
3 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib64/:./cpm:/opt/rti_connext_dds-6.0.0/lib/x64Linux4gcc7.3.0
4 | export PATH=$PATH:$HOME/bin:$HOME/.local/bin:/opt/rti_connext_dds-6.0.0:/opt/rti_connext_dds-6.0.0/bin:/opt/rti_connext_dds-6.0.0/lib/x64Linux4gcc7.3.0:/opt/rti_connext_dds-6.0.0/bin:/opt/raspbian-toolchain-gcc-4.7.2-linux64/bin
5 | export NDDSHOME=/opt/rti_connext_dds-6.0.0
6 | export RASPBIAN_TOOLCHAIN=/opt/raspbian-toolchain-gcc-4.7.2-linux64
7 | export RTI_LICENSE_FILE=/opt/rti_connext_dds-6.0.0/rti_license.dat
8 |
9 | export DDS_INITIAL_PEER=rtps@udpv4://192.168.1.249:25598
10 |
11 | exit_script() {
12 | tmux kill-session -t "rticlouddiscoveryservice"
13 | }
14 |
15 | trap exit_script SIGINT SIGTERM
16 |
17 | # Start local software (WARNING: domain hardcoded right now)
18 | tmux new-session -d -s "rticlouddiscoveryservice" "rticlouddiscoveryservice -transport 25598 >stdout_rticlouddiscoveryservice.txt 2>stderr_rticlouddiscoveryservice.txt"
19 |
20 | # Default domain is 21, just like the vehicle default domain (-> domain for real lab tests)
21 | ./build/test_receiver --dds_domain=21 --dds_initial_peer=$DDS_INITIAL_PEER
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/basic_circle/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.1)
2 | add_definitions(-Wall -Wextra -Werror=return-type)
3 | set (CMAKE_CXX_STANDARD 11)
4 |
5 | # RTI DDS
6 | add_definitions(-DRTI_UNIX -DRTI_LINUX -DRTI_64BIT -DRTI_STATIC)
7 | include_directories(SYSTEM $ENV{NDDSHOME}/include)
8 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds)
9 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds/hpp)
10 | link_libraries(nddscpp2z nddscz nddscorez)
11 | link_directories($ENV{NDDSHOME}/lib/x64Linux4gcc7.3.0)
12 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -m64 -Wl,--no-as-needed")
13 |
14 |
15 | # CPM lib
16 | include_directories(SYSTEM ../../../../cpm_lib/include/)
17 | include_directories(SYSTEM ../../../../cpm_lib/include/cpm/dds/)
18 | link_directories(../../../../cpm_lib/build/)
19 |
20 |
21 |
22 | include_directories(src)
23 | link_libraries(dl nsl m pthread rt)
24 |
25 | add_executable(basic_circle
26 | src/main.cpp
27 | )
28 | target_link_libraries(basic_circle cpm)
29 |
30 |
31 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/basic_circle/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # exit when any command fails
3 | set -e
4 |
5 | mkdir -p build
6 |
7 | # Copy local communication QoS, use correct IP
8 | IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
9 | sed -e "s/TEMPLATE_IP/${IP_SELF}/g" \
10 | <./QOS_LOCAL_COMMUNICATION.xml.template \
11 | >./build/QOS_LOCAL_COMMUNICATION.xml
12 |
13 | cd build
14 | cmake ..
15 | make -j$(nproc)
16 | cd ..
17 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/basic_circle/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 | if [ $# -eq 0 ]
8 | then
9 | echo "Missing argument vehicle ID"
10 | else
11 | ./build/basic_circle --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --vehicle_id=$1
12 | fi
13 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/basic_line/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.1)
2 | add_definitions(-Wall -Wextra -Werror=return-type)
3 | set (CMAKE_CXX_STANDARD 11)
4 |
5 | # RTI DDS
6 | add_definitions(-DRTI_UNIX -DRTI_LINUX -DRTI_64BIT -DRTI_STATIC)
7 | include_directories(SYSTEM $ENV{NDDSHOME}/include)
8 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds)
9 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds/hpp)
10 | link_libraries(nddscpp2z nddscz nddscorez)
11 | link_directories($ENV{NDDSHOME}/lib/x64Linux4gcc7.3.0)
12 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -m64 -Wl,--no-as-needed")
13 |
14 |
15 | # CPM lib
16 | include_directories(SYSTEM ../../../../cpm_lib/include/)
17 | include_directories(SYSTEM ../../../../cpm_lib/include/cpm/dds/)
18 | link_directories(../../../../cpm_lib/build/)
19 |
20 |
21 |
22 | include_directories(src)
23 | link_libraries(dl nsl m pthread rt)
24 |
25 |
26 |
27 | add_executable(basic_line
28 | src/main.cpp
29 | )
30 | target_link_libraries(basic_line cpm)
31 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/basic_line/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | mkdir build
5 | cd build
6 | cmake ..
7 | make -j$(nproc)
8 | cd ..
9 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/basic_line/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 | if [ $# -eq 0 ]
8 | then
9 | echo "Missing argument vehicle ID"
10 | else
11 | ./build/basic_line --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --vehicle_id=$1
12 | fi
13 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/central_routing/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # exit when any command fails
3 | set -e
4 |
5 | mkdir -p build
6 |
7 | # Copy local communication QoS, use correct IP
8 | IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
9 | sed -e "s/TEMPLATE_IP/${IP_SELF}/g" \
10 | <./QOS_LOCAL_COMMUNICATION.xml.template \
11 | >./build/QOS_LOCAL_COMMUNICATION.xml
12 |
13 | cd build
14 | cmake .. -DCMAKE_BUILD_TYPE=Release
15 | make -j$(nproc)
16 | cd ..
17 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/central_routing/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 | if [ $# -eq 0 ]
8 | then
9 | echo "Missing argument vehicle ID list (comma separated)"
10 | else
11 | ./build/central_routing --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --vehicle_ids=$1
12 | fi
13 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/central_routing/src/lane_graph_tools.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include "lane_graph.hpp"
3 | #include "Pose2D.hpp"
4 | #include
5 | using std::vector;
6 |
7 | /**
8 | * \class LaneGraphTools
9 | * \brief TODO
10 | * \ingroup central_routing
11 | */
12 | class LaneGraphTools : public LaneGraph
13 | {
14 | //! TODO
15 | std::vector> edges_s;
16 | public:
17 | //! TODO
18 | std::vector< std::vector< std::vector< std::vector > > > edge_path_collisions;
19 |
20 | //! Constructor TODO
21 | LaneGraphTools();
22 |
23 | /**
24 | * \brief TODO
25 | * \param pose TODO
26 | * \param out_edge_index TODO
27 | * \param out_edge_path_index TODO
28 | */
29 | bool map_match_pose(Pose2D pose, int &out_edge_index, int &out_edge_path_index) const;
30 |
31 | /**
32 | * \brief TODO
33 | * \param edge_index TODO
34 | */
35 | vector find_subsequent_edges(int edge_index) const;
36 |
37 | /**
38 | * \brief TODO
39 | * \param route_edge_indices
40 | * \param edge_index
41 | * \param edge_path_index
42 | * \param delta_s
43 | */
44 | void move_along_route(vector route_edge_indices, size_t &edge_index, size_t &edge_path_index, double &delta_s) const;
45 | };
46 |
47 | /**
48 | * \brief TODO
49 | * \ingroup central_routing
50 | */
51 | extern const LaneGraphTools laneGraphTools;
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/controller_test_loop/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.1)
2 | add_definitions(-Wall -Wextra -Werror=return-type)
3 | set (CMAKE_CXX_STANDARD 11)
4 |
5 | # RTI DDS
6 | add_definitions(-DRTI_UNIX -DRTI_LINUX -DRTI_64BIT -DRTI_STATIC)
7 | include_directories(SYSTEM $ENV{NDDSHOME}/include)
8 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds)
9 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds/hpp)
10 | link_libraries(nddscpp2z nddscz nddscorez)
11 | link_directories($ENV{NDDSHOME}/lib/x64Linux4gcc7.3.0)
12 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -m64 -Wl,--no-as-needed")
13 |
14 |
15 | # CPM lib
16 | include_directories(SYSTEM ../../../../cpm_lib/include/)
17 | include_directories(SYSTEM ../../../../cpm_lib/include/cpm/dds/)
18 | link_directories(../../../../cpm_lib/build/)
19 |
20 |
21 |
22 | include_directories(src)
23 | link_libraries(dl nsl m pthread rt)
24 |
25 | add_executable(controller_test_loop
26 | src/main.cpp
27 | )
28 | target_link_libraries(controller_test_loop cpm)
29 |
30 |
31 |
32 |
33 | add_executable(platoon
34 | src/main_platoon.cpp
35 | )
36 | target_link_libraries(platoon cpm)
37 |
38 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/controller_test_loop/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # exit when any command fails
3 | set -e
4 |
5 | mkdir -p build
6 | cd build
7 | cmake ..
8 | make -j$(nproc)
9 | cd ..
10 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/controller_test_loop/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 |
8 | ./build/controller_test_loop --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER
9 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/controller_test_loop/run_platoon.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 |
8 | ./build/platoon --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER
9 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/diagonal_figure_eight/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.1)
2 | add_definitions(-Wall -Wextra -Werror=return-type)
3 | set (CMAKE_CXX_STANDARD 11)
4 |
5 | # RTI DDS
6 | add_definitions(-DRTI_UNIX -DRTI_LINUX -DRTI_64BIT -DRTI_STATIC)
7 | include_directories(SYSTEM $ENV{NDDSHOME}/include)
8 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds)
9 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds/hpp)
10 | link_libraries(nddscpp2z nddscz nddscorez)
11 | link_directories($ENV{NDDSHOME}/lib/x64Linux4gcc7.3.0)
12 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -m64 -Wl,--no-as-needed")
13 |
14 |
15 | # CPM lib
16 | include_directories(SYSTEM ../../../../cpm_lib/include/)
17 | include_directories(SYSTEM ../../../../cpm_lib/include/cpm/dds/)
18 | link_directories(../../../../cpm_lib/build/)
19 |
20 |
21 |
22 | include_directories(src)
23 | link_libraries(dl nsl m pthread rt)
24 |
25 | add_executable(diagonal_figure_eight
26 | src/main.cpp
27 | )
28 | target_link_libraries(diagonal_figure_eight cpm)
29 |
30 |
31 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/diagonal_figure_eight/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | mkdir build
5 | cd build
6 | cmake ..
7 | make -j$(nproc)
8 | cd ..
9 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/diagonal_figure_eight/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 | if [ $# -eq 0 ]
8 | then
9 | echo "Missing argument vehicle ID"
10 | else
11 | ./build/diagonal_figure_eight --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --vehicle_id=$1
12 | fi
13 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/distributed_routing/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # exit when any command fails
3 | set -e
4 |
5 | mkdir -p build
6 |
7 | # Copy local communication QoS, use correct IP
8 | IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
9 | sed -e "s/TEMPLATE_IP/${IP_SELF}/g" \
10 | <./QOS_LOCAL_COMMUNICATION.xml.template \
11 | >./build/QOS_LOCAL_COMMUNICATION.xml
12 |
13 | cd build
14 | cmake ..
15 | make -j$(nproc)
16 | cd ..
17 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/distributed_routing/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 | if [ $# -eq 0 ]
8 | then
9 | echo "Missing argument vehicle ID list (comma separated)"
10 | else
11 | ./build/distributed_routing --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --vehicle_ids=$1
12 | fi
13 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # exit when any command fails
3 | set -e
4 |
5 | mkdir -p build
6 |
7 | # Copy local communication QoS, use correct IP
8 | IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
9 | sed -e "s/TEMPLATE_IP/${IP_SELF}/g" \
10 | <./QOS_LOCAL_COMMUNICATION.xml.template \
11 | >./build/QOS_LOCAL_COMMUNICATION.xml
12 |
13 | ./rtigen.bash
14 | cd build
15 | cmake ..
16 | make -j$(nproc)
17 | cd ..
18 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/eval.sh:
--------------------------------------------------------------------------------
1 | # Generates evaluation logs for the specified modes for N (as set) vehicles.
2 | # It places them in the folder ./data/MODE/N/
3 | # It currently uses the planning horizon which is hardcoded in the HLC. To generate data for different planning horizons the value of N_STEPS_SPEED_PROFILE has to be changed in VehicleTrajectoryPlanningState.hpp.
4 |
5 | HORIZON="200"
6 | SEEDSTART=101
7 | SEEDEND=105
8 |
9 | for ((SEED=SEEDSTART;SEED<=SEEDEND;SEED++))
10 | do
11 | mkdir -p data_seed_${SEED}
12 | cd ./data_seed_${SEED}
13 | mkdir -p 200 # horizon in timesteps
14 | cd 200
15 | for MODE in 0 1 2
16 | do
17 | mkdir -p $MODE
18 | cd $MODE
19 | for N in 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20
20 | do
21 | if [[ -d "$N" ]]
22 | then
23 | continue
24 | fi
25 | mkdir $N
26 | cd ./$N
27 | ../../../../build/simulation --n=$N --hlc_mode=$MODE --path_seed=$SEED --prio_seed=$SEED >> /dev/null
28 | cd ..
29 | done
30 | cd ..
31 | done
32 | cd ..
33 | cd ..
34 | done
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/rtigen.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | set -e
3 |
4 | mkdir -p ./src/dds_idl_cpp
5 |
6 | # Generate IDL C++
7 | find src/dds_idl/ -type f | xargs -n 1 rtiddsgen -replace -legacyPlugin -language C++11 -d ./src/dds_idl_cpp/ -I ../../../../cpm_lib/dds_idl/
8 |
9 | # Copy headers to public inclues
10 | mkdir -p include/dds
11 | (cd src/dds_idl_cpp;find -type f) | grep \\.h | xargs -n 1 -I ARG cp src/dds_idl_cpp/ARG include/dds/
12 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 | # only one vehicle id is to be specified as the hlc has to be run distributedly
8 | ./build/dynamic_priorities --vehicle_ids=1 --hlc_mode=2
9 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/run_simulation.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 | # heptrack can be used to analyse memory usage and detect leaks
8 | # heaptrack ./build/simulation --n=5 --hlc_mode=2 --steps=100
9 |
10 | ./build/simulation --n=15 --hlc_mode=2
11 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/src/dds_idl/FallbackSync.idl:
--------------------------------------------------------------------------------
1 | #include "Header.idl"
2 |
3 | #ifndef FALLBACKSYNC_IDL
4 | #define FALLBACKSYNC_IDL
5 |
6 | /**
7 | *
8 | * \ingroup cpmlib_idl
9 | */
10 | struct FallbackSync {
11 | Header header;
12 |
13 | octet vehicle_id; //@key
14 | octet syncid;
15 | boolean feasible;
16 | };
17 | #endif
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/src/dds_idl/FutureCollisionAssessment.idl:
--------------------------------------------------------------------------------
1 | #include "Header.idl"
2 |
3 | #ifndef FUTURECOLLISIONASSESSMENT_IDL
4 | #define FUTURECOLLISIONASSESSMENT_IDL
5 |
6 | /**
7 | *
8 | * \ingroup cpmlib_idl
9 | */
10 | struct FutureCollisionAssessment {
11 | Header header;
12 |
13 | octet vehicle_id; //@key
14 | int16 fca;
15 | };
16 | #endif
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/src/dds_idl/Trajectory.idl:
--------------------------------------------------------------------------------
1 | #include "Header.idl"
2 |
3 | #ifndef TRAJECTORY_IDL
4 | #define TRAJECTORY_IDL
5 |
6 | /**
7 | * \struct LaneGraphPosition
8 | * \brief TODO
9 | * \ingroup cpmlib_idl
10 | */
11 | struct LaneGraphPosition
12 | {
13 | //! TODO
14 | TimeStamp estimated_arrival_time;
15 | //! TODO
16 | unsigned short edge_index;
17 | //! TODO
18 | unsigned short edge_path_index;
19 | };
20 |
21 |
22 | /**
23 | * \enum MessageType
24 | * \brief TODO
25 | * \ingroup cpmlib_idl
26 | */
27 | enum MessageType
28 | {
29 | Iterative,
30 | Final,
31 | Optimal
32 | };
33 |
34 | /**
35 | * \struct HlcCommunication
36 | * \brief TODO
37 | * \ingroup cpmlib_idl
38 | */
39 | struct Trajectory
40 | {
41 | //! TODO
42 | octet vehicle_id; //@key
43 | //! TODO
44 | Header header;
45 | //! TODO
46 | MessageType type;
47 | //! TODO
48 | boolean has_collisions;
49 | //! TODO
50 | sequence lane_graph_positions;
51 | };
52 | #endif
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/src/dds_idl/VehiclePath.idl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/high_level_controller/examples/cpp/dynamic_priorities/src/dds_idl/VehiclePath.idl
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/src/get_edge_indices.py:
--------------------------------------------------------------------------------
1 | # full lane graph edges:
2 | full_graph_start = [0,1,2,3,4,5,11,10,9,20,8,7,12,11,13,13,6,5,17,18,19,15,16,14]
3 | full_graph_end = [1,2,3,4,5,6,0,11,10,9,9,8,13,12,14,7,7,15,18,19,20,16,17,15]
4 |
5 | # example edge subset
6 | oval_vert_beg = [0,1,2,3,4,5,6,7,8,9, 10,11]
7 | oval_vert_end = [1,2,3,4,5,6,7,8,9,10,11, 0]
8 |
9 | oval_hor_beg = [11,12,13,14,15,16,17,18,19,20,9, 10]
10 | oval_hor_end = [12,13,14,15,16,17,18,19,20, 9,10,11]
11 |
12 | indices_vert = []
13 | indices_hor = []
14 | # get indices of example subset edges
15 | index = 0
16 | for edge in full_graph_start:
17 | print(index, " : ", edge, " -> ", full_graph_end[index])
18 | index_subset_1 = 0
19 | index_subset_2 = 0
20 | for edge_subset in oval_vert_beg:
21 | if edge == edge_subset and full_graph_end[index] == oval_vert_end[index_subset_1]:
22 | indices_vert.append(index)
23 | index_subset_1 +=1
24 | for edge_subseth in oval_hor_beg:
25 | if edge == edge_subseth and full_graph_end[index] == oval_hor_end[index_subset_2]:
26 | indices_hor.append(index)
27 | index_subset_2 +=1
28 | index+=1
29 |
30 |
31 | print(indices_vert)
32 | print(indices_hor)
33 | print(len(indices_vert))
34 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/src/ovals_example.hpp:
--------------------------------------------------------------------------------
1 | #include
2 | const std::vector hor_oval_edges_index = std::vector{0, 1, 2, 3, 4, 5, 6, 7, 8, 10, 11, 16 };
3 | const std::vector vert_oval_edges_index = std::vector{7, 8, 9, 12, 13, 14, 18, 19, 20, 21, 22, 23 };
4 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/src/simulation.hpp:
--------------------------------------------------------------------------------
1 | #include "VehicleTrajectoryPlanner.hpp"
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/visualization/.gitignore:
--------------------------------------------------------------------------------
1 | lane_graph.mat
2 | lane_graph.cpp
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/visualization/car_position.m:
--------------------------------------------------------------------------------
1 | function [x,y,yaw] = car_position(DataTraj, lane_graph, m_i, t_i)
2 | edge_index = DataTraj(m_i).lane_graph_positions(t_i).edge_index+1;
3 | edge_path_index = DataTraj(m_i).lane_graph_positions(t_i).edge_path_index+1;
4 |
5 |
6 | x = lane_graph.edges(edge_index).path(edge_path_index,1);
7 | y = lane_graph.edges(edge_index).path(edge_path_index,2);
8 |
9 | yaw = atan(lane_graph.edges(edge_index).path(edge_path_index,4)...
10 | / lane_graph.edges(edge_index).path(edge_path_index,3));
11 | end
12 |
13 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/visualization/pgf_to_pdf.tex:
--------------------------------------------------------------------------------
1 | \documentclass[tikz]{standalone}
2 | %\usetikzlibrary{...}% tikz package already loaded by 'tikz' option
3 | \usepackage{siunitx}
4 | \begin{document}
5 | \input{consecutive_boxplot.pgf}
6 | \end{document}
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/visualization/search_fca_index.m:
--------------------------------------------------------------------------------
1 | % searches fca message (starting at a given index)
2 | function f_i = search_fca_index(DataFCA, time, vehicle_id, valid_stamp)
3 | f_i = 1;
4 | fca = DataFCA(f_i);
5 | while(~((int64(fca.header.valid_after_stamp.nanoseconds) < valid_stamp+10000 && ...
6 | int64(fca.header.valid_after_stamp.nanoseconds) > valid_stamp-10000 ) ...
7 | & (fca.vehicle_id == vehicle_id)) & (f_i <= numel(DataFCA)))
8 | f_i = f_i + 1;
9 | if(f_i <= numel(DataFCA))
10 | fca = DataFCA(f_i);
11 | else
12 | disp(['Error: Message not found! ' ...
13 | 'v_id: ' int2str(vehicle_id) ...
14 | ' valid_stamp: ' int2str(valid_stamp) ...
15 | ' time: ' int2str(time)]);
16 | end
17 | end
18 | disp(['v_id:' int2str(vehicle_id) ...
19 | ' FCA: ' int2str(fca.fca) ...
20 | ' f_i:' int2str(f_i) ...
21 | ' fca stamp: ' int2str(fca.header.valid_after_stamp.nanoseconds) ...
22 | ' time: ' int2str(time) ...
23 | ' valid_stamp: ' int2str(valid_stamp)])
24 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/visualization/search_trajectory_message_index.m:
--------------------------------------------------------------------------------
1 | % searches trajectory message
2 | function m_i = search_trajectory_message_index(DataTraj, time, type, vehicle_id) % type='Optimal' or type='Final'
3 | m_i = 1; % message index
4 | trajectory = DataTraj(m_i);
5 | while(~(trajectory.header.create_stamp.nanoseconds == time & strcmp(trajectory.type, type) & trajectory.vehicle_id == vehicle_id) & m_i <= numel(DataTraj))
6 | m_i = m_i + 1;
7 | if(m_i <= numel(DataTraj))
8 | trajectory = DataTraj(m_i);
9 | else
10 | disp('Error: Message not found!')
11 | end
12 | end
13 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/visualization/transformedRectangle.m:
--------------------------------------------------------------------------------
1 | function [ polygon_XY ] = transformedRectangle( x,y,angle, Length, Width )
2 | % TRANSFORMED_RECTANGLE Creates a rectangle with the given position and orientation.
3 |
4 | unitSquare = [ 0 0 0 1; 1 0 0 1; 1 1 0 1; 0 1 0 1]';
5 |
6 | % Read this bottom-up
7 | polygon_XY = makehgtform('translate',[x y 0]) ...
8 | * makehgtform('zrotate',angle) ...
9 | * makehgtform('scale',[Length Width 1]) ...
10 | * makehgtform('translate',[-.5 -.5 0]) ...
11 | * unitSquare;
12 |
13 | polygon_XY = polygon_XY(1:2,:);
14 | end
15 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/visualization/vehColor.m:
--------------------------------------------------------------------------------
1 | function color = vehColor(index)
2 | % VEHCOLOR Return vehicle color specifier for vehicle with given index.
3 |
4 | vehColors = [0.8941 0.1020 0.1098;...
5 | 0.2157 0.4941 0.7216;...
6 | 0.3020 0.6863 0.2902;...
7 | 0.5961 0.3059 0.6392;...
8 | 1.0000 0.4980 0 ;...
9 | 1.0000 1.0000 0.2000;...
10 | 0.6510 0.3373 0.1569;...
11 | 0.9686 0.5059 0.7490];
12 | color = vehColors(mod(index-1,size(vehColors,1))+1,:);
13 | end
14 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/dynamic_priorities/visualization/visualise_graph.py:
--------------------------------------------------------------------------------
1 | #import networkx as nx
2 | import igraph
3 | from igraph import *
4 | import matplotlib.pyplot as plt
5 |
6 | #G = nx.DiGraph()
7 |
8 |
9 | #G.add_edges_from([(20, 9) ,(9, 8) ,(8, 7) ,(7, 13) ,(7, 6) ,(13, 12) ,(12, 15) ,(12, 14) ,(15, 11) ,(11, 10) ,(10, 8) ,(6, 0) ,(0, 1) ,(1, 2) ,(2, 3) ,(3, 4) ,(4, 5) ,(4, 17) ,(5, 16) ,(16, 11) ,(14, 23) ,(23, 21) ,(21, 22) ,(22, 18) ,(18, 19) ,(19, 20) ,(17, 21)])
10 | #labels = {20:1,9:2,8:3,7:4,13:5,12:6,15:7,11:8,10:9,6:5,0:6,1:7,2:8,3:9,4:10,5:11,16:12,14:7,23:8,21:9,22:10,18:11,19:12,17:11}
11 |
12 | #pos = nx.spring_layout(G, seed=2)
13 | #nx.draw(G, pos)
14 | #nx.draw_networkx_labels(G, pos, labels)
15 |
16 | #nx.draw_networkx_labels(G, nx.spring_layout(G), labels)
17 |
18 |
19 | g = Graph([(20, 9) ,(9, 8) ,(8, 7) ,(7, 13) ,(7, 6) ,(13, 12) ,(12, 15) ,(12, 14) ,(15, 11) ,(11, 10) ,(10, 8) ,(6, 0) ,(0, 1) ,(1, 2) ,(2, 3) ,(3, 4) ,(4, 5) ,(4, 17) ,(5, 16) ,(16, 11) ,(14, 23) ,(23, 21) ,(21, 22) ,(22, 18) ,(18, 19) ,(19, 20) ,(17, 21)], directed=True)
20 | tmplabel = {20:1,9:2,8:3,7:4,13:5,12:6,15:7,11:8,10:9,6:5,0:6,1:7,2:8,3:9,4:10,5:11,16:12,14:7,23:8,21:9,22:10,18:11,19:12,17:11}
21 | labels = []
22 | for i in range(1,len(tmplabel)):
23 | print(i)
24 | labels += [tmplabel[i]]
25 | g.vs["label"] = labels
26 | print(g.vs["label"])
27 | layout = g.layout("kk")
28 | fig, ax = plt.subplots()
29 | plot(g, layout=layout)
30 | #plt.show()
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/eight_zero/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.1)
2 | add_definitions(-Wall -Wextra -Werror=return-type)
3 | set (CMAKE_CXX_STANDARD 11)
4 |
5 | # RTI DDS
6 | add_definitions(-DRTI_UNIX -DRTI_LINUX -DRTI_64BIT -DRTI_STATIC)
7 | include_directories(SYSTEM $ENV{NDDSHOME}/include)
8 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds)
9 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds/hpp)
10 | link_libraries(nddscpp2z nddscz nddscorez)
11 | link_directories($ENV{NDDSHOME}/lib/x64Linux4gcc7.3.0)
12 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -m64 -Wl,--no-as-needed")
13 |
14 |
15 | # CPM lib
16 | include_directories(SYSTEM ../../../../cpm_lib/include/)
17 | include_directories(SYSTEM ../../../../cpm_lib/include/cpm/dds/)
18 | link_directories(../../../../cpm_lib/build/)
19 |
20 |
21 |
22 | include_directories(src)
23 | link_libraries(dl nsl m pthread rt)
24 |
25 | add_executable(eight_zero
26 | src/main.cpp
27 | src/Eight.cpp
28 | src/Eight.hpp
29 | )
30 | target_link_libraries(eight_zero cpm)
31 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/eight_zero/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | mkdir -p build
5 | cd build
6 | cmake ..
7 | make -j$(nproc)
8 | cd ..
9 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/eight_zero/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 | if [ $# -eq 0 ]
8 | then
9 | echo "Missing argument vehicle ID"
10 | else
11 | ./build/eight_zero --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --vehicle_ids=$1
12 | fi
13 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/two_vehicles_drive/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.1)
2 | add_definitions(-Wall -Wextra -Werror=return-type)
3 | set (CMAKE_CXX_STANDARD 11)
4 |
5 | # RTI DDS
6 | add_definitions(-DRTI_UNIX -DRTI_LINUX -DRTI_64BIT -DRTI_STATIC)
7 | include_directories(SYSTEM $ENV{NDDSHOME}/include)
8 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds)
9 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds/hpp)
10 | link_libraries(nddscpp2z nddscz nddscorez)
11 | link_directories($ENV{NDDSHOME}/lib/x64Linux4gcc7.3.0)
12 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -m64 -Wl,--no-as-needed")
13 |
14 |
15 | # CPM lib
16 | include_directories(SYSTEM ../../../../cpm_lib/include/)
17 | include_directories(SYSTEM ../../../../cpm_lib/include/cpm/dds/)
18 | link_directories(../../../../cpm_lib/build/)
19 |
20 |
21 |
22 | include_directories(src)
23 | link_libraries(dl nsl m pthread rt)
24 |
25 | add_executable(two_vehicles_drive
26 | src/main.cpp
27 | )
28 | target_link_libraries(two_vehicles_drive cpm)
29 |
30 |
31 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/two_vehicles_drive/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | mkdir build
5 | cd build
6 | cmake ..
7 | make -j$(nproc)
8 | cd ..
9 |
--------------------------------------------------------------------------------
/high_level_controller/examples/cpp/two_vehicles_drive/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 | if [ $# -eq 0 ]
8 | then
9 | echo "Missing argument vehicle ID"
10 | else
11 | ./build/two_vehicles_drive --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --vehicle_id=$1
12 | fi
13 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/ControllerFiles/MPC_init.m:
--------------------------------------------------------------------------------
1 |
2 | % Preprocessing step for MPC controller.
3 |
4 | % x_measured is column vector of current measurement. u_prev is a column vector of last input
5 | function [iter] = MPC_init(scenario, x_measured, u_prev)
6 |
7 | iter = struct;
8 | nu=scenario.model.nu;
9 |
10 | % we start from previous last solution
11 | if size ( u_prev,2) ~= nu
12 | u_prev = u_prev';
13 | end
14 | iter.u0= u_prev;
15 |
16 |
17 | iter.x0 = x_measured;
18 |
19 | % generate reference trajectory for process variables.
20 | iter.RefData = sampleReferenceTrajectory(...
21 | scenario.Hp, ... % number of prediction steps
22 | scenario.path_pos,...
23 | scenario.path_speed,...
24 | scenario.currPos,...
25 | scenario.dt); % distance traveled in one timestep
26 |
27 |
28 |
29 | end
30 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/ControllerFiles/QP_controller.m:
--------------------------------------------------------------------------------
1 |
2 |
3 | function [U,trajectoryPrediction,controllerOutput] = QP_controller( scenario,iter,prevOutput )
4 | controllerOutput = struct;
5 |
6 | mpc = generate_mpc_matrices( scenario,iter );
7 | qp = convert_to_QP( scenario,iter,mpc );
8 |
9 | % take previous output as initialization
10 | du_prev = prevOutput.du;
11 | n_du = length(du_prev);
12 | du = reshape(du_prev,[n_du,1]);
13 | du(1:end-1,:)=du(2:end,:);
14 | du(end,:) = 0;
15 |
16 |
17 | controllerOutput.resultInvalid = false;
18 |
19 |
20 | disp('entering optimizer');
21 | % try last MPC sol.
22 | [du , feasible , ~, controllerOutput.optimization_log] = QP_optimizer( scenario,iter,qp, mpc, du );
23 | disp('finished optimization');
24 |
25 | if ~feasible
26 | disp(['\n\n\n']);
27 | end
28 |
29 | controllerOutput.du=du;
30 |
31 | controllerOutput.feasible=feasible;
32 |
33 | [trajectoryPrediction,U] = decode_deltaU(scenario,iter,mpc,du);
34 |
35 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/ControllerFiles/QP_evaluate.m:
--------------------------------------------------------------------------------
1 |
2 | % Check QP constraints for feasiblity
3 | function [ ...
4 | feasible,...
5 | objValue, ...
6 | max_violation, ...
7 | sum_violations ] = QP_evaluate( scenario, qp, deltaU )
8 |
9 | cfg = config;
10 | sum_violations = 0;
11 | max_violation = 0;
12 |
13 | feasible = true;
14 | % Hp = scenario.Hp;
15 | % H_coll =scenario.H_coll;
16 | nObst = scenario.nObst;
17 |
18 |
19 |
20 |
21 | objValue = deltaU'*qp.p0*deltaU + qp.q0'*deltaU + qp.r0;
22 |
23 |
24 | % VEHICLES
25 | if nObst > 0
26 |
27 | ci = (qp.A_coll * deltaU-qp.b_coll);
28 |
29 |
30 |
31 | if (max(ci) > cfg.QCQP.constraintTolerance)
32 | sum_violations = sum((ci>0).*ci);
33 | max_violation = max(ci);
34 | feasible = false;
35 | end
36 |
37 | end
38 |
39 |
40 |
41 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/ControllerFiles/blk.m:
--------------------------------------------------------------------------------
1 | % Copyright 2016 Bassam Alrifaee
2 | %
3 | % This program is free software: you can redistribute it and/or modify
4 | % it under the terms of the GNU General Public License version 3 as
5 | % published by the Free Software Foundation.
6 | %
7 | % This program is distributed in the hope that it will be useful,
8 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
9 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 | % GNU General Public License for more details.
11 | %
12 | % You should have received a copy of the GNU General Public License
13 | % along with this program. If not, see .
14 |
15 |
16 | function [ slice ] = blk( i,n )
17 | % Convenience function for block matrix indexing.
18 | % i: timestep
19 | % n: size of vector for each time step
20 |
21 | slice = (n*(i-1)+1) : n*i;
22 | end
23 |
24 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/ControllerFiles/config.m:
--------------------------------------------------------------------------------
1 | % Copyright 2016 Bassam Alrifaee
2 | %
3 | % This program is free software: you can redistribute it and/or modify
4 | % it under the terms of the GNU General Public License version 3 as
5 | % published by the Free Software Foundation.
6 | %
7 | % This program is distributed in the hope that it will be useful,
8 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
9 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 | % GNU General Public License for more details.
11 | %
12 | % You should have received a copy of the GNU General Public License
13 | % along with this program. If not, see .
14 |
15 | % Central configuration file. Holds solver, controller, etc. settings. Anything that's not scenario-specific.
16 | function [ cfg ] = config
17 |
18 | cfg = struct;
19 |
20 | cfg.MIP_CPLEX = struct;
21 | cfg.MIP_CPLEX.bigM = 1000;
22 | cfg.MIP_CPLEX.R_Gain = 0.1;
23 | cfg.MIP_CPLEX.polygonalNormApproximationDegree = 6;
24 | cfg.MIP_CPLEX.timelimit = 300; % in seconds
25 | cfg.MIP_CPLEX.obstAsQCQP = 1;
26 |
27 | cfg.Lagrange_Mosek = struct;
28 | cfg.Lagrange_Mosek.R_Gain = 10;
29 |
30 | cfg.QCQP.default_dsafeExtra = 0;
31 |
32 | % conversion between distance tolerance and
33 | % constraint tolerance: cons_tol = 2 * d_safe * d_tol
34 | cfg.QCQP.constraintTolerance = 2 * 2.1 * 1e-3; % ~1mm is sufficient
35 | end
36 |
37 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/ControllerFiles/decode_deltaU.m:
--------------------------------------------------------------------------------
1 | % function to transform du into process variables Y
2 | function [Traj,U] = decode_deltaU(scenario,iter,mpc,du_result)
3 |
4 | Hp = scenario.Hp;
5 | Hu = scenario.Hu;
6 | nu = scenario.model.nu;
7 | ny = scenario.model.ny;
8 | U = zeros(nu,Hp);
9 | du = reshape(du_result,[nu,Hu]);
10 |
11 | % Control values
12 |
13 | U(:,1) = du(:,1) + iter.u0;
14 | for k=2:Hu
15 | U(:,k) = du(:,k) + U(:,k-1);
16 | end
17 | for k=Hu+1:Hp
18 | U(:,k) = U(:,Hu);
19 | end
20 |
21 |
22 | % Predicted trajectory
23 |
24 | Y = mpc.freeResponse + mpc.Theta*du_result;
25 | Y=reshape(Y,[scenario.model.ny,Hp]);
26 | Traj = Y(1:2,:); % 1:2 corresponds to pos and vel value
27 |
28 |
29 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/ControllerFiles/generate_mpc_matrices.m:
--------------------------------------------------------------------------------
1 |
2 | function mpc = generate_mpc_matrices( scenario,iter )
3 |
4 | % Compute all relevant discretization and MPC matrices for all
5 | % vehicles.
6 |
7 | nx = scenario.model.nx;
8 | nu = scenario.model.nu;
9 | ny = scenario.model.ny;
10 | Hp = scenario.Hp;
11 | Hu = scenario.Hu;
12 | dt = scenario.dt;
13 |
14 | mpc=struct;
15 |
16 | %% GENARATE MATRICES
17 | for i=1:Hp
18 | mpc.Reference( blk(i,ny),1 ) = iter.RefData.ReferenceTrajectoryPoints(:,i);
19 | end
20 |
21 | mpc.A = zeros(nx,nx,Hp);
22 | mpc.B = zeros(nx,nu,Hp);
23 | mpc.E = zeros(nx,Hp);
24 |
25 | [ A,B,C,E ] ...
26 | = discretize( iter.x0(:), iter.u0(:), dt, scenario.model );
27 | [ Psi, Gamma, mpc.Theta, Pie ] ...
28 | = prediction_matrices( A,B,C,nx,nu,ny,Hp,Hu );
29 |
30 | mpc.freeResponse = Psi *iter.x0(:) + Gamma *iter.u0(:) + Pie*E;
31 |
32 | [ mpc.H , mpc.g, mpc.r ] ...
33 | = mpc_cost_function_matrices( scenario.Q, scenario.R, nu,ny,Hp,Hu,mpc,scenario.Q_final );
34 |
35 | % For the simple linearization, the same A,B,E are used in
36 | % every prediction step.
37 | for i=1:Hp
38 | mpc.A(:,:,i) = A;
39 | mpc.B(:,:,i) = B;
40 | mpc.E(:,i) = E;
41 | end
42 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/ControllerFiles/local2global.m:
--------------------------------------------------------------------------------
1 | % function which computes the points in global coordinates from local
2 | % coordinate points
3 | function globalPoints=local2global(progress,ref_points,currPos)
4 | polyline = [currPos, ref_points];
5 | delta_s = diff(polyline,1,2);
6 | delta_s_distance = [0 , sqrt(delta_s(1,:).^2 + delta_s(2,:).^2)];
7 |
8 | local_coord = cumsum(delta_s_distance);
9 |
10 | if progress(end) > local_coord(end)
11 | progress
12 | local_coord
13 | disp('HLC planned beyond lanelet horizon, warning');
14 | progress =min(progress,local_coord(end));
15 | end
16 |
17 | global_x = interp1(local_coord,polyline(1,:),progress);
18 | global_y = interp1(local_coord,polyline(2,:),progress);
19 |
20 | globalPoints=[global_x; global_y];
21 |
22 |
23 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/ControllerFiles/mpc_cost_function_matrices.m:
--------------------------------------------------------------------------------
1 | % Copyright 2016 Bassam Alrifaee
2 | %
3 | % This program is free software: you can redistribute it and/or modify
4 | % it under the terms of the GNU General Public License version 3 as
5 | % published by the Free Software Foundation.
6 | %
7 | % This program is distributed in the hope that it will be useful,
8 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
9 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 | % GNU General Public License for more details.
11 | %
12 | % You should have received a copy of the GNU General Public License
13 | % along with this program. If not, see .
14 |
15 | function [ H, g, r ] = mpc_cost_function_matrices( Q_weight, R_weight, nu,ny,Hp,Hu,mpc,Q_final )
16 |
17 | % Compute G and H matrices in the quadratic cost function.
18 | Q = diag(repmat(Q_weight,1,Hp));
19 | Q(end-ny+1:end, end-ny+1:end) = diag(Q_final);
20 |
21 | R = diag(repmat(R_weight,1,Hu));
22 | Error = mpc.Reference - mpc.freeResponse;
23 | % make symmetric if Q or R werent symmetric
24 | H = symmetric(mpc.Theta'*Q*mpc.Theta + R);
25 | g = -2*mpc.Theta'*Q*Error;
26 |
27 | r = Error' * Q * Error;
28 | end
29 |
30 | function A = symmetric( A )
31 | A=.5*(A+A');
32 | end
33 |
34 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/README.txt:
--------------------------------------------------------------------------------
1 | This folder contains the Central Routing Example with CommonRoad data import.
2 | - Call main_vehicle_ids.m from the labcontrolcenter for the simulation
3 | - The folder Map contains the CPM Lab Map as CommonRoad format without a planning problem
4 | The file is read once and then cached in a .mat-file with the same name. if the XML file is updated, the .mat has to be deleted in order to read the XML file with the updates. This reading process of the XML file takes ~30sec (which is why it is cached).
5 | - The folder RTree is needed to build the RTree. The original version of Guttman is used, so there is room for improvements as in R* and R+ Trees for future work.
6 |
7 | - The folder ControllerFiles contains all files for the MPC problem. Priority_QP.m is the function to call
8 |
9 | - PathPlanner is the path planning class, which dynamically extends the path by choosing random successors
10 | - configVehicle.m is used for the configuration of the HLC/MPC
11 |
12 | - vehicle.m is a class which is instanced for every vehicle. This class can be seen as the local HLC of each vehicle.
13 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/RTree/Point2D_RTree.m:
--------------------------------------------------------------------------------
1 | % data type for a 2D-point
2 | classdef Point2D_RTree
3 | properties (Access = public)
4 | x = double(0);
5 | y = double(0);
6 | end
7 | methods
8 |
9 | function obj = Point2D_RTree(x0,y0)
10 |
11 | if nargin == 2
12 | obj.x=x0;
13 | obj.y=y0;
14 | else
15 | % empty
16 | end
17 | end
18 |
19 | function delete(obj)
20 | end
21 | function out=get_x(obj)
22 | out = obj.x;
23 | end
24 | function obj=set_x(obj,value)
25 | obj.x = value;
26 | end
27 | function out=get_y(obj)
28 | out = obj.y;
29 | end
30 | function obj=set_y(obj,value)
31 | obj.y = value;
32 | end
33 | end
34 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/RTree/RBTElement.m:
--------------------------------------------------------------------------------
1 | classdef RBTElement < handle
2 | %
3 | % A class that implements pass by reference elements with numeric keys for
4 | % instances of the RedBlackTree class
5 | %
6 | % NOTE: This class is used internally by the RedBlackTree class
7 | %
8 | %
9 | % Public properties
10 | %
11 | properties (Access = public)
12 | key; % key
13 | left = nan % left child
14 | right = nan; % right child
15 | p = nan; % parent
16 | color = false; % color (true = red,false = black)
17 | size = 0; % size of subtree rooted at this element
18 | value = []; % miscellaneous data
19 | end
20 |
21 | %
22 | % Public methods
23 | %
24 | methods (Access = public)
25 | %
26 | % Constructor
27 | %
28 | function this = RBTElement(key,value)
29 | % Initialize key
30 | this.key = key;
31 |
32 | % Set value data, if specified
33 | if (nargin == 2)
34 | this.value = value;
35 | end
36 | end
37 |
38 | %
39 | % Element is nan if its key is nan
40 | %
41 | function bool = isnan(this)
42 | bool = isnan(this.key);
43 | end
44 | end
45 | end
46 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/RTree/drawCenterline.m:
--------------------------------------------------------------------------------
1 |
2 | % function to visualize the centerlines of the lanelet maps
3 | function drawCenterline(r_tree,map)
4 |
5 | hold on
6 | idList = r_tree.get_idList();
7 | Iterator = idList;
8 | while Iterator.hasSucc()
9 | Iterator = Iterator.get_Succ();
10 | keyId = Iterator.get_Content();
11 | lanelet = map.Search(keyId).value;
12 | lanelet.drawCenterline();
13 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/RTree/drawLaneletMap.m:
--------------------------------------------------------------------------------
1 | % function to visualize left and right bound of each lanelet in the map
2 | function fig =drawLaneletMap(r_tree,map)
3 | fig = figure;
4 | hold on
5 | idList = r_tree.get_idList;
6 | Iterator = idList;
7 | while Iterator.hasSucc()
8 | Iterator = Iterator.get_Succ();
9 | keyId = Iterator.get_Content();
10 | lanelet = map.Search(keyId).value;
11 | lanelet.drawLanelet();
12 | 1;
13 | end
14 |
15 |
16 | hold off
17 | end
18 |
19 |
20 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_central_routing/configVehicle.m:
--------------------------------------------------------------------------------
1 | function cfg = configVehicle
2 |
3 | cfg.Hu = 6; % Stellhorizont
4 | cfg.Hp = 8; % (oberer) Praediktionshorizont
5 | cfg.dt = 0.25; % Abtastzeit in Sekunden
6 | cfg.dsafeVehicles = 0.15; % Sicherheitsabstand
7 | cfg.minSpeed = 0.3; % minimale Geschwindigkeit
8 | cfg.speed_target = 1; % Reisegeschwindigkeit
9 | end
10 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/CommonRoadImport/PlanningProblem.m:
--------------------------------------------------------------------------------
1 | classdef PlanningProblem < handle
2 |
3 | properties
4 | id;
5 | initialState;
6 | numGoalState = int8(0);
7 | goalState;
8 |
9 |
10 | end
11 | methods
12 |
13 | function obj = PlanningProblem(New_PlanningProblem)
14 | numAttributes = size(New_PlanningProblem.Attributes,1);
15 | for k=1:numAttributes
16 | if strcmp(New_PlanningProblem.Attributes.Name, 'id')
17 | obj.id = New_PlanningProblem.Attributes.Value;
18 | end
19 | end
20 | numChildren = size(New_PlanningProblem.Children,2);
21 | for k=1:numChildren
22 | switch New_PlanningProblem.Children(k).Name
23 |
24 | case 'initialState'
25 | obj.initialState = stateTostruct(New_PlanningProblem.Children(k));
26 |
27 | case 'goalState'
28 | obj.numGoalState = obj.numGoalState+1;
29 | obj.goalState{1,obj.numGoalState} = goalstateTostruct(New_PlanningProblem.Children(k));
30 |
31 | end
32 |
33 | end
34 | end
35 | end
36 |
37 |
38 |
39 |
40 |
41 |
42 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/ControllerFiles/MPC_init.m:
--------------------------------------------------------------------------------
1 |
2 | % Preprocessing step for MPC controller.
3 |
4 | % x_measured is column vector of current measurement. u_prev is a column vector of last input
5 | function [iter] = MPC_init(scenario, x_measured, u_prev)
6 |
7 | iter = struct;
8 |
9 |
10 | nu=scenario.model.nu;
11 |
12 | % we start from previous last solution
13 | if size ( u_prev,2) ~= nu
14 | u_prev = u_prev';
15 | end
16 | iter.u0= u_prev;
17 |
18 |
19 | iter.x0 = x_measured;
20 |
21 |
22 | %% generate reference trajectory for process variables.
23 | iter.RefData = sampleReferenceTrajectory(...
24 | scenario.Hp, ... % number of prediction steps
25 | scenario.path_pos,...
26 | scenario.path_speed,...
27 | scenario.currPos,...
28 | scenario.dt); % distance traveled in one timestep
29 |
30 |
31 |
32 | end
33 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/ControllerFiles/QP_evaluate.m:
--------------------------------------------------------------------------------
1 |
2 | % Check QP constraints for feasiblity
3 | function [ ...
4 | feasible,...
5 | objValue, ...
6 | max_violation ...
7 | ] = QP_evaluate( scenario, qp, deltaU )
8 |
9 | cfg = config;
10 | % sum_violations = 0;
11 | max_violation = 0;
12 |
13 | feasibleColl = true;
14 | feasibleGoal = true;
15 | % Hp = scenario.Hp;
16 | % H_coll =scenario.H_coll;
17 | nObst = scenario.nObst;
18 |
19 |
20 |
21 |
22 | objValue = deltaU'*qp.p0*deltaU + qp.q0'*deltaU + qp.r0;
23 |
24 |
25 | % VEHICLES
26 | if nObst > 0
27 |
28 | ci = (qp.A_coll * deltaU-qp.b_coll);
29 |
30 |
31 |
32 | if (max(ci) > cfg.QCQP.constraintTolerance)
33 | % sum_violations = sum((ci>0).*ci);
34 | max_violation = max(ci);
35 | feasibleColl = false;
36 | end
37 | end
38 | if ~isempty(qp.A_goal)
39 |
40 | ci = (qp.A_goal * deltaU-qp.b_goal);
41 |
42 |
43 |
44 | if (max(ci) > cfg.QCQP.constraintTolerance)
45 | % sum_violations = sum((ci>0).*ci);
46 | max_violation = max(ci);
47 | feasibleGoal = false;
48 | end
49 | end
50 | feasible.feasibleGoal = feasibleGoal;
51 | feasible.feasibleColl = feasibleColl;
52 |
53 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/ControllerFiles/blk.m:
--------------------------------------------------------------------------------
1 | % Copyright 2016 Bassam Alrifaee
2 | %
3 | % This program is free software: you can redistribute it and/or modify
4 | % it under the terms of the GNU General Public License version 3 as
5 | % published by the Free Software Foundation.
6 | %
7 | % This program is distributed in the hope that it will be useful,
8 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
9 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 | % GNU General Public License for more details.
11 | %
12 | % You should have received a copy of the GNU General Public License
13 | % along with this program. If not, see .
14 |
15 |
16 | function [ slice ] = blk( i,n )
17 | % Convenience function for block matrix indexing.
18 | % i: timestep
19 | % n: size of vector for each time step
20 |
21 | slice = (n*(i-1)+1) : n*i;
22 | end
23 |
24 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/ControllerFiles/config.m:
--------------------------------------------------------------------------------
1 | % Copyright 2016 Bassam Alrifaee
2 | %
3 | % This program is free software: you can redistribute it and/or modify
4 | % it under the terms of the GNU General Public License version 3 as
5 | % published by the Free Software Foundation.
6 | %
7 | % This program is distributed in the hope that it will be useful,
8 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
9 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 | % GNU General Public License for more details.
11 | %
12 | % You should have received a copy of the GNU General Public License
13 | % along with this program. If not, see .
14 |
15 | % Central configuration file. Holds solver, controller, etc. settings. Anything that's not scenario-specific.
16 | function [ cfg ] = config
17 |
18 | cfg = struct;
19 |
20 | cfg.MIP_CPLEX = struct;
21 | cfg.MIP_CPLEX.bigM = 1000;
22 | cfg.MIP_CPLEX.R_Gain = 0.1;
23 | cfg.MIP_CPLEX.polygonalNormApproximationDegree = 6;
24 | cfg.MIP_CPLEX.timelimit = 300; % in seconds
25 | cfg.MIP_CPLEX.obstAsQCQP = 1;
26 |
27 | cfg.Lagrange_Mosek = struct;
28 | cfg.Lagrange_Mosek.R_Gain = 10;
29 |
30 | cfg.QCQP.default_dsafeExtra = 0;
31 |
32 | % conversion between distance tolerance and
33 | % constraint tolerance: cons_tol = 2 * d_safe * d_tol
34 | cfg.QCQP.constraintTolerance = 2 * 2.1 * 1e-3; % ~1mm is sufficient
35 | end
36 |
37 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/ControllerFiles/decode_deltaU.m:
--------------------------------------------------------------------------------
1 | % function to transform du into process variables Y
2 | function [Traj,U] = decode_deltaU(scenario,iter,mpc,du_result)
3 |
4 | Hp = scenario.Hp;
5 | Hu = scenario.Hu;
6 | nu = scenario.model.nu;
7 | ny = scenario.model.ny;
8 | U = zeros(nu,Hp);
9 | du = reshape(du_result,[nu,Hu]);
10 |
11 | % Control values
12 |
13 | U(:,1) = du(:,1) + iter.u0;
14 | for k=2:Hu
15 | U(:,k) = du(:,k) + U(:,k-1);
16 | end
17 | for k=Hu+1:Hp
18 | U(:,k) = U(:,Hu);
19 | end
20 |
21 |
22 | % Predicted trajectory
23 |
24 | Y = mpc.freeResponse + mpc.Theta*du_result;
25 | Y=reshape(Y,[scenario.model.ny,Hp]);
26 | Traj = Y(1:2,:); % 1:2 corresponds to pos and vel value
27 |
28 |
29 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/ControllerFiles/generate_mpc_matrices.m:
--------------------------------------------------------------------------------
1 |
2 | function mpc = generate_mpc_matrices( scenario,iter )
3 |
4 | % Compute all relevant discretization and MPC matrices for all
5 | % vehicles.
6 |
7 | nx = scenario.model.nx;
8 | nu = scenario.model.nu;
9 | ny = scenario.model.ny;
10 | Hp = scenario.Hp;
11 | Hu = scenario.Hu;
12 | dt = scenario.dt;
13 |
14 | mpc=struct;
15 |
16 | %% GENARATE MATRICES
17 | for i=1:Hp
18 | mpc.Reference( blk(i,ny),1 ) = iter.RefData.ReferenceTrajectoryPoints(:,i);
19 | end
20 |
21 | mpc.A = zeros(nx,nx,Hp);
22 | mpc.B = zeros(nx,nu,Hp);
23 | mpc.E = zeros(nx,Hp);
24 |
25 | [ A,B,C,E ] ...
26 | = discretize( iter.x0(:), iter.u0(:), dt, scenario.model );
27 | [ Psi, Gamma, mpc.Theta, Pie ] ...
28 | = prediction_matrices( A,B,C,nx,nu,ny,Hp,Hu );
29 |
30 | mpc.freeResponse = Psi *iter.x0(:) + Gamma *iter.u0(:) + Pie*E;
31 |
32 | [ mpc.H , mpc.g, mpc.r ] ...
33 | = mpc_cost_function_matrices( scenario.Q, scenario.R, nu,ny,Hp,Hu,mpc,scenario.Q_final );
34 |
35 | % For the simple linearization, the same A,B,E are used in
36 | % every prediction step.
37 | for i=1:Hp
38 | mpc.A(:,:,i) = A;
39 | mpc.B(:,:,i) = B;
40 | mpc.E(:,i) = E;
41 | end
42 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/ControllerFiles/local2global.m:
--------------------------------------------------------------------------------
1 | % function which computes the points in global coordinates from local
2 | % coordinate points
3 | function globalPoints=local2global(progress,ref_points,currPos)
4 | polyline = [currPos, ref_points];
5 | delta_s = diff(polyline,1,2);
6 | delta_s_distance = [0 , sqrt(delta_s(1,:).^2 + delta_s(2,:).^2)];
7 |
8 | local_coord = cumsum(delta_s_distance);
9 |
10 | if progress(end) > local_coord(end)
11 | progress
12 | local_coord
13 | disp('HLC planned beyond lanelet horizon, warning');
14 | progress =min(progress,local_coord(end));
15 | end
16 |
17 | global_x = interp1(local_coord,polyline(1,:),progress);
18 | global_y = interp1(local_coord,polyline(2,:),progress);
19 |
20 | globalPoints=[global_x; global_y];
21 |
22 |
23 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/ControllerFiles/mpc_cost_function_matrices.m:
--------------------------------------------------------------------------------
1 | % Copyright 2016 Bassam Alrifaee
2 | %
3 | % This program is free software: you can redistribute it and/or modify
4 | % it under the terms of the GNU General Public License version 3 as
5 | % published by the Free Software Foundation.
6 | %
7 | % This program is distributed in the hope that it will be useful,
8 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
9 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 | % GNU General Public License for more details.
11 | %
12 | % You should have received a copy of the GNU General Public License
13 | % along with this program. If not, see .
14 |
15 | function [ H, g, r ] = mpc_cost_function_matrices( Q_weight, R_weight, nu,ny,Hp,Hu,mpc,Q_final )
16 |
17 | % Compute G and H matrices in the quadratic cost function.
18 | Q = diag(repmat(Q_weight,1,Hp));
19 | Q(end-ny+1:end, end-ny+1:end) = diag(Q_final);
20 |
21 | R = diag(repmat(R_weight,1,Hu));
22 | Error = mpc.Reference - mpc.freeResponse;
23 | % make symmetric if Q or R werent symmetric
24 | H = symmetric(mpc.Theta'*Q*mpc.Theta + R);
25 | g = -2*mpc.Theta'*Q*Error;
26 |
27 | r = Error' * Q * Error;
28 | end
29 | function A = symmetric( A )
30 | A=.5*(A+A');
31 | end
32 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/README.txt:
--------------------------------------------------------------------------------
1 | This folder contains the implementation to solve CommonRoad planning problems.
2 | - Call main_vehicle_ids.m from the labcontrolcenter for the simulation
3 | - The folder Map contains the CommonRoad files with the planning problems. The selected file has to be also written into main_vehicle_ids.m ('filepath'), because the LCC currently does not pass the filepath as function argument when calling main_vehicle_ids. The file is read once and then cached in a .mat-file with the same name. if the XML file is updated, the .mat has to be deleted in order to read the XML file with the updates. This reading process of the XML file takes some time (which is why it is cached).
4 | - The folder RTree is needed to build the RTree. The original version of Guttman is used, so there is room for improvements as in R* and R+ Trees for future work.
5 |
6 | - The folder ControllerFiles contains all files for the MPC problem. Priority_QP.m is the function to call
7 |
8 | - PathPlanner is the path planning class, which uses Dijkstra to find a path from start to goal at the start, and later updates the remaining path by discarding path edges we already passed.
9 |
10 | - configVehicle.m is used for the configuration of the HLC/MPC
11 |
12 | - vehicle.m is a class which is instanced for every vehicle. This class can be seen as the local HLC of each vehicle.
13 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/RTree/Point2D_RTree.m:
--------------------------------------------------------------------------------
1 | % data type for a 2D-point
2 | classdef Point2D_RTree
3 | properties (Access = public)
4 | x = double(0);
5 | y = double(0);
6 | end
7 | methods
8 |
9 | function obj = Point2D_RTree(x0,y0)
10 |
11 | if nargin == 2
12 | obj.x=x0;
13 | obj.y=y0;
14 | else
15 | % empty
16 | end
17 | end
18 |
19 | function delete(obj)
20 | end
21 | function out=get_x(obj)
22 | out = obj.x;
23 | end
24 | function obj=set_x(obj,value)
25 | obj.x = value;
26 | end
27 | function out=get_y(obj)
28 | out = obj.y;
29 | end
30 | function obj=set_y(obj,value)
31 | obj.y = value;
32 | end
33 | end
34 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/RTree/RBTElement.m:
--------------------------------------------------------------------------------
1 | classdef RBTElement < handle
2 | %
3 | % A class that implements pass by reference elements with numeric keys for
4 | % instances of the RedBlackTree class
5 | %
6 | % NOTE: This class is used internally by the RedBlackTree class
7 | %
8 | %
9 | % Public properties
10 | %
11 | properties (Access = public)
12 | key; % key
13 | left = nan % left child
14 | right = nan; % right child
15 | p = nan; % parent
16 | color = false; % color (true = red,false = black)
17 | size = 0; % size of subtree rooted at this element
18 | value = []; % miscellaneous data
19 | end
20 |
21 | %
22 | % Public methods
23 | %
24 | methods (Access = public)
25 | %
26 | % Constructor
27 | %
28 | function this = RBTElement(key,value)
29 | % Initialize key
30 | this.key = key;
31 |
32 | % Set value data, if specified
33 | if (nargin == 2)
34 | this.value = value;
35 | end
36 | end
37 |
38 | %
39 | % Element is nan if its key is nan
40 | %
41 | function bool = isnan(this)
42 | bool = isnan(this.key);
43 | end
44 | end
45 | end
46 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/RTree/drawCenterline.m:
--------------------------------------------------------------------------------
1 |
2 | % function to visualize the centerlines of the lanelet maps
3 | function drawCenterline(r_tree,map)
4 |
5 | hold on
6 | idList = r_tree.get_idList();
7 | Iterator = idList;
8 | while Iterator.hasSucc()
9 | Iterator = Iterator.get_Succ();
10 | keyId = Iterator.get_Content();
11 | lanelet = map.Search(keyId).value;
12 | lanelet.drawCenterline();
13 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/RTree/drawLaneletMap.m:
--------------------------------------------------------------------------------
1 | % function to visualize left and right bound of each lanelet in the map
2 | function fig =drawLaneletMap(r_tree,map)
3 | fig = figure;
4 | hold on
5 | idList = r_tree.get_idList;
6 | Iterator = idList;
7 | while Iterator.hasSucc()
8 | Iterator = Iterator.get_Succ();
9 | keyId = Iterator.get_Content();
10 | lanelet = map.Search(keyId).value;
11 | lanelet.drawLanelet();
12 | 1;
13 | end
14 |
15 |
16 | hold off
17 | end
18 |
19 |
20 |
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/QP_commonroad_target/configVehicle.m:
--------------------------------------------------------------------------------
1 | function cfg = configVehicle
2 |
3 | cfg.Hu = 7; % Stellhorizont
4 | cfg.Hp = 8; % (oberer) Praediktionshorizont
5 | cfg.dt = 0.25; % Abtastzeit in Sekunden
6 | cfg.dsafeVehicles = 0.2; % Sicherheitsabstand
7 | cfg.minSpeed = 0.02; % minimale Geschwindigkeit
8 | cfg.speed_target = 1; % Reisegeschwindigkeit
9 |
10 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/leader_follower/leader.m:
--------------------------------------------------------------------------------
1 | function [msg]=leader(vehicle_id, t_now)
2 | %% Do not display figures
3 | set(0,'DefaultFigureVisible','off');
4 |
5 | % Get data index
6 | point_period_nanoseconds = 250000000;
7 | t_eval = ((t_now + uint64(500000000)) / point_period_nanoseconds) * point_period_nanoseconds;
8 |
9 | trajectory_index = (t_eval) / point_period_nanoseconds;
10 | % Add vehicle ID to data index so that different cars have slightly different trajectories
11 | trajectory_index = trajectory_index + vehicle_id * 2;
12 |
13 | % Get current trajectory from pre-computed trajectory list
14 | trajectory_point = leader_trajectory(trajectory_index);
15 |
16 | %Create msg
17 | trajectory = VehicleCommandTrajectory;
18 | trajectory.vehicle_id = uint8(vehicle_id);
19 | trajectory_points = [];
20 | point1 = TrajectoryPoint;
21 |
22 | time = t_eval + 400000000;
23 | stamp = TimeStamp;
24 | stamp.nanoseconds = uint64(time);
25 | point1.t = stamp;
26 | point1.px = trajectory_point(1);
27 | point1.py = trajectory_point(2);
28 | point1.vx = trajectory_point(3);
29 | point1.vy = trajectory_point(4);
30 |
31 | trajectory_points = [trajectory_points [point1]];
32 | trajectory.trajectory_points = trajectory_points;
33 |
34 | % Return msg
35 | msg = trajectory;
36 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/platoon/main_vehicle_amount.m:
--------------------------------------------------------------------------------
1 | function main_vehicle_amount(amountOfVehicles)
2 | vehicle_ids = 1:amountOfVehicles;
3 | main_vehicle_ids(vehicle_ids);
4 | end
--------------------------------------------------------------------------------
/high_level_controller/examples/matlab/read_system_trigger.m:
--------------------------------------------------------------------------------
1 | function [got_start, got_stop] = read_system_trigger(reader_systemTrigger, trigger_stop)
2 | %READ_SYSTEM_TRIGGER Reads system trigger topic for start and stop signals
3 | % [GOT_START, GOT_STOP] = READ_SYSTEM_TRIGGER(READER_SYSTEMTRIGGER, TRIGGER_STOP)
4 | % where READER_SYSTEMTRIGGER is a DDS reader of the systemTrigger topic and
5 | % TRIGGER_STOP is the agreed number for a stop signal,
6 | % returns true as output if the corresponding signal was received,
7 | % false if it was not received.
8 | [trigger, ~, sample_count, ~] = reader_systemTrigger.take();
9 | got_stop = false;
10 | got_start = false;
11 | if sample_count > 0
12 | % look at most recent signal with (end)
13 | if trigger(end).next_start().nanoseconds() == trigger_stop
14 | got_stop = true;
15 | else
16 | got_start = true;
17 | end
18 | end
19 | end
--------------------------------------------------------------------------------
/indoor_positioning_system/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # exit when any command fails
3 | set -e
4 |
5 | mkdir -p build
6 | cd build
7 | cmake ..
8 | make -j$(nproc) && ./unittest
9 | cd ..
10 |
--------------------------------------------------------------------------------
/indoor_positioning_system/recordings/convert_to_csv.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | export LC_NUMERIC="en_US.UTF-8"
4 |
5 | rtirecconv -format csv -decodeChar text -decodeOctet hex -time epoch -compact no -filePrefix topic $1
--------------------------------------------------------------------------------
/indoor_positioning_system/recordings/record.bash:
--------------------------------------------------------------------------------
1 | rtirecord -cfgFile recordings/recording_config.xml -cfgName mydefault
--------------------------------------------------------------------------------
/indoor_positioning_system/recordings/recording_multiple_vehicles_manual_driving.dat_0_0:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/indoor_positioning_system/recordings/recording_multiple_vehicles_manual_driving.dat_0_0
--------------------------------------------------------------------------------
/indoor_positioning_system/recordings/recording_single_vehicle_manual_driving.dat_0_0:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/indoor_positioning_system/recordings/recording_single_vehicle_manual_driving.dat_0_0
--------------------------------------------------------------------------------
/indoor_positioning_system/recordings/recording_unreliable_ID.dat_0_0:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/indoor_positioning_system/recordings/recording_unreliable_ID.dat_0_0
--------------------------------------------------------------------------------
/indoor_positioning_system/src/DetectVehicleID.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include
3 | #include
4 | #include "cpm/Logging.hpp"
5 | #include "types.hpp"
6 |
7 | /**
8 | * \class DetectVehicleID
9 | * \brief TODO
10 | * \ingroup ips
11 | */
12 | class DetectVehicleID
13 | {
14 | //! TODO
15 | const std::vector identification_LED_period_ticks;
16 | //! TODO
17 | const std::vector identification_LED_enabled_ticks;
18 |
19 | public:
20 | /**
21 | * \brief Constructor TODO
22 | * \param _identification_LED_period_ticks TODO
23 | * \param _identification_LED_enabled_ticks TODO
24 | */
25 | DetectVehicleID(
26 | std::vector _identification_LED_period_ticks,
27 | std::vector _identification_LED_enabled_ticks
28 | );
29 |
30 |
31 | /**
32 | * \brief Tracks vehicles over time and identifies ID from middle LED
33 | * \param vehiclePointTimeseries List of VehiclePoints. Time between entries is required to be 20 ms
34 | * \return VehiclePoints with correct ID
35 | */
36 | VehiclePoints apply(const VehiclePointTimeseries &vehiclePointTimeseries);
37 |
38 | };
--------------------------------------------------------------------------------
/indoor_positioning_system/src/PoseCalculation.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include "types.hpp"
3 | #include "VehicleObservation.hpp"
4 |
5 | /**
6 | * \class PoseCalculation
7 | * \brief TODO
8 | * \ingroup ips
9 | */
10 | class PoseCalculation
11 | {
12 | //! TODO
13 | std::vector calibration_px;
14 | //! TODO
15 | std::vector calibration_py;
16 | //! TODO
17 | std::vector calibration_dx;
18 | //! TODO
19 | std::vector calibration_dy;
20 |
21 | public:
22 | /**
23 | * \brief Constructor TODO
24 | */
25 | PoseCalculation();
26 |
27 | /**
28 | * \brief TODO
29 | * \param vehiclePoints
30 | */
31 | std::vector apply(const VehiclePoints &vehiclePoints);
32 |
33 | };
--------------------------------------------------------------------------------
/indoor_positioning_system/src/UndistortPoints.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include "types.hpp"
3 | #include "LedPoints.hpp"
4 |
5 | /**
6 | * \class UndistortPoints
7 | * \brief TODO
8 | * \ingroup ips
9 | */
10 | class UndistortPoints
11 | {
12 |
13 | //! TODO
14 | std::vector calibration_x;
15 | //! TODO
16 | std::vector calibration_y;
17 |
18 |
19 | public:
20 | /**
21 | * \brief Constructor TODO
22 | * \param _calibration_x TODO
23 | * \param _calibration_y TODO
24 | */
25 | UndistortPoints(
26 | std::vector _calibration_x,
27 | std::vector _calibration_y
28 | );
29 |
30 | /**
31 | * \brief Converts the vehicle LED points from image coordinates to floor coordiantes
32 | * \param led_points TODO
33 | */
34 | FloorPoints apply(LedPoints led_points);
35 |
36 |
37 | };
--------------------------------------------------------------------------------
/indoor_positioning_system/src/pseudocode.txt:
--------------------------------------------------------------------------------
1 |
2 |
3 | void IPS_pseudo_code()
4 | {
5 | vehicle_point_buffer = {};
6 |
7 | while(1)
8 | {
9 | image_points = receive_image_points_dds();
10 |
11 | floor_points = undistort_points(image_points);
12 | vehicle_points = detect_vehicles(floor_points);
13 | vehicle_point_buffer.add(vehicle_points);
14 | vehicle_points_timeseries = vehicle_point_buffer.get_last_n(50);
15 | identified_vehicles = find_vehicle_ids(vehicle_points_timeseries);
16 | vehicle_poses = calculate_vehicle_poses(identified_vehicles);
17 |
18 | dss_broadcast(vehicle_poses);
19 |
20 | draw_visualization(
21 | floor_points,
22 | vehicle_points,
23 | identified_vehicles,
24 | vehicle_poses
25 | );
26 |
27 | }
28 |
29 | }
--------------------------------------------------------------------------------
/indoor_positioning_system/src/types.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include
3 | #include
4 | #include
5 | #include
6 |
7 | /**
8 | * \struct FloorPoints
9 | * \brief TODO
10 | * \ingroup ips
11 | */
12 | struct FloorPoints
13 | {
14 | //! TODO
15 | uint64_t timestamp;
16 |
17 | //! TODO
18 | std::vector points; // LED points in floor coordinates (meters)
19 | };
20 |
21 | /**
22 | * \struct VehiclePointSet
23 | * \brief TODO
24 | * \ingroup ips
25 | */
26 | struct VehiclePointSet
27 | {
28 | //! TODO
29 | int id = 0;
30 | //! TODO
31 | bool center_present = false;
32 |
33 | //! TODO
34 | cv::Point2d front;
35 | //! TODO
36 | cv::Point2d center;
37 | //! TODO
38 | cv::Point2d back_left;
39 | //! TODO
40 | cv::Point2d back_right;
41 | };
42 |
43 | /**
44 | * \struct VehiclePoints
45 | * \brief TODO
46 | * \ingroup ips
47 | */
48 | struct VehiclePoints
49 | {
50 | //! TODO
51 | uint64_t timestamp;
52 | //! TODO
53 | std::vector vehicles;
54 | };
55 |
56 | using VehiclePointTimeseries = std::list;
--------------------------------------------------------------------------------
/indoor_positioning_system/test/catch.cpp:
--------------------------------------------------------------------------------
1 | #define CATCH_CONFIG_MAIN
2 | #include "catch.hpp"
--------------------------------------------------------------------------------
/lab_control_center/bash/environment_variables.bash:
--------------------------------------------------------------------------------
1 | # Sets environment variables required for the programs to run. Most important: LD_LIBRARY_PATH (should only be set in a local terminal, never globally!) - hints to the location of the cpm library
2 | # This file is used on the remote system (NUC)
3 |
4 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib64/:~/dev/software/cpm_lib/build:/opt/rti_connext_dds-6.0.0/lib/x64Linux4gcc7.3.0
5 | export PATH=$PATH:$HOME/bin:$HOME/.local/bin:/opt/rti_connext_dds-6.0.0:/opt/rti_connext_dds-6.0.0/bin:/opt/rti_connext_dds-6.0.0/lib/x64Linux4gcc7.3.0:/opt/rti_connext_dds-6.0.0/bin:/opt/raspbian-toolchain-gcc-4.7.2-linux64/bin
6 | export NDDSHOME=/opt/rti_connext_dds-6.0.0
7 | export RASPBIAN_TOOLCHAIN=/opt/raspbian-toolchain-gcc-4.7.2-linux64
8 | export RTI_LICENSE_FILE=/opt/rti_connext_dds-6.0.0/rti_license.dat
9 |
10 | # Replace this if you want the software to run on a PC that is not the Lab's main PC
11 | # export IP_SELF="$(hostname -I)"
12 | # export IP_SELF="$(echo $IP_SELF)"
13 | # export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
14 |
15 | export DDS_INITIAL_PEER=rtps@udpv4://192.168.1.249:25598
16 |
--------------------------------------------------------------------------------
/lab_control_center/bash/environment_variables_local.bash:
--------------------------------------------------------------------------------
1 | # Put this into external file / find out if it is really necessary
2 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib64/:/tmp/software/cpm_lib/build/:/opt/rti_connext_dds-6.0.0/lib/x64Linux4gcc7.3.0
3 | export PATH=$PATH:$HOME/bin:$HOME/.local/bin:/opt/rti_connext_dds-6.0.0:/opt/rti_connext_dds-6.0.0/bin:/opt/rti_connext_dds-6.0.0/lib/x64Linux4gcc7.3.0:/opt/rti_connext_dds-6.0.0/bin:/opt/raspbian-toolchain-gcc-4.7.2-linux64/bin
4 | export NDDSHOME=/opt/rti_connext_dds-6.0.0
5 | export RASPBIAN_TOOLCHAIN=/opt/raspbian-toolchain-gcc-4.7.2-linux64
6 | export RTI_LICENSE_FILE=/opt/rti_connext_dds-6.0.0/rti_license.dat
7 |
8 | # Replace this if you want the software to run on a PC that is not the Lab's main PC
9 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
10 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
11 |
--------------------------------------------------------------------------------
/lab_control_center/bash/reboot_raspberry.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | sshpass -p $RPIPWD ssh -o StrictHostKeyChecking=no -t pi@$1 'sudo reboot now'
4 |
--------------------------------------------------------------------------------
/lab_control_center/bash/remote_kill.bash:
--------------------------------------------------------------------------------
1 |
2 | #!/bin/bash
3 | # IP must be set
4 | # This file is called by the LCC when distributed / remote deployment was selected and kill has been pressed
5 | # DESCRIPTION: Kill the middleware and script sessions that were started on the NUCs remotely
6 | for i in "$@"
7 | do
8 | case $i in
9 | --ip=*)
10 | IP="${i#*=}"
11 | shift # past argument=value
12 | ;;
13 | *)
14 | # unknown option
15 | ;;
16 | esac
17 | done
18 |
19 | trap exit_script SIGINT SIGTERM
20 |
21 | # Ping to make sure that the NUC is available - we want blocking behaviour in case it is not, this is handled by the C++ program
22 | # Write to /dev/null to suppress output
23 | until ping -c1 ${IP} >/dev/null 2>&1; do sleep 0.1; done
24 |
25 | ssh guest@${IP} << 'EOF'
26 | tmux kill-session -t "middleware"
27 | tmux kill-session -t "script"
28 | EOF
--------------------------------------------------------------------------------
/lab_control_center/bash/remote_start.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # This file is used on the remote system (NUC)
3 | # DESCRIPTION: Kill previous sessions of the middleware and the selected script, then launch the currently selected script + middleware in a new tmux session (using tmux_....bash scripts)
4 | #Get command line arguments
5 | for i in "$@"
6 | do
7 | case $i in
8 | --script_path=*)
9 | SCRIPT_PATH="${i#*=}"
10 | shift # past argument=value
11 | ;;
12 | --script_arguments=*)
13 | SCRIPT_ARGS="${i#*=}"
14 | shift # past argument=value
15 | ;;
16 | --middleware_arguments=*)
17 | MIDDLEWARE_ARGS="${i#*=}"
18 | shift # past argument=value
19 | ;;
20 | *)
21 | # unknown option
22 | ;;
23 | esac
24 | done
25 |
26 | # Kill potential previous sessions
27 | tmux kill-session -t "middleware"
28 | tmux kill-session -t "script"
29 |
30 |
31 | # Kill potential previous sessions - DO NOT change these session names, unless you intend to change them in the whole software repo
32 | rm -rf ~/dev/lcc_script_logs;mkdir -p ~/dev/lcc_script_logs
33 |
34 | # Start middleware
35 | tmux new-session -d -s "middleware" "cd /tmp/scripts/;bash tmux_middleware.bash --middleware_arguments='${MIDDLEWARE_ARGS}' &> ~/dev/lcc_script_logs/tmux_middleware.txt"
36 |
37 | # Start script
38 | tmux new-session -d -s "script" "cd /tmp/scripts/;bash tmux_script.bash --script_path=${SCRIPT_PATH} --script_arguments='${SCRIPT_ARGS}' &> ~/dev/lcc_script_logs/tmux_script.txt"
39 |
--------------------------------------------------------------------------------
/lab_control_center/bash/tmux_middleware.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # This file is used on the remote system (NUC)
3 | # DESCRIPTION: Start the middleware. Load relevant environment variables beforehand.
4 | #Get command line arguments
5 | for i in "$@"
6 | do
7 | case $i in
8 | --middleware_arguments=*)
9 | MIDDLEWARE_ARGS="${i#*=}"
10 | shift # past argument=value
11 | ;;
12 | *)
13 | # unknown option
14 | ;;
15 | esac
16 | done
17 |
18 | #Load environment variables, like RTI location, library location, Matlab location...
19 | . ./environment_variables.bash
20 |
21 | # Start screen for middleware; detach and start middleware
22 | cd ~/dev/software/middleware/build
23 |
24 | ./middleware ${MIDDLEWARE_ARGS} &> ~/dev/lcc_script_logs/middleware.log
25 |
--------------------------------------------------------------------------------
/lab_control_center/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # exit when any command fails
3 | set -e
4 |
5 | # Get directory of bash script
6 | BASH_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
7 |
8 | # Get yaml
9 | pushd ../..
10 | if [ ! -d "yaml-cpp" ]; then
11 | git clone https://github.com/jbeder/yaml-cpp.git
12 | cd yaml-cpp
13 | git checkout yaml-cpp-0.7.0
14 | mkdir -p build
15 | cd build
16 | cmake .. -DBUILD_SHARED_LIBS=ON
17 | make -j$(nproc)
18 | fi
19 | popd
20 |
21 |
22 | mkdir -p build
23 |
24 | cd build
25 | cmake .. -DSIMULATION=$SIMULATION
26 | make -j$(nproc)
27 | cd ..
28 |
29 | # Create launcher link to LCC
30 | if [ -d "${HOME}/.local/share/applications/" ]; then
31 | escaped_dir=$(printf '%s\n' "${BASH_DIR}" | sed 's:[][\/.^$*]:\\&:g')
32 | sed 's/TEMPLATE_LCC_DIR/'"$escaped_dir"'/g' lab-control-center.desktop > $HOME/.local/share/applications/lab-control-center.desktop
33 | fi
34 |
--------------------------------------------------------------------------------
/lab_control_center/cmake/FindLibXML++.cmake:
--------------------------------------------------------------------------------
1 | # find libxml++
2 | #
3 | # exports:
4 | #
5 | # LibXML++_FOUND
6 | # LibXML++_INCLUDE_DIRS
7 | # LibXML++_LIBRARIES
8 | #
9 |
10 | include(FindPkgConfig)
11 | include(FindPackageHandleStandardArgs)
12 |
13 | # Use pkg-config to get hints about paths
14 | pkg_check_modules(LibXML++_PKGCONF REQUIRED libxml++-2.6)
15 |
16 | # Include dir
17 | find_path(LibXML++_INCLUDE_DIR
18 | NAMES libxml++/libxml++.h
19 | PATHS ${LibXML++_PKGCONF_INCLUDE_DIRS}
20 | )
21 |
22 | # Finally the library itself
23 | find_library(LibXML++_LIBRARY
24 | NAMES xml++ xml++-2.6
25 | PATHS ${LibXML++_PKGCONF_LIBRARY_DIRS}
26 | )
27 |
28 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(LibXML++ DEFAULT_MSG LibXML++_LIBRARY LibXML++_INCLUDE_DIR)
29 |
30 |
31 | if(LibXML++_PKGCONF_FOUND)
32 | set(LibXML++_LIBRARIES ${LibXML++_LIBRARY} ${LibXML++_PKGCONF_LIBRARIES})
33 | set(LibXML++_INCLUDE_DIRS ${LibXML++_INCLUDE_DIR} ${LibXML++_PKGCONF_INCLUDE_DIRS})
34 | set(LibXML++_FOUND yes)
35 | else()
36 | set(LibXML++_LIBRARIES)
37 | set(LibXML++_INCLUDE_DIRS)
38 | set(LibXML++_FOUND no)
39 | endif()
40 |
41 | # Set the include dir variables and the libraries and let libfind_process do the rest.
42 | # NOTE: Singular variables for this library, plural for libraries this this lib depends on.
43 | #set(LibXML++_PROCESS_INCLUDES LibXML++_INCLUDE_DIR)
44 | #set(LibXML++_PROCESS_LIBS LibXML++_LIBRARY)
--------------------------------------------------------------------------------
/lab_control_center/gdb_run.bash:
--------------------------------------------------------------------------------
1 | # Run the LCC with GDB, so that errors are not missed as easily, especially if they cannot be reproduced
2 |
3 | # Get and go to LCC Path
4 | LCC_PATH="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )/"
5 | cd ${LCC_PATH}
6 |
7 | # Load environment Variables
8 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
9 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
10 |
11 | gdb -args ./build/lab_control_center --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --number_of_vehicles=20
--------------------------------------------------------------------------------
/lab_control_center/icon.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/lab_control_center/icon.png
--------------------------------------------------------------------------------
/lab_control_center/lab-control-center.desktop:
--------------------------------------------------------------------------------
1 | [Desktop Entry]
2 |
3 | # if you want to have a link for all users, do:
4 | # subsitute all "~" here with the path to your base directory
5 | # sudo cp $HOME/dev/software/lab_control_center/lab-control-center.desktop /usr/share/applications/
6 | # sudo mkdir -p /usr/share/icons/LCC
7 | # sudo cp $HOME/dev/software/lab_control_center/icon.png /usr/share/icons/LCC/icon.png
8 | # ($HOME/dev should be replaced with your install path in case you installed it somewhere else)
9 |
10 | # Application, Links (URL), or Directory (sub-group for applications)
11 | Type=Application
12 |
13 | Encoding=UTF-8
14 | Icon=TEMPLATE_LCC_DIR/icon.png
15 | Name=Lab Control Center
16 | GenericName=LCC
17 | Comment=Starts the LCC
18 | Exec=bash -c TEMPLATE_LCC_DIR/run.bash
19 |
20 | # To pass arguments use %u or %U
21 | # Exec=/thing/to/execute %u
22 |
23 | # Should run in a terminal?
24 | Terminal=true
25 |
26 | # Name of the window. Optional. See below.
27 | # Allows windows to stack
28 | # If this does not match, duplicate icons may show up in your dock
29 | StartupWMClass=lab_control_center
30 |
31 | # Optional categories
32 | Categories=Development;
33 |
--------------------------------------------------------------------------------
/lab_control_center/labcam/.gitignore:
--------------------------------------------------------------------------------
1 | build
--------------------------------------------------------------------------------
/lab_control_center/labcam/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | project("labcam")
2 | set(CMAKE_BUILD_TYPE Release)
3 |
4 | cmake_minimum_required(VERSION 3.1)
5 | add_definitions(-Wall -Wextra -Werror=return-type -Wno-unknown-pragmas)
6 | set (CMAKE_CXX_STANDARD 11)
7 |
8 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake" CACHE STRING "Modules for CMake" FORCE)
9 |
10 | # OpenCV
11 | set(OpenCV_LIBS_IN imgproc)
12 | find_package(OpenCV 4.0.0 REQUIRED ${OpenCV_LIBS_IN})
13 | link_libraries(${OpenCV_LIBS})
14 |
15 | # GStreamer
16 | find_package(GStreamer 1.0 REQUIRED)
17 | find_package(GLIB REQUIRED COMPONENTS)
18 | message("-- found GLIB ${GLib}")
19 | include_directories(${GSTREAMER_INCLUDE_DIRS})
20 |
21 | # Pylon
22 | find_package(Pylon REQUIRED)
23 | include_directories(${PYLON_INCLUDE_DIRS})
24 | message("-- found Pylon Version ${PYLON_VERSION}")
25 |
26 | # CPM lib
27 | link_directories(../cpm_lib/build/)
28 |
29 | # LabCam Binary
30 | FILE(GLOB SRC src/main.cpp)
31 | add_executable(labcam_recorder ${SRC})
32 |
33 | # LabCam Lib
34 | include_directories("include")
35 | FILE(GLOB SRC src/LabCam.cpp src/LabCamIface.cpp src/CInstantCameraAppSrc.cpp)
36 | add_library(labcamlib SHARED ${SRC})
37 | target_link_libraries(labcamlib ${OpenCV_LIBRARIES} ${PYLON_LIBRARIES} ${GLIB_LIBRARIES} ${GSTREAMER_LIBRARIES} ${GLib_LIBDIR} gobject-2.0 pthread)
38 | target_link_libraries(labcam_recorder cpm labcamlib)
--------------------------------------------------------------------------------
/lab_control_center/labcam/Readme.md:
--------------------------------------------------------------------------------
1 | # Labcam Requisites {#LabcamReadme}
2 |
3 | - Pylon SDK installieren
4 | - gstreamer
5 |
6 | sudo apt-get install libgstreamer*1.0* gstreamer*1.0*
7 |
8 | ## Camera Config
9 |
10 | Im Ordner camera_config liegt eine *.pfs Datei, die auf die Kamera geladen werden muss. Dies kann per Pylon Viewer gemacht werden. Zuerst mit Kamera verbinden, dann die pfs-Datei über Camera -> Load Features einmal laden. Um die Einstellung persistent zu machen, muss in den Einstellungen der Kamera unter Configuration Sets zunächst User Set 1 ausgewählt wird, dann "User Set Save" ausführt und unter Default Startup Set "User Set 1" auswählt.
--------------------------------------------------------------------------------
/lab_control_center/labcam/include/labcam/LabCamIface.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include
3 | #include
4 |
5 | class LabCam;
6 |
7 | /**
8 | * \brief This module provides standardized access to the LabCam.
9 | * \ingroup lcc_labcam
10 | */
11 | class LabCamIface {
12 | private:
13 | //! Contains the actual LabCam object.
14 | LabCam* impl_;
15 |
16 | public:
17 | /**
18 | * \brief Default Constructor
19 | */
20 | LabCamIface();
21 | ~LabCamIface() = default;
22 |
23 | /**
24 | * \brief Starts recording of LabCam.
25 | * \param path Folder in which the resulting video will be saved.
26 | * \param file_name Filename of the recording.
27 | */
28 | void startRecording(std::string path, std::string file_name);
29 |
30 | /**
31 | * \brief Stops current recording.
32 | */
33 | void stopRecording();
34 | };
--------------------------------------------------------------------------------
/lab_control_center/labcam/src/LabCamIface.cpp:
--------------------------------------------------------------------------------
1 | #include "labcam/LabCamIface.hpp"
2 | #include "labcam/LabCam.hpp"
3 |
4 | /**
5 | * \file LabCamIface.cpp
6 | * \ingroup lcc_labcam
7 | */
8 |
9 | LabCamIface::LabCamIface(){
10 | impl_ = new LabCam();
11 | }
12 |
13 | void LabCamIface::startRecording(std::string path, std::string file_name){
14 | impl_->startRecording(path, file_name);
15 | }
16 |
17 | void LabCamIface::stopRecording(){
18 | impl_->stopRecording();
19 | }
--------------------------------------------------------------------------------
/lab_control_center/parameters.yaml:
--------------------------------------------------------------------------------
1 | parameters:
2 | bool:
3 | {}
4 | uint64_t:
5 | middleware_period_ms:
6 | value: 400
7 | info: "The period with which the middleware calls the HLC, in milliseconds"
8 | int:
9 | {}
10 | double:
11 | trajectory_controller/lateral_D_gain:
12 | value: 4
13 | info: ""
14 | trajectory_controller/lateral_P_gain:
15 | value: 7
16 | info: ""
17 | string:
18 | {}
19 | ints:
20 | active_vehicle_ids:
21 | value: []
22 | info: "Currently active vehicle ids"
23 | doubles:
24 | {}
25 |
--------------------------------------------------------------------------------
/lab_control_center/recording/record.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | cd "$(dirname "$0")"
3 | export SCRIPT_DIR=$(pwd)
4 | export RECORDING_TIMESTAMP=$(date +%Y_%m_%d_%H_%M_%S)
5 |
6 | # Create recording config
7 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
8 |
9 | # Update recording config
10 | cat $SCRIPT_DIR/rti_recording_config_template.xml \
11 | | sed -e "s|TEMPLATE_IP|${IP_SELF}|g" \
12 | | sed -e "s|TEMPLATE_RECORDING_FOLDER|${RECORDING_TIMESTAMP}|g" \
13 | | sed -e "s|TEMPLATE_DOMAIN_ID|${DDS_DOMAIN}|g" \
14 | | sed -e "s|TEMPLATE_NDDSHOME|${NDDSHOME}|g" \
15 | >./rti_recording_config.xml
16 |
17 |
18 | # Start recording
19 | rtirecordingservice -cfgFile ./rti_recording_config.xml -cfgName cpm_recorder
20 |
21 | # The script continues here after the user presses Ctrl+C
--------------------------------------------------------------------------------
/lab_control_center/run.bash:
--------------------------------------------------------------------------------
1 | # Get and go to LCC Path
2 | LCC_PATH="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )/"
3 | cd ${LCC_PATH}
4 |
5 | # Load environment Variables
6 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
7 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
8 |
9 | ./build/lab_control_center --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --number_of_vehicles=20
--------------------------------------------------------------------------------
/lab_control_center/src/GoToPlanner.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include // shared_ptr
4 | #include
5 | #include
6 |
7 | #include "Pose2D.hpp"
8 | #include "TimeSeriesAggregator.hpp"
9 | #include "TrajectoryCommand.hpp"
10 |
11 | /**
12 | * \class GoToPlanner
13 | * \brief Class to control vehicles to poses
14 | * \ingroup lcc
15 | */
16 | class GoToPlanner {
17 | public:
18 | GoToPlanner(
19 | std::function()> get_goal_poses
20 | ,std::function get_vehicle_data
21 | ,std::shared_ptr trajectory_command
22 | ,std::string absolute_executable_path
23 | );
24 |
25 | void go_to_start_poses();
26 | void go_to_poses(std::vector goal_poses);
27 |
28 | private:
29 | std::function()> get_goal_poses;
30 | std::function get_vehicle_data;
31 | std::shared_ptr trajectory_command;
32 | std::thread planner_thread;
33 | std::string matlab_functions_path;
34 | };
--------------------------------------------------------------------------------
/lab_control_center/src/Joystick.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include "defaults.hpp"
4 |
5 | /**
6 | * \class Joystick
7 | * \brief Class for joystick controls
8 | * \ingroup lcc
9 | */
10 | class Joystick {
11 | //! TODO
12 | uint8_t joystick_buttons[256] = {};
13 | //! TODO
14 | int16_t joystick_axes[256] = {};
15 | //! TODO
16 | int js_fd = 0;
17 |
18 | /**
19 | * \brief TODO
20 | */
21 | void update();
22 |
23 | public:
24 | /**
25 | * \brief TODO
26 | * \param device_file TODO
27 | */
28 | Joystick(string device_file);
29 | /**
30 | * \brief TODO
31 | * \param id TODO
32 | */
33 | bool getButton(uint8_t id);
34 | /**
35 | * \brief TODO
36 | * \param id TODO
37 | */
38 | int16_t getAxis(uint8_t id);
39 | /**
40 | * \brief TODO
41 | */
42 | ~Joystick();
43 | };
--------------------------------------------------------------------------------
/lab_control_center/src/LogLevelSetter.cpp:
--------------------------------------------------------------------------------
1 | #include "LogLevelSetter.hpp"
2 |
3 | /**
4 | * \file LogLevelSetter.cpp
5 | * \ingroup lcc
6 | */
7 |
8 | LogLevelSetter::LogLevelSetter() :
9 | log_level_writer("logLevel", true, true, true)
10 | {
11 |
12 | }
13 |
14 | LogLevelSetter& LogLevelSetter::Instance()
15 | {
16 | static thread_local std::unique_ptr _instance(new LogLevelSetter());
17 |
18 | return *_instance.get();
19 | }
20 |
21 | void LogLevelSetter::set_log_level(unsigned short log_level)
22 | {
23 | //Create the LogLevel msg object
24 | LogLevel msg;
25 | msg.log_level(log_level);
26 |
27 | //Write the message / send it to all other participants within the domain
28 | log_level_writer.write(msg);
29 |
30 | //Priority 1 to always show this message in the logs -
31 | //it is not a critical error per se, but a level change
32 | //can be crucial for debugging and should thus always be indicated
33 | cpm::Logging::Instance().write(1, "Log level was changed to %i", static_cast(log_level));
34 | }
--------------------------------------------------------------------------------
/lab_control_center/src/commonroad_classes/InterfaceGeometry.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include "src/commonroad_classes/geometry/Point.hpp"
4 |
5 | /**
6 | * \class InterfaceGeometry
7 | * \brief This interface requires the deriving classes to implement a transfrom function
8 | * It is mainly used for clarity, to define common behaviour
9 | * \ingroup lcc_commonroad
10 | */
11 | class InterfaceGeometry
12 | {
13 | public:
14 | /**
15 | * \brief Get center (positional value) of a certain (composed) shape
16 | * \return Center of the shape
17 | */
18 | virtual std::pair get_center() = 0;
19 |
20 | //! Destructor. Good practice
21 | virtual ~InterfaceGeometry() {};
22 | };
--------------------------------------------------------------------------------
/lab_control_center/src/commonroad_classes/InterfaceTransformTime.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | /**
4 | * \class InterfaceTransformTime
5 | * \brief This interface requires the deriving classes to implement a transform function for timing
6 | * \ingroup lcc_commonroad
7 | */
8 | class InterfaceTransformTime
9 | {
10 | public:
11 | /**
12 | * \brief This function is used to change timing-related values, like velocity, where needed
13 | * \param time_scale The factor with which time step size was changed (e.g. 1.0->2.0 results in a factor of 0.5)
14 | */
15 | virtual void transform_timing(double time_scale) = 0;
16 |
17 | //! Destructor. Good practice
18 | virtual ~InterfaceTransformTime() {};
19 | };
--------------------------------------------------------------------------------
/lab_control_center/src/commonroad_classes/SpecificationError.cpp:
--------------------------------------------------------------------------------
1 | #include "commonroad_classes/SpecificationError.hpp"
2 |
3 | /**
4 | * \file SpecificationError.cpp
5 | * \ingroup lcc_commonroad
6 | */
7 |
8 | SpecificationError::SpecificationError(const std::string& msg) :
9 | std::runtime_error(msg)
10 | {
11 |
12 | }
--------------------------------------------------------------------------------
/lab_control_center/src/commonroad_classes/SpecificationError.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 | /**
6 | * \brief SpecificationError
7 | * This class defines an error that is thrown in constructors of classes used to translate Commonroad XML files in case that the file does not meed the expected structure / specification
8 | * \ingroup lcc_commonroad
9 | */
10 | class SpecificationError : virtual public std::runtime_error
11 | {
12 | public:
13 | /**
14 | * \brief Constructor for the specification error type, to create such an error
15 | * \param msg Error message
16 | */
17 | SpecificationError(const std::string& msg);
18 | };
--------------------------------------------------------------------------------
/lab_control_center/src/defaults.cpp:
--------------------------------------------------------------------------------
1 | #include "defaults.hpp"
2 |
3 | /**
4 | * \file defaults.cpp
5 | * \ingroup lcc
6 | */
7 |
8 | double frand() { return (double(rand()))/RAND_MAX; }
--------------------------------------------------------------------------------
/lab_control_center/test/LICENSE.txt:
--------------------------------------------------------------------------------
1 | Copyright 2019 Technical University of Munich, Professorship of Cyber-Physical Systems.
2 |
3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
4 |
5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
6 |
7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
8 |
9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
10 |
11 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
12 |
--------------------------------------------------------------------------------
/lab_control_center/test/TimerTestRealtime.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include
4 | #include "cpm/Timer.hpp"
5 | #include "cpm/CommandLineReader.hpp"
6 | #include "cpm/Logging.hpp"
7 |
8 |
9 | /**
10 | * \file TimerTestRealtime.cpp
11 | * \brief Test scenario: Creates a real-time timer that should be visible and stoppable from the LCC' timer tab
12 | * \ingroup lcc
13 | */
14 |
15 | int main(int argc, char *argv[]) {
16 | cpm::Logging::Instance().set_id("Logger_test");
17 |
18 | std::cout << "Creating timers..." << std::endl;
19 |
20 | std::string id = cpm::cmd_parameter_string("id", "sim_test", argc, argv);
21 | uint64_t period = cpm::cmd_parameter_uint64_t("period", 1000000ull, argc, argv);
22 |
23 | auto timer1 = cpm::Timer::create(id, period, 1ull, true, false, false);
24 | timer1->start([&](uint64_t t_now) {
25 | std::cout << "Time now: " << t_now << std::endl;
26 | std::this_thread::sleep_for(std::chrono::milliseconds(100));
27 | });
28 |
29 | std::cout << "Shutting down..." << std::endl;
30 |
31 | return 0;
32 | }
--------------------------------------------------------------------------------
/lab_control_center/test/TimerTestSimulated.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 | #include
4 | #include "cpm/Timer.hpp"
5 | #include "cpm/CommandLineReader.hpp"
6 | #include "cpm/Logging.hpp"
7 |
8 | /**
9 | * \file TimerTestSimulated.cpp
10 | * \brief Test scenario: Creates a simulated-time timer that should be visible and stoppable from the LCC' timer tab
11 | * \ingroup lcc
12 | */
13 |
14 | int main(int argc, char *argv[]) {
15 | cpm::Logging::Instance().set_id("Logger_test");
16 |
17 | std::cout << "Creating timers..." << std::endl;
18 |
19 | std::string id = cpm::cmd_parameter_string("id", "sim_test", argc, argv);
20 | uint64_t period = cpm::cmd_parameter_uint64_t("period", 1000000ull, argc, argv);
21 | uint64_t offset = cpm::cmd_parameter_uint64_t("offset", 0ull, argc, argv);
22 |
23 | auto timer1 = cpm::Timer::create(id, period, offset, true, true, true);
24 | timer1->start([&](uint64_t t_now) {
25 | std::cout << "Time now: " << t_now << std::endl;
26 | std::this_thread::sleep_for(std::chrono::milliseconds(100));
27 | });
28 |
29 | std::cout << "Shutting down..." << std::endl;
30 |
31 | return 0;
32 | }
--------------------------------------------------------------------------------
/lab_control_center/ui/commonroad/LaneletModelRecord.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | /**
7 | * \class LaneletModelRecord
8 | * \brief A GTK Tree...Record to store lanelet info that cannot be drawn conveniently on the map
9 | * \ingroup lcc_ui
10 | */
11 | class LaneletModelRecord : public Gtk::TreeModelColumnRecord {
12 | public:
13 | /**
14 | * \brief Constructor
15 | *
16 | * Sets the entries / columns for the TreeView that uses this record, here lanelet ID | lanelet type(s) | speed(2018) | user_one_way | user_bidirectional
17 | */
18 | LaneletModelRecord() {
19 | add(lanelet_id);
20 | add(lanelet_type);
21 | add(user_one_way);
22 | add(user_bidirectional);
23 | add(speed_2018);
24 | }
25 |
26 | //! Commonroad lanelet ID, column of the TreeView that uses this record
27 | Gtk::TreeModelColumn lanelet_id;
28 | //! Lanelet type, column of the TreeView that uses this record
29 | Gtk::TreeModelColumn lanelet_type;
30 | //! Allowed lanelet users in driving direction, column of the TreeView that uses this record
31 | Gtk::TreeModelColumn user_one_way;
32 | //! Allowed lanelet users bidirectional, column of the TreeView that uses this record
33 | Gtk::TreeModelColumn user_bidirectional;
34 | //! Lanelet speed limit (2018 specs), column of the TreeView that uses this record
35 | Gtk::TreeModelColumn speed_2018;
36 | };
--------------------------------------------------------------------------------
/lab_control_center/ui/commonroad/ProblemModelRecord.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | /**
7 | * \class ProblemModelRecord
8 | * \brief A GTK Tree...Record to store a planning problem ID, goal speed and goal time
9 | * \ingroup lcc_ui
10 | */
11 | class ProblemModelRecord : public Gtk::TreeModelColumnRecord {
12 | public:
13 | /**
14 | * \brief Constructor
15 | *
16 | * Sets the entries / columns for the TreeView that uses this record, here problem ID | goal speed | goal time.
17 | */
18 | ProblemModelRecord() {
19 | add(problem_id);
20 | add(problem_goal_speed);
21 | add(problem_goal_time);
22 | }
23 |
24 | //! Commonroad planning problem ID, column of the TreeView that uses this record
25 | Gtk::TreeModelColumn problem_id;
26 | //! Goal speed in a commonroad goal state, column of the TreeView that uses this record
27 | Gtk::TreeModelColumn problem_goal_speed;
28 | //! Goal time in a commonroad goal state, column of the TreeView that uses this record
29 | Gtk::TreeModelColumn problem_goal_time;
30 | };
--------------------------------------------------------------------------------
/lab_control_center/ui/lcc_errors/LCCErrorModelRecord.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | /**
7 | * \class LCCErrorModelRecord
8 | * \brief A GTK Tree...Record for storing error timestamps and content
9 | * \ingroup lcc_ui
10 | */
11 | class LCCErrorModelRecord : public Gtk::TreeModelColumnRecord {
12 | public:
13 | /**
14 | * \brief Constructor that defines the columns for a TreeView that uses this Record. Here: Timestamp | Error message
15 | */
16 | LCCErrorModelRecord() {
17 | add(timestamps);
18 | add(error_content);
19 | }
20 |
21 | //! Timestamp column
22 | Gtk::TreeModelColumn timestamps;
23 | //! Error message column
24 | Gtk::TreeModelColumn error_content;
25 | };
--------------------------------------------------------------------------------
/lab_control_center/ui/logger/LoggerModelRecord.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | /**
7 | * \class LoggerModelRecord
8 | * \brief A GTK Tree...Record for storing log ID, content and timestamp
9 | * \ingroup lcc_ui
10 | */
11 | class LoggerModelRecord : public Gtk::TreeModelColumnRecord {
12 | public:
13 | /**
14 | * \brief Constructor that defines the columns for the TreeView that uses this Record, in this case Log ID | Log content | Log timestamp
15 | */
16 | LoggerModelRecord() {
17 | add(log_id);
18 | add(log_content);
19 | add(log_stamp);
20 | }
21 |
22 | //! Column for log IDs
23 | Gtk::TreeModelColumn log_id;
24 | //! Column for log message contents
25 | Gtk::TreeModelColumn log_content;
26 | //! Column for log timestamps
27 | Gtk::TreeModelColumn log_stamp;
28 | };
--------------------------------------------------------------------------------
/lab_control_center/ui/map_view/car.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/lab_control_center/ui/map_view/car.png
--------------------------------------------------------------------------------
/lab_control_center/ui/map_view/car_small.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/lab_control_center/ui/map_view/car_small.png
--------------------------------------------------------------------------------
/lab_control_center/ui/map_view/labcam.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/lab_control_center/ui/map_view/labcam.png
--------------------------------------------------------------------------------
/lab_control_center/ui/map_view/map.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/lab_control_center/ui/map_view/map.png
--------------------------------------------------------------------------------
/lab_control_center/ui/map_view/object_small.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/lab_control_center/ui/map_view/object_small.png
--------------------------------------------------------------------------------
/lab_control_center/ui/params/ParamModelRecord.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | /**
7 | * \class ParamModelRecord
8 | * \brief A LCC UI GTK Tree...Record for storing the name, type, value and additional information of a parameter
9 | * \ingroup lcc_ui
10 | */
11 | class ParamModelRecord : public Gtk::TreeModelColumnRecord {
12 | public:
13 | /**
14 | * \brief Constructor that defines the columns for a TreeView that uses this Record. Here: Param name | Param type | Param value | Additional info / description
15 | */
16 | ParamModelRecord() {
17 | add(column_name);
18 | add(column_type);
19 | add(column_value);
20 | add(column_info);
21 | }
22 |
23 | //! Column for the parameter name
24 | Gtk::TreeModelColumn column_name;
25 | //! Column for the parameter type
26 | Gtk::TreeModelColumn column_type;
27 | //! Column for the parameter value
28 | Gtk::TreeModelColumn column_value;
29 | //! Column for the parameter description
30 | Gtk::TreeModelColumn column_info;
31 | };
--------------------------------------------------------------------------------
/lab_control_center/ui/right_tabs/right_tabs.glade:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
9 |
10 |
--------------------------------------------------------------------------------
/lab_control_center/ui/setup/upload_window.glade:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
35 |
36 |
--------------------------------------------------------------------------------
/lab_control_center/ui/timer/TimerModelRecord.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | /**
7 | * \class TimerModelRecord
8 | * \brief LCC UI GTK Tree...Record class that stores timer information: The timer user ID, last msg received from that timer,
9 | * status of the user and next desired time step (simulated time)
10 | * \ingroup lcc_ui
11 | */
12 | class TimerModelRecord : public Gtk::TreeModelColumnRecord {
13 | public:
14 | /**
15 | * \brief Constructor
16 | *
17 | * Sets the entries / columns for the TreeView that uses this record, here Timer ID | Time since last message | Participant status | Next time step
18 | */
19 | TimerModelRecord() {
20 | add(column_id);
21 | add(column_last_message);
22 | add(column_participant_status);
23 | add(column_next_step);
24 | }
25 |
26 | //! Timer ID, column of the TreeView that uses this record
27 | Gtk::TreeModelColumn column_id;
28 | //! Time since last message from this timer instance, column of the TreeView that uses this record
29 | Gtk::TreeModelColumn column_last_message;
30 | //! Status of the timer / participant with the given ID, column of the TreeView that uses this record
31 | Gtk::TreeModelColumn column_participant_status;
32 | //! Next time step of the timer (for simulated time), column of the TreeView that uses this record
33 | Gtk::TreeModelColumn column_next_step;
34 | };
--------------------------------------------------------------------------------
/lab_control_center/valgrind_run.bash:
--------------------------------------------------------------------------------
1 | # Run the LCC with GDB, so that errors are not missed as easily, especially if they cannot be reproduced
2 |
3 | # Get and go to LCC Path
4 | LCC_PATH="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )/"
5 | cd ${LCC_PATH}
6 |
7 | # Load environment Variables
8 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
9 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
10 |
11 | valgrind --tool=memcheck --leak-check=yes --num-callers=20 --log-file=vgdump --suppressions=gtk.suppression ./build/lab_control_center --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER --number_of_vehicles=20
--------------------------------------------------------------------------------
/low_level_controller/vehicle_atmega2560_firmware.atsln:
--------------------------------------------------------------------------------
1 |
2 | Microsoft Visual Studio Solution File, Format Version 12.00
3 | # Atmel Studio Solution File, Format Version 11.00
4 | VisualStudioVersion = 14.0.23107.0
5 | MinimumVisualStudioVersion = 10.0.40219.1
6 | Project("{54F91283-7BC4-4236-8FF9-10F437C3AD48}") = "vehicle_atmega2560_firmware", "vehicle_atmega2560_firmware\vehicle_atmega2560_firmware.cproj", "{DCE6C7E3-EE26-4D79-826B-08594B9AD897}"
7 | EndProject
8 | Global
9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution
10 | Debug|AVR = Debug|AVR
11 | Release|AVR = Release|AVR
12 | EndGlobalSection
13 | GlobalSection(ProjectConfigurationPlatforms) = postSolution
14 | {DCE6C7E3-EE26-4D79-826B-08594B9AD897}.Debug|AVR.ActiveCfg = Debug|AVR
15 | {DCE6C7E3-EE26-4D79-826B-08594B9AD897}.Debug|AVR.Build.0 = Debug|AVR
16 | {DCE6C7E3-EE26-4D79-826B-08594B9AD897}.Release|AVR.ActiveCfg = Release|AVR
17 | {DCE6C7E3-EE26-4D79-826B-08594B9AD897}.Release|AVR.Build.0 = Release|AVR
18 | EndGlobalSection
19 | GlobalSection(SolutionProperties) = preSolution
20 | HideSolutionNode = FALSE
21 | EndGlobalSection
22 | EndGlobal
23 |
--------------------------------------------------------------------------------
/low_level_controller/vehicle_atmega2560_firmware/adc.c:
--------------------------------------------------------------------------------
1 | /**
2 | * \file adc.c
3 | *
4 | * \date Created: 26.09.2018 12:31:36
5 | * \author maczijewski
6 | *
7 | * \brief Provides an interface for measuring battery voltage and motor current.
8 | *
9 | * \ingroup low_level_controller
10 | */
11 |
12 | #include "adc.h"
13 | #include "util.h"
14 | #include
15 | #include
16 |
17 |
18 | // The ADC is used to measure the battery voltage and the motor current sense.
19 | // Battery: ADC9, use 1.1V reference, battery voltage is scaled appropriately with a voltage divider.
20 | // Current Sense: ADC8, range: 0 mV to about 500 mV => use 1.1V reference
21 |
22 |
23 | void adc_measure(uint16_t* battery_voltage, uint16_t* current_sense) {
24 |
25 |
26 | // battery measurement
27 | SET_BIT(ADMUX, MUX0); // mux 100001 for ADC9
28 | _delay_us(125);
29 | ADCSRA |= (1 << ADSC);
30 | while (ADCSRA & (1 << ADSC) );
31 | *battery_voltage = ADC;
32 |
33 |
34 | _delay_us(125);
35 |
36 |
37 | // current sense measurement
38 | CLEAR_BIT(ADMUX, MUX0); // mux 100000 for ADC8
39 | _delay_us(125);
40 | ADCSRA |= (1 << ADSC);
41 | while (ADCSRA & (1 << ADSC) );
42 | *current_sense = ADC;
43 | }
44 |
45 |
46 | void adc_setup() {
47 | SET_BIT(ADCSRA, ADEN); // enable ADC
48 | SET_BIT(ADMUX, REFS1); // 1.1V reference
49 | SET_BIT(ADCSRB, MUX5); // mux[5:0] = 10000X for ADC8 or ADC9
50 |
51 | // set prescaler 128
52 | SET_BIT(ADCSRA, ADPS2);
53 | SET_BIT(ADCSRA, ADPS1);
54 | SET_BIT(ADCSRA, ADPS0);
55 | }
--------------------------------------------------------------------------------
/low_level_controller/vehicle_atmega2560_firmware/adc.h:
--------------------------------------------------------------------------------
1 | /**
2 | * \file adc.h
3 | *
4 | * \date Created: 26.09.2018 12:31:25
5 | * \author maczijewski
6 | *
7 | * \brief Provides an interface for measuring battery voltage and motor current.
8 | *
9 | * \ingroup low_level_controller
10 | */
11 |
12 |
13 | #ifndef ADC_H_
14 | #define ADC_H_
15 |
16 |
17 | #include
18 |
19 | /**
20 | * \brief After calling this function the given pointers will contain the results of the measurements.
21 | * \param battery_voltage The measured battery voltage
22 | * \param current_sense The measured motor current
23 | *
24 | * \author maczijewski
25 | * \ingroup low_level_controller
26 | */
27 | void adc_measure(uint16_t* battery_voltage, uint16_t* current_sense);
28 |
29 | /**
30 | * \brief Setup of the AD-converter
31 | *
32 | * \author maczijewski
33 | * \ingroup low_level_controller
34 | */
35 | void adc_setup();
36 |
37 |
38 | #endif /* ADC_H_ */
--------------------------------------------------------------------------------
/low_level_controller/vehicle_atmega2560_firmware/imu.h:
--------------------------------------------------------------------------------
1 | /**
2 | * \file imu.h
3 | *
4 | * \author maczijewski
5 | * \date Created: 27.09.2018 10:54:07
6 | *
7 | * \brief This module provides an interface for setting up and reading values of the inertial measurement unit (IMU).
8 | *
9 | * For details on the used IMU and the implementation see \link imu.c \endlink.
10 | *
11 | * \ingroup low_level_controller
12 | */
13 |
14 |
15 | #ifndef IMU_H_
16 | #define IMU_H_
17 |
18 |
19 | #include
20 | #include
21 |
22 | /**
23 | * \brief Starts IMU and configures its power mode, operation mode, and sensors.
24 | *
25 | * \author maczijewski
26 | * \ingroup low_level_controller
27 | */
28 | bool imu_setup();
29 |
30 | /**
31 | * \brief Reads the sensor values currently provided by the IMU.
32 | * \param imu_yaw value of the yaw relative to the initial orientation. This result is not directly
33 | * read from the IMU but accumulated based on the yaw_rate.
34 | * \param imu_yaw_rate rate with which the yaw changes
35 | * \param imu_acceleration_forward acceleration in forward direction
36 | * \param imu_acceleration_left acceleration to the left
37 | * \param imu_acceleration_up acceleration upwards
38 | *
39 | * \author maczijewski
40 | * \ingroup low_level_controller
41 | */
42 | bool imu_read(
43 | uint16_t* imu_yaw,
44 | int16_t* imu_yaw_rate,
45 | int16_t* imu_acceleration_forward,
46 | int16_t* imu_acceleration_left,
47 | int16_t* imu_acceleration_up
48 | );
49 |
50 |
51 | #endif /* IMU_H_ */
--------------------------------------------------------------------------------
/low_level_controller/vehicle_atmega2560_firmware/motor.c:
--------------------------------------------------------------------------------
1 | /**
2 | * \file motor.c
3 | *
4 | * \author maczijewski
5 | * \date Created: 20.09.2018 21:37:53
6 | *
7 | * \ingroup low_level_controller
8 | */
9 |
10 |
11 | #include "motor.h"
12 | #include "util.h"
13 | #include
14 | #include
15 |
16 |
17 | #define MOTOR_DIRECTION_BRAKE 0
18 | #define MOTOR_DIRECTION_FORWARD 1
19 | #define MOTOR_DIRECTION_REVERSE 2
20 |
21 |
22 | void motor_set_direction(uint8_t direction)
23 | {
24 | if(direction & MOTOR_DIRECTION_REVERSE) {
25 | SET_BIT(PORTC, 3);
26 | }
27 | else {
28 | CLEAR_BIT(PORTC, 3);
29 | }
30 |
31 | if(direction & MOTOR_DIRECTION_FORWARD) {
32 | SET_BIT(PORTC, 5);
33 | }
34 | else {
35 | CLEAR_BIT(PORTC, 5);
36 | }
37 | }
38 |
39 |
40 | void motor_set_duty(uint16_t duty) // values from 0 to 400
41 | {
42 | if(duty > 400) {
43 | duty = 400;
44 | }
45 | OCR4B = duty;
46 | }
47 |
48 |
49 | void motor_setup()
50 | {
51 | // PWM mode 9: PWM,Phase and Frequency Correct
52 | SET_BIT(TCCR4B, WGM43);
53 | SET_BIT(TCCR4A, WGM40);
54 |
55 | // Set PWM frequency to 20kHz
56 | OCR4A = 400;
57 |
58 | // Enable output on Pin 16 / OC4B / PH4
59 | SET_BIT(TCCR4A, COM4B1);
60 | SET_BIT(DDRH, 4);
61 |
62 | // no clock scaling, counter runs at 16MHz
63 | SET_BIT(TCCR4B, CS40);
64 |
65 | // Enable direction control
66 | SET_BIT(DDRC, 3);
67 | SET_BIT(DDRC, 5);
68 | motor_set_direction(MOTOR_DIRECTION_FORWARD);
69 | }
70 |
--------------------------------------------------------------------------------
/low_level_controller/vehicle_atmega2560_firmware/odometer.h:
--------------------------------------------------------------------------------
1 | /**
2 | * \file odometer.h
3 | *
4 | * \date 20.09.2018 17:43:13
5 | * \author: maczijewski
6 | *
7 | * \brief This module provides an interface for setting up and reading values of the odometer and inferred values.
8 | *
9 | * For implementation details see \link odometer.c \endlink.
10 | */
11 |
12 |
13 | #ifndef ODOMETER_H_
14 | #define ODOMETER_H_
15 |
16 | /**
17 | * \brief Current speed of the vehicle. Inferred by using the last few odometer steps and
18 | * the time between these subsequent measurements.
19 | *
20 | * \author maczijewski
21 | * \ingroup low_level_controller
22 | */
23 | int16_t get_speed();
24 |
25 | /**
26 | * \brief Returns the current count of odometer steps. Note: Overflow after about 6705km ;)
27 | *
28 | * \author maczijewski
29 | * \ingroup low_level_controller
30 | */
31 | int32_t get_odometer_count();
32 |
33 | /**
34 | * \brief Sets up all relevant registers such that the odometer provides correct values.
35 | *
36 | * \author maczijewski
37 | * \ingroup low_level_controller
38 | */
39 | void odometer_setup();
40 |
41 |
42 | #endif /* ODOMETER_H_ */
--------------------------------------------------------------------------------
/low_level_controller/vehicle_atmega2560_firmware/servo_timer.h:
--------------------------------------------------------------------------------
1 | /**
2 | * \file servo_timer.h
3 | *
4 | * \date 24.09.2018 16:18:47
5 | * \author: maczijewski
6 | *
7 | * \brief This module provides an interface for setting up and controlling the servo via PWM,
8 | * where the servo PWM signal runs at 50 Hz
9 | */
10 |
11 |
12 | #ifndef SERVO_TIMER_H_
13 | #define SERVO_TIMER_H_
14 |
15 |
16 | #include
17 |
18 |
19 | /**
20 | * \brief Returns the time since chip startup, in 20 msec increments.
21 | *
22 | * \author maczijewski
23 | * \ingroup low_level_controller
24 | */
25 | uint32_t get_tick();
26 |
27 | /**
28 | * \brief Sets up all relevant registers such that the pwm for the servo works properly.
29 | *
30 | * \author maczijewski
31 | * \ingroup low_level_controller
32 | */
33 | void servo_timer_setup();
34 |
35 | /**
36 | * \brief Sets the position of the servo by providing a PWM value in the range [1800,4200]
37 | * where 3000 is the middle value where the vehicle drives straight.
38 | * \param pwm
39 | *
40 | * \author maczijewski
41 | * \ingroup low_level_controller
42 | */
43 | void set_servo_pwm(uint16_t pwm);
44 |
45 |
46 | #endif /* SERVO_TIMER_H_ */
--------------------------------------------------------------------------------
/low_level_controller/vehicle_atmega2560_firmware/spi.h:
--------------------------------------------------------------------------------
1 | /**
2 | * \file spi.h
3 | *
4 | * \date Created: 21.09.2018 19:42:56
5 | * \author maczijewski
6 | *
7 | * \brief In order to make communication between mid_level_controller and
8 | * low_level_controller possible the Serial Peripheral Interface (SPI)
9 | * is used. This module here provides an interface to use SPI for
10 | * communicating with the mid_level_controller.
11 | *
12 | * \ingroup low_level_controller
13 | */
14 |
15 |
16 | #ifndef SPI_H_
17 | #define SPI_H_
18 |
19 |
20 | #include "spi_packets.h"
21 |
22 | /**
23 | * \brief Exchanges fixed bytes of data with the mid_level_controller via SPI.
24 | * Note: This low_level_controller is the slave. Since the communication
25 | * is synchronous, spi_exchange will block until the transfer is
26 | * completed.
27 | * \param packet_send The package to be sent to mid_level_controller.
28 | * \param packet_received The received package from mid_level_controller.
29 | *
30 | * \author maczijewski
31 | * \ingroup low_level_controller
32 | */
33 | void spi_exchange(spi_miso_data_t *packet_send, spi_mosi_data_t *packet_received);
34 |
35 | /**
36 | * \brief Sets up all relevant registers such that SPI can be used.
37 | *
38 | * \author maczijewski
39 | * \ingroup low_level_controller
40 | */
41 | void spi_setup();
42 |
43 |
44 | #endif /* SPI_H_ */
--------------------------------------------------------------------------------
/low_level_controller/vehicle_atmega2560_firmware/util.h:
--------------------------------------------------------------------------------
1 | /**
2 | * \file util.h
3 | *
4 | * \author maczijewski
5 | * \date Created: 20.09.2018 09:39:30
6 | *
7 | * \brief This file provides some utility defines.
8 | *
9 | * \ingroup low_level_controller
10 | */
11 |
12 | #ifndef UTIL_H_
13 | #define UTIL_H_
14 |
15 | /**
16 | * \brief Given a byte p, this define sets the n-th bit.
17 | * \ingroup low_level_controller
18 | */
19 | #define SET_BIT(p,n) ((p) |= (1 << (n)))
20 |
21 | /**
22 | * \brief Given a byte p, this define resets the n-th bit.
23 | * \ingroup low_level_controller
24 | */
25 | #define CLEAR_BIT(p,n) ((p) &= ~((1) << (n)))
26 |
27 | /**
28 | * \brief Given a byte p, this define toggles the n-th bit.
29 | * \ingroup low_level_controller
30 | */
31 | #define TOGGLE_BIT(p,n) ((p) ^= (1 << (n)))
32 |
33 |
34 | #endif /* UTIL_H_ */
--------------------------------------------------------------------------------
/low_level_controller/vehicle_atmega2560_firmware/watchdog.h:
--------------------------------------------------------------------------------
1 | /**
2 | * \file watchdog.h
3 | *
4 | * \date Created: 6/21/2019 13:35:35
5 | * \author: cfrauzem
6 | *
7 | * \brief The watchdog of the low_level_controller, which can be accessed via this module,
8 | * controls the state of the vehicle. If the watchdog is enabled and the
9 | * low_level_controller didn't get up-to-date information of the mid_level_controller
10 | * for too long, the watchdog will provoke the low_level_controller to be set into
11 | * safe-mode.
12 | *
13 | * \ingroup low_level_controller
14 | */
15 |
16 | #ifndef WATCHDOG_H_
17 | #define WATCHDOG_H_
18 |
19 |
20 | #include "spi_packets.h"
21 |
22 | extern volatile uint8_t safe_mode_flag;
23 |
24 | /**
25 | * \brief Disable the functionality of the watchdog.
26 | *
27 | * \author cfrauzem
28 | * \ingroup low_level_controller
29 | */
30 | void watchdog_disable();
31 |
32 | /**
33 | * \brief Enable the functionality of the watchdog.
34 | *
35 | * \author cfrauzem
36 | * \ingroup low_level_controller
37 | */
38 | void watchdog_enable();
39 |
40 | /**
41 | * \brief Reset the watchdog to the initial value. Has to be done regularily such that
42 | * the safe-mode is not provoked when low_level_controller and mid_level_controller
43 | * work together in a proper way.
44 | *
45 | * \author cfrauzem
46 | * \ingroup low_level_controller
47 | */
48 | void watchdog_reset();
49 |
50 | #endif /* WATCHDOG_H_ */
--------------------------------------------------------------------------------
/mid_level_controller/Doxygen_mlc_file_descriptions.hpp:
--------------------------------------------------------------------------------
1 | //File descriptions for the mid level controller, e.g. for bash files
2 |
3 | /**
4 | * \page vehicle_files_page Vehicle Files
5 | * \subpage v_f_start
6 | * \subpage v_f_bootloader
7 | * \subpage v_f_build
8 | * \subpage v_f_reboot_all
9 | * \subpage v_f_run
10 | * \subpage v_f_readme
11 | * \ingroup vehicle_files
12 | */
13 |
14 | /**
15 | * \page v_f_start ./package/start.bash
16 | * \brief Start script on the vehicle, which gets the vehicle ID from the vehicle's IP and with that information starts the vehicle software
17 | */
18 |
19 | /**
20 | * \page v_f_bootloader bootloader_raspberry.bash
21 | * \brief This script is installed on each raspberry under "/root/". It downloads the raspberry software from the master PC and runs it using ./package/start.bash
22 | */
23 |
24 | /**
25 | * \page v_f_build build.bash
26 | * \brief Builds the vehicle software for the raspberry and for local simulation, publishes it s.t. it can be downloaded by the raspberry
27 | */
28 |
29 | /**
30 | * \page v_f_reboot_all reboot_all_raspberry.bash
31 | * \brief Script to reboot vehicles / raspberry
32 | */
33 |
34 | /**
35 | * \page v_f_run run.bash
36 | * \brief Run script for the simulated vehicle
37 | */
38 |
39 | /**
40 | * \page v_f_readme ./Readme
41 | * \brief Instructions on how to run RTI DDS on the vehicle, soon deprecated due to the switch to eProsima
42 | */
--------------------------------------------------------------------------------
/mid_level_controller/Readme/HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files/community05_0_transparent.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/mid_level_controller/Readme/HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files/community05_0_transparent.png
--------------------------------------------------------------------------------
/mid_level_controller/Readme/HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files/css_2THG1eGiBIizsWFeexsNe1iDifJ00QRS9uSd03rY9co.css:
--------------------------------------------------------------------------------
1 | body,input,textarea,select{color:#000;background:none;}body.two-sidebars,body.sidebar-first,body.sidebar-second,body{width:640px;}#sidebar-first,#sidebar-second,.navigation,#toolbar,#footer-wrapper,.tabs,.add-or-remove-shortcuts{display:none;}.one-sidebar #content,.two-sidebars #content{width:100%;}#triptych-wrapper{width:960px;margin:0;padding:0;border:none;}#triptych-first,#triptych-middle,#triptych-last{width:250px;}#comments .title,#comments form,.comment_forbidden{display:none;}
2 |
--------------------------------------------------------------------------------
/mid_level_controller/Readme/HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files/raspbian-jessie-config_dialog.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/mid_level_controller/Readme/HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files/raspbian-jessie-config_dialog.png
--------------------------------------------------------------------------------
/mid_level_controller/Readme/HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files/raspbian_jessie_setup.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/mid_level_controller/Readme/HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files/raspbian_jessie_setup.png
--------------------------------------------------------------------------------
/mid_level_controller/Readme/HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files/raspi-config.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/mid_level_controller/Readme/HOWTO_Run_RTI_Connext_DDS_on_Raspberry_Pi_Data_Distribution_Service_(DDS)_Community_RTI_Connext_Users_files/raspi-config.jpg
--------------------------------------------------------------------------------
/mid_level_controller/Toolchain.cmake:
--------------------------------------------------------------------------------
1 | # Define our host system
2 | SET(CMAKE_SYSTEM_NAME Linux)
3 | SET(CMAKE_SYSTEM_VERSION 1)
4 |
5 | # Define the cross compiler locations
6 | SET(CMAKE_C_COMPILER $ENV{RASPBIAN_TOOLCHAIN}/bin/arm-linux-gnueabihf-gcc)
7 | SET(CMAKE_CXX_COMPILER $ENV{RASPBIAN_TOOLCHAIN}/bin/arm-linux-gnueabihf-g++)
8 |
9 | # Define the sysroot path for the RaspberryPi distribution in our tools folder
10 | SET(CMAKE_FIND_ROOT_PATH $ENV{RASPBIAN_TOOLCHAIN}/arm-raspbian-linux-gnueabi/sysroot/)
11 |
12 | # Use our definitions for compiler tools
13 | SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
14 | # Search for libraries and headers in the target directories only
15 | SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
16 | SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
17 |
--------------------------------------------------------------------------------
/mid_level_controller/bootloader_raspberry.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | # This script is installed on each raspberry under "/root/".
4 | # It downloads the raspberry software from the master PC
5 | # and runs it.
6 |
7 |
8 | # Add the following line to "/etc/rc.local" to enable auto start. Install tmux first.
9 | # tmux new-session -d -s "bootloader_raspberry" "bash /root/bootloader_raspberry.bash"
10 |
11 |
12 | sleep 5
13 | cd /tmp
14 | rm package.tar.gz
15 | wget http://192.168.1.249/raspberry/package.tar.gz
16 | tar -xzvf package.tar.gz
17 | cd package
18 | bash start.bash
19 |
--------------------------------------------------------------------------------
/mid_level_controller/get_all_logs.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | IDS=$'
4 | 01
5 | 02
6 | 03
7 | 04
8 | 05
9 | 06
10 | 07
11 | 08
12 | 09
13 | 10
14 | 11
15 | 12
16 | 13
17 | 14
18 | 15
19 | 16
20 | 17
21 | 18
22 | 19'
23 |
24 | folder="logs/$(date +%Y_%m_%d_%H_%M_%S)"
25 | mkdir -p $folder
26 |
27 | for id in $IDS;
28 | do
29 | echo "Retrieve log from vehicle $id"
30 | sshpass -p "cpmcpmcpm" scp pi@192.168.1.1$id:/tmp/package/Log* $folder/log_veh_$id.log &
31 | sleep 0.1
32 | done
33 |
34 | # Run last command in foreground so that hopefully all output is given until the shell allows the next command
35 | echo "Retrieve log from vehicle 20"
36 | sshpass -p "cpmcpmcpm" scp pi@192.168.1.120:/tmp/package/Log* log_veh_20
--------------------------------------------------------------------------------
/mid_level_controller/package/start.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF="$(hostname -I)"
5 |
6 | echo IP_SELF
7 | echo $IP_SELF
8 |
9 | # Copy local communication QoS, use correct IP
10 | sed -e "s/TEMPLATE_IP/${IP_SELF}/g" \
11 | <$DIR/QOS_LOCAL_COMMUNICATION.xml.template \
12 | >$DIR/build/QOS_LOCAL_COMMUNICATION.xml
13 |
14 | export VEHICLE_ID="$(hostname -I | tail -c 4)"
15 | export VEHICLE_ID="$(echo $VEHICLE_ID)"
16 |
17 |
18 | echo VEHICLE_ID
19 | echo $VEHICLE_ID
20 |
21 | wget http://192.168.1.249/raspberry/DDS_DOMAIN
22 | wget http://192.168.1.249/raspberry/DDS_INITIAL_PEER
23 |
24 |
25 | LD_LIBRARY_PATH=/tmp/package chrt -r 99 ./vehicle_rpi_firmware --simulated_time=false --dds_domain=$(cat DDS_DOMAIN) --dds_initial_peer=$(cat DDS_INITIAL_PEER) --vehicle_id=$VEHICLE_ID >stdout_$VEHICLE_ID.txt 2>stderr_$VEHICLE_ID.txt
26 |
--------------------------------------------------------------------------------
/mid_level_controller/reboot_all_raspberry.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | IPS=$'
4 | 192.168.1.101
5 | 192.168.1.102
6 | 192.168.1.103
7 | 192.168.1.104
8 | 192.168.1.105
9 | 192.168.1.106
10 | 192.168.1.107
11 | 192.168.1.108
12 | 192.168.1.109
13 | 192.168.1.110
14 | 192.168.1.111
15 | 192.168.1.112
16 | 192.168.1.113
17 | 192.168.1.114
18 | 192.168.1.115
19 | 192.168.1.116
20 | 192.168.1.117
21 | 192.168.1.118
22 | 192.168.1.119
23 | 192.168.1.120'
24 |
25 |
26 | for ip in $IPS;
27 | do
28 | echo "Rebooting $ip"
29 | sshpass -p "cpmcpmcpm" ssh -o StrictHostKeyChecking=no -t -t pi@$ip 'sudo reboot now' &
30 | sleep 0.1
31 | done
32 |
33 |
34 |
--------------------------------------------------------------------------------
/mid_level_controller/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 |
4 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
5 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
6 |
7 | ./build_x64_sim/vehicle_rpi_firmware --simulated_time=false --vehicle_id=$1 --pose=$2 --dds_domain=$DDS_DOMAIN --dds_initial_peer=$DDS_INITIAL_PEER >stdout_$1.txt 2>stderr_$1.txt
8 |
--------------------------------------------------------------------------------
/mid_level_controller/src/PathInterpolation.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include "VehicleCommandPathTracking.hpp"
4 |
5 | /**
6 | * \struct PathInterpolation
7 | * \brief TODO
8 | * \ingroup vehicle
9 | */
10 | struct PathInterpolation {
11 | //! TODO
12 | double s_queried;
13 | //! TODO
14 | double position_x;
15 | //! TODO
16 | double position_y;
17 | //! TODO
18 | double velocity_x;
19 | //! TODO
20 | double velocity_y;
21 | //! TODO
22 | double acceleration_x;
23 | //! TODO
24 | double acceleration_y;
25 | //! TODO
26 | double yaw;
27 | //! TODO
28 | double speed;
29 | //! TODO
30 | double curvature;
31 |
32 | /**
33 | * \brief TODO
34 | * \param s_queried TOOD
35 | * \param start_point TOOD
36 | * \param end_point TOOD
37 | */
38 | PathInterpolation(const double s_queried, const PathPoint start_point, const PathPoint end_point);
39 | };
--------------------------------------------------------------------------------
/mid_level_controller/src/PathTrackingController.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include
3 | #include "VehicleCommandPathTracking.hpp"
4 | #include "VehicleState.hpp"
5 | #include "Visualization.hpp"
6 | #include "cpm/Writer.hpp"
7 |
8 |
9 | /**
10 | * \class PathTrackingController
11 | * \brief TODO
12 | * \ingroup vehicle
13 | */
14 | class PathTrackingController
15 | {
16 | //! TOOD
17 | cpm::Writer writer_Visualization;
18 | //! TOOD
19 | uint8_t vehicle_id;
20 |
21 | /**
22 | * \brief TODO
23 | * \param path TODO
24 | * \param x TODO
25 | * \param y TODO
26 | */
27 | Pose2D find_reference_pose(
28 | const std::vector &path,
29 | const double x,
30 | const double y
31 | );
32 |
33 | public:
34 | /**
35 | * \brief TODO Constructor
36 | * \param vehicle_id TODO
37 | */
38 | PathTrackingController(uint8_t vehicle_id);
39 |
40 | /**
41 | * \brief TODO
42 | * \param vehicleState TODO
43 | * \param commandPathTracking TODO
44 | */
45 | double control_steering_servo(
46 | const VehicleState &vehicleState,
47 | const VehicleCommandPathTracking &commandPathTracking
48 | );
49 | };
--------------------------------------------------------------------------------
/mid_level_controller/src/SensorCalibration.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 |
4 | #include "VehicleState.hpp"
5 |
6 | extern "C" {
7 | #include "spi.h"
8 | }
9 |
10 | /**
11 | * \class SensorCalibration
12 | * \brief This class contains information on how raw sensor data of the low_level_controller
13 | * can be interpreted.
14 | * \ingroup vehicle
15 | */
16 | class SensorCalibration {
17 |
18 | public:
19 | /**
20 | * \brief This function converts raw sensor data from low_level_controller into
21 | * meaningful data, i.e. data with a common scale.
22 | * \param spi_miso_data The data package received from low_level_controller
23 | */
24 | static VehicleState convert(spi_miso_data_t spi_miso_data);
25 | };
--------------------------------------------------------------------------------
/mid_level_controller/src/SimulationIPS.cxx:
--------------------------------------------------------------------------------
1 | #include "SimulationIPS.hpp"
2 | #include "cpm/ParticipantSingleton.hpp"
3 | #include "cpm/stamp_message.hpp"
4 | #include
5 | #include
6 |
7 | /**
8 | * \file SimulationIPS.cxx
9 | * \ingroup vehicle
10 | */
11 |
12 | /**
13 | * \brief TODO
14 | * \ingroup vehicle
15 | */
16 | static inline double frand() { return (double(rand()))/RAND_MAX; }
17 |
18 | /**
19 | * \brief TODO
20 | * \ingroup vehicle
21 | */
22 | static inline double frand_sym() { return frand()*2-1; }
23 |
24 | SimulationIPS::SimulationIPS(std::string topic_name)
25 | :
26 | writer_vehicleObservation(topic_name)
27 | {
28 |
29 | }
30 |
31 |
32 |
33 | void SimulationIPS::update(VehicleObservation simulatedState)
34 | {
35 | // Simulate probability of detection
36 | if( rand()%20 == 1 ) return;
37 |
38 |
39 | // simulate signal noise
40 | simulatedState.pose().x(simulatedState.pose().x() + 0.002 * frand_sym());
41 | simulatedState.pose().y(simulatedState.pose().y() + 0.002 * frand_sym());
42 | simulatedState.pose().yaw(simulatedState.pose().yaw() + 0.01 * frand_sym());
43 |
44 | simulatedState.pose().yaw(remainder(simulatedState.pose().yaw(), 2*M_PI)); // yaw in range [-PI, PI]
45 |
46 |
47 | delay_buffer.push_back(simulatedState);
48 |
49 |
50 | // Send old sample
51 | if(delay_buffer.size() > 3)
52 | {
53 | writer_vehicleObservation.write(delay_buffer.front());
54 | delay_buffer.pop_front();
55 | }
56 | }
--------------------------------------------------------------------------------
/mid_level_controller/src/SimulationIPS.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include "VehicleObservation.hpp"
3 | #include "cpm/Writer.hpp"
4 | #include
5 | #include
6 |
7 | /**
8 | * \class SimulationIPS
9 | * \brief TODO
10 | * \ingroup vehicle
11 | */
12 | class SimulationIPS
13 | {
14 | //! TODO
15 | cpm::Writer writer_vehicleObservation;
16 |
17 | //! TODO
18 | std::list delay_buffer;
19 |
20 | public:
21 | /**
22 | * \brief TODO Constructor
23 | * \param topic_name TODO
24 | */
25 | SimulationIPS(std::string topic_name);
26 |
27 | /**
28 | * \brief TODO
29 | * \param simulatedState TODO
30 | */
31 | void update(VehicleObservation simulatedState);
32 |
33 | };
--------------------------------------------------------------------------------
/mid_level_controller/src/TrajectoryInterpolation.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include "VehicleCommandTrajectory.hpp"
4 |
5 | /**
6 | * \struct TrajectoryInterpolation
7 | * \brief TODO
8 | * \ingroup vehicle
9 | */
10 | struct TrajectoryInterpolation {
11 | //! TODO
12 | double t_now;
13 | //! TODO
14 | double position_x;
15 | //! TODO
16 | double position_y;
17 | //! TODO
18 | double velocity_x;
19 | //! TODO
20 | double velocity_y;
21 | //! TODO
22 | double acceleration_x;
23 | //! TODO
24 | double acceleration_y;
25 | //! TODO
26 | double yaw;
27 | //! TODO
28 | double speed;
29 | //! TODO
30 | double curvature;
31 |
32 | /**
33 | * \brief TODO
34 | * \param stamp_now TODO
35 | * \param start_point TODO
36 | * \param end_point TODO
37 | */
38 | TrajectoryInterpolation(uint64_t stamp_now, TrajectoryPoint start_point, TrajectoryPoint end_point);
39 | };
--------------------------------------------------------------------------------
/mid_level_controller/src/casadi_mpc_fn.h:
--------------------------------------------------------------------------------
1 | /* This file was automatically generated by CasADi.
2 | The CasADi copyright holders make no ownership claim of its contents. */
3 |
4 | /**
5 | * \file casadi_mpc_fn.h
6 | * \brief TODO, auto-generated
7 | * \ingroup vehicle
8 | */
9 |
10 | #ifdef __cplusplus
11 | extern "C" {
12 | #endif
13 |
14 | #ifndef casadi_real
15 | #define casadi_real double
16 | #endif
17 |
18 | #ifndef casadi_int
19 | #define casadi_int long long int
20 | #endif
21 |
22 | int casadi_mpc_fn(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem);
23 | int casadi_mpc_fn_alloc_mem(void);
24 | int casadi_mpc_fn_init_mem(int mem);
25 | void casadi_mpc_fn_free_mem(int mem);
26 | int casadi_mpc_fn_checkout(void);
27 | void casadi_mpc_fn_release(int mem);
28 | void casadi_mpc_fn_incref(void);
29 | void casadi_mpc_fn_decref(void);
30 | casadi_int casadi_mpc_fn_n_out(void);
31 | casadi_int casadi_mpc_fn_n_in(void);
32 | casadi_real casadi_mpc_fn_default_in(casadi_int i);
33 | const char* casadi_mpc_fn_name_in(casadi_int i);
34 | const char* casadi_mpc_fn_name_out(casadi_int i);
35 | const casadi_int* casadi_mpc_fn_sparsity_in(casadi_int i);
36 | const casadi_int* casadi_mpc_fn_sparsity_out(casadi_int i);
37 | int casadi_mpc_fn_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w);
38 | #ifdef __cplusplus
39 | } /* extern "C" */
40 | #endif
41 |
--------------------------------------------------------------------------------
/mid_level_controller/src/spi.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | #include "../../low_level_controller/vehicle_atmega2560_firmware/spi_packets.h"
3 |
4 | /**
5 | * \brief Sets up all relevant registers such that SPI can be used.
6 | * \ingroup vehicle
7 | */
8 | void spi_init();
9 |
10 | /**
11 | * \brief Exchanges fixed bytes of data with the mid_level_controller via SPI.
12 | * Note: This mid_level_controller is the master.
13 | * \ingroup vehicle
14 | *
15 | * \param spi_mosi_data The package to be sent to low_level_controller.
16 | * \param spi_miso_data_out After transfer is finished, this variable contains the received
17 | * package from low_level_controller.
18 | * \param n_transmission_attempts_out After transfer is finished, this variable indicates the
19 | * number of transmission attempts which were needed.
20 | * \param transmission_successful_out After transfer is finished, this flag indicates whether
21 | * the transmission was successful (1) or not (0).
22 | */
23 | void spi_transfer(
24 | spi_mosi_data_t spi_mosi_data,
25 | spi_miso_data_t *spi_miso_data_out,
26 | int *n_transmission_attempts_out,
27 | int *transmission_successful_out
28 | );
--------------------------------------------------------------------------------
/middleware/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.1)
2 | add_definitions(-Wall -Wextra -Werror=return-type)
3 | set (CMAKE_CXX_STANDARD 17)
4 |
5 | # RTI DDS
6 | add_definitions(-DRTI_UNIX -DRTI_LINUX -DRTI_64BIT -DRTI_STATIC)
7 | include_directories(SYSTEM $ENV{NDDSHOME}/include)
8 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds)
9 | include_directories(SYSTEM $ENV{NDDSHOME}/include/ndds/hpp)
10 | link_libraries(dl nsl m pthread rt)
11 | link_libraries(nddscpp2z nddscz nddscorez)
12 | link_directories($ENV{NDDSHOME}/lib/x64Linux4gcc7.3.0)
13 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -m64 -Wl,--no-as-needed")
14 |
15 | # CPM lib
16 | include_directories(SYSTEM ../cpm_lib/include/)
17 | include_directories(SYSTEM ../cpm_lib/include/cpm/dds/)
18 | link_directories(../cpm_lib/build/)
19 |
20 |
21 | include_directories(src)
22 |
23 | set (SOURCES
24 | src/Communication.hpp
25 | src/TypedCommunication.hpp
26 | src/TypedCommunication.cpp
27 | )
28 |
29 | add_executable( middleware
30 | src/main.cpp
31 | ${SOURCES}
32 | )
33 |
34 | target_link_libraries(middleware cpm)
35 |
36 | add_executable(unittest
37 | test/catch.cpp
38 | test/test_goals_to_hlc.cpp
39 | test/test_hlc_to_vehicle.cpp
40 | test/test_vehicle_to_middleware.cpp
41 | test/test_middleware_to_hlc.cpp
42 | test/test_vehicle_read.cpp
43 | ${SOURCES}
44 | )
45 |
46 | target_link_libraries(unittest cpm)
--------------------------------------------------------------------------------
/middleware/Doxygen_middleware_file_descriptions.hpp:
--------------------------------------------------------------------------------
1 | //File descriptions for middleware files
2 |
3 | /**
4 | * \page mw_files Middleware Files
5 | * \subpage mw_build
6 | * \subpage mw_run
7 | * \subpage qos_loc
8 | * \ingroup middleware_files
9 | */
10 |
11 | /**
12 | * \page mw_build build.bash
13 | * \brief Build script for the middleware
14 | *
15 | * Also creates a package to download for the NUC/HLC when it boots to get the latest version of the middleware.
16 | */
17 |
18 | /**
19 | * \page mw_run run.bash
20 | * \brief Run script for the middleware.
21 | *
22 | * Works given a vehicle ID (or multiple IDs, comma-separated) and a parameter that determines if simulated time should be used (true / false).
23 | */
24 |
25 | /**
26 | * \page qos_loc QOS_LOCAL_COMMUNICATION.xml.template
27 | * \brief Contains QoS settings
28 | *
29 | * QoS settings for the DDS participant used for the local-only communication between middleware and HLC.
30 | * See https://cpm.embedded.rwth-aachen.de/doc/display/CLD/Middleware+Usage
31 | */
--------------------------------------------------------------------------------
/middleware/build.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | # exit when any command fails
3 | set -e
4 |
5 | # Get directory of bash script
6 | BASH_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
7 |
8 | mkdir -p build
9 |
10 | # Copy local communication QoS, use correct IP
11 | IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
12 | sed -e "s/TEMPLATE_IP/${IP_SELF}/g" \
13 | <./QOS_LOCAL_COMMUNICATION.xml.template \
14 | >./build/QOS_LOCAL_COMMUNICATION.xml
15 |
16 | cd build
17 | cmake ..
18 | make -j$(nproc)
19 |
20 | # Publish middleware package via http/apache for the HLCs to download
21 | if [ -z $SIMULATION ]; then
22 | if [ ! -d "/var/www/html/nuc" ]; then
23 | sudo mkdir -p "/var/www/html/nuc"
24 | sudo chmod a+rwx "/var/www/html/nuc"
25 | fi
26 | cd ${BASH_DIR}
27 | rm -rf middleware_package
28 | mkdir middleware_package
29 | cp ${BASH_DIR}/build/middleware ./middleware_package
30 | cp ${BASH_DIR}/QOS_LOCAL_COMMUNICATION.xml.template ./middleware_package
31 | tar -czf middleware_package.tar.gz middleware_package
32 | rm -f /var/www/html/nuc/middleware_package.tar.gz
33 | cp ./middleware_package.tar.gz /var/www/html/nuc
34 | rm -rf middleware_package
35 | rm -rf middleware_package.tar.gz
36 | fi
37 |
38 | # Perform unittest
39 | cd ${BASH_DIR}/build
40 | ./unittest
41 |
--------------------------------------------------------------------------------
/middleware/run.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | # Argument 1: Vehicle ID
4 | # Argument 2: Simulated time
5 | vehicle_ids=$1
6 | simulated_time=$2
7 | middleware_id=$(printf "middleware_${vehicle_ids}" )
8 | echo $middleware_id
9 |
10 | export IP_SELF=$(ip route get 8.8.8.8 | awk -F"src " 'NR==1{split($2,a," ");print a[1]}')
11 | export DDS_INITIAL_PEER=rtps@udpv4://$IP_SELF:25598
12 |
13 | # Start screen for middleware; detach and start middleware
14 | cd ./build
15 | ./middleware --node_id=${middleware_id} --vehicle_ids=${vehicle_ids} --dds_domain=$DDS_DOMAIN --simulated_time=${simulated_time} --dds_initial_peer=${DDS_INITIAL_PEER}
--------------------------------------------------------------------------------
/middleware/test/catch.cpp:
--------------------------------------------------------------------------------
1 | #define CATCH_CONFIG_MAIN
2 | #include "catch.hpp"
--------------------------------------------------------------------------------
/tools/battery_level/Doxygen_battery_level.hpp:
--------------------------------------------------------------------------------
1 | //Doxygen software group and file descriptions for camera calibration
2 |
3 | /**
4 | * \defgroup battery_level Battery Level
5 | * \brief Relation between battery level and voltage. Includes experiment data.
6 | * \ingroup tools
7 | */
8 |
9 | /**
10 | * \page bl_files Files
11 | * \subpage bl_main
12 | * \ingroup battery_level
13 | */
14 |
15 | /**
16 | * \page bl_main main.m
17 | * \brief TODO
18 | */
--------------------------------------------------------------------------------
/tools/battery_level/u-level.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/battery_level/u-level.png
--------------------------------------------------------------------------------
/tools/camera_calibration/Doxygen_camera_calibration.hpp:
--------------------------------------------------------------------------------
1 | //Doxygen software group and file descriptions for camera calibration
2 |
3 | /**
4 | * \defgroup camera_calibration Camera Calibration
5 | * \brief Camera calibration matlab files, and points in calibration_points.csv
6 | * \ingroup tools
7 | */
8 |
9 | /**
10 | * \page camera_calib_files Camera Calibration Files
11 | * \subpage camera_calib_cal
12 | * \subpage camera_calib_end
13 | * \ingroup camera_calibration
14 | */
15 |
16 | /**
17 | * \page camera_calib_cal calibrationCheckerboardPoints.m
18 | * \brief TODO
19 | */
20 |
21 | /**
22 | * \page camera_calib_end end_to_end_calibration.m
23 | * \brief TODO
24 | */
--------------------------------------------------------------------------------
/tools/camera_calibration/calibrationCheckerboardPoints.m:
--------------------------------------------------------------------------------
1 | function calibrationCheckerboardPoints
2 |
3 | if exist('checkerboardPoints.mat','file')
4 | load checkerboardPoints
5 | else
6 | I=imread('/home/janis/Desktop/Image__2019-04-30__13-01-44.png');
7 | [imagePoints,boardSize] = detectCheckerboardPoints(I);
8 | save checkerboardPoints
9 | end
10 |
11 | imagePoints = reshape(imagePoints, boardSize(1)-1, boardSize(2)-1, 2);
12 |
13 | %imagePoints = flipud(imagePoints);
14 | imagePoints = fliplr(imagePoints);
15 |
16 | f = fopen('calibration_points.csv', 'w');
17 | fprintf(f,'world_x; world_y; image_x; image_y;\n');
18 |
19 | for ix = 1:size(imagePoints, 2)
20 | for iy = 1:size(imagePoints, 1)
21 | row = [(ix/10);(iy/10); squeeze(imagePoints(iy,ix,:))];
22 | fprintf(f,'%3.4f; ', row);
23 | fprintf(f,'\n');
24 | end
25 | end
26 |
27 |
28 | fclose(f);
29 |
30 | clf
31 |
32 | I = insertText(I,[imagePoints(1,end,1),imagePoints(1,end,2)],'x','FontSize',40);
33 | I = insertText(I,[imagePoints(end,1,1),imagePoints(end,1,2)],'y','FontSize',40);
34 | I = insertText(I,[imagePoints(1,1,1),imagePoints(1,1,2)],'origin','FontSize',40);
35 | imshow(I);
36 | hold on;
37 | plot(imagePoints(:,:,1),imagePoints(:,:,2),'rx');
38 | plot(imagePoints(1,1,1),imagePoints(1,1,2),'ro');
39 |
40 |
41 | end
42 |
43 |
--------------------------------------------------------------------------------
/tools/ips_pose_calibration/Doxygen_ips.hpp:
--------------------------------------------------------------------------------
1 | //Doxygen software group and file descriptions for camera calibration
2 |
3 | /**
4 | * \defgroup ips_calibration IPS Calibration
5 | * \brief Includes a matlab file for IPS pose calibration
6 | * \ingroup tools
7 | */
8 |
9 | /**
10 | * \page ips_calib_files IPS Calibration Files
11 | * \subpage ips_calib_pose
12 | * \ingroup ips_calibration
13 | */
14 |
15 | /**
16 | * \page ips_calib_pose pose_calibration.m
17 | * \brief TODO
18 | */
--------------------------------------------------------------------------------
/tools/map_print/Doxygen_map.hpp:
--------------------------------------------------------------------------------
1 | //Doxygen software group and file descriptions for camera calibration
2 |
3 | /**
4 | * \defgroup map Map Data
5 | * \brief TODO Includes map (print) data and lane graph matlab files
6 | * \ingroup tools
7 | */
--------------------------------------------------------------------------------
/tools/map_print/WorkflowPrint.txt:
--------------------------------------------------------------------------------
1 | Matlab:
2 | Export with
3 | print(fig, 'x.pdf', '-dpdf', '-painters')
4 |
5 | Inkscape:
6 | Import 'x.pdf'
7 | Ctrl+Shift+D -> "Resize page to drawing or selection"
8 | Save as 'x.svg'
9 |
10 | Scribus:
11 | Open 'x.svg'
12 | Edit -> Colors...
13 | Manually convert palette to CMYK
14 | Create "Tiefschwarz": 60 % Cyan, 50 % Magenta, 45 % Gelb, 100 % Schwarz
15 | Delete RGB black #000000, replace with "Tiefschwarz"
16 | Delete RGB white #ffffff, replace with CMYK white
17 | Convert other colors if necessary
18 | Increase size of background rectangle, to avoid stripes on the edges "Blitzer" (Optional?)
19 | Select All (Ctrl-A)
20 | Ungroup (Ctrl-Shift-G) multiple times
21 | Deselect All
22 | Select Background rectangle
23 | Properties (F2)
24 | Edit geometry: X,Y = -1, Width, Height: Increase by 2
25 | File -> Export -> As PDF
26 | General, Compatibility: PDF/X-3
27 | Pre-press, crop marks: check
28 | Pre-press, bleed marks: check
29 | Pre-press, registration marks: check
30 | Pre-press, bleed settings: ~1% of the document size
31 | experience value: 2pt for 4.5m print in 1:10 scale
32 | See also: "maxxprint_druckdaten_anforderungen.pdf"
33 |
--------------------------------------------------------------------------------
/tools/map_print/figure_eight_map_print/Klothoide.m:
--------------------------------------------------------------------------------
1 | function path = Klothoide(a,s,Y0)
2 | %
3 | % R = 625;
4 | % phi = pi/4;
5 |
6 | % a = R * sqrt(2 * phi);
7 | % s = 2 * phi * R;
8 |
9 |
10 | s_grid = linspace(0,s,1000);
11 | [~,Y] = ode45(@(s_,y_)klode(s_,y_,a), s_grid, Y0);
12 |
13 |
14 | path = Y(2:end,:);
15 | end
16 |
17 | function dY = klode(s,Y,a)
18 | dY = nan(size(Y));
19 | phi = Y(3);
20 | kappa = Y(4);
21 | d_phi = kappa;
22 | d_kappa = 1/a/abs(a);
23 | dx = cos(phi);
24 | dy = sin(phi);
25 | dY(1) = dx;
26 | dY(2) = dy;
27 | dY(3) = d_phi;
28 | dY(4) = d_kappa;
29 | end
--------------------------------------------------------------------------------
/tools/map_print/figure_eight_map_print/format_path_as_text.m:
--------------------------------------------------------------------------------
1 | function format_path_as_text(path)
2 |
3 | clc
4 |
5 | % trim to make a perfect loop
6 | dist = sqrt((path(:,1)-path(end,1)).^2 + (path(:,2)-path(end,2)).^2);
7 | i=find(diff(dist)>0);
8 | i=i(1)+1;
9 | path = path(i:end,:);
10 |
11 | % output controller path
12 | f = fopen('path.h','w');
13 |
14 | fprintf(f,'const vector path_x {');
15 | fprintf(f,'%f,',path(:,1)');
16 | fprintf(f,'};\n');
17 |
18 | fprintf(f,'const vector path_y {');
19 | fprintf(f,'%f,',path(:,2)');
20 | fprintf(f,'};\n');
21 |
22 | fprintf(f,'const vector path_yaw {');
23 | fprintf(f,'%f,',path(:,3)');
24 | fprintf(f,'};\n');
25 |
26 | fprintf(f,'const vector path_curvature {');
27 | fprintf(f,'%f,',path(:,4)');
28 | fprintf(f,'};\n');
29 |
30 | fclose(f);
31 |
32 |
33 | % output matlab visualization path
34 | f = fopen('path.m','w');
35 |
36 | fprintf(f,' path_x = [');
37 | fprintf(f,'%f,',path(1:50:end,1)');
38 | fprintf(f,'];\n');
39 |
40 | fprintf(f,' path_y = [');
41 | fprintf(f,'%f,',path(1:50:end,2)');
42 | fprintf(f,'];\n');
43 |
44 | fclose(f);
45 |
46 |
47 | end
48 |
49 |
--------------------------------------------------------------------------------
/tools/map_print/figure_eight_map_print/prepress.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/map_print/figure_eight_map_print/prepress.pdf
--------------------------------------------------------------------------------
/tools/map_print/map_calibration_squares/main.m:
--------------------------------------------------------------------------------
1 | function main
2 |
3 |
4 | plot_scale = 1000 / 25.4 * 72; % 1:1 scale
5 | plot_scale = 50 / 25.4 * 72; % 1:20 scale
6 | plot_scale = 100 / 25.4 * 72; % 1:10 scale
7 |
8 | full_width = 4.5;
9 | full_height = 4.0;
10 |
11 | close all
12 | figure('Visible','off')
13 | hold on
14 | axis equal
15 |
16 | fig = gcf;
17 | fig.Color = 'white';
18 |
19 | xlim(plot_scale*[-1 1] * full_width/2)
20 | ylim(plot_scale*[-1 1] * full_height/2)
21 |
22 | for i = -23:23
23 | for j = -21:21
24 | if mod(i+j, 2) == 0
25 | patch(...
26 | plot_scale*(0.05*[-1 1 1 -1]+0.0000001+0.1*(i-1)),...
27 | plot_scale*(0.05*[-1 -1 1 1]+0.0000001+0.05+0.1*(j-1)),...
28 | [0 0 0], 'EdgeColor', 'none')
29 | end
30 | end
31 | end
32 |
33 | axis off
34 | ax = gca;
35 | ax.Position = [0 0 1 1];
36 | fig.PaperPositionMode = 'manual';
37 | fig.PaperType = '';
38 | fig.PaperUnits = 'points';
39 | fig.PaperSize = [full_width full_height] * plot_scale;
40 | fig.InvertHardcopy = 'off';
41 | fig.PaperPosition = [0 0 full_width full_height] * plot_scale;
42 |
43 | print(fig, 'x.pdf', '-dpdf', '-painters')
44 | end
45 |
46 |
--------------------------------------------------------------------------------
/tools/map_print/map_calibration_squares/prepress.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/map_print/map_calibration_squares/prepress.pdf
--------------------------------------------------------------------------------
/tools/map_print/map_print2/cache.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/map_print/map_print2/cache.mat
--------------------------------------------------------------------------------
/tools/map_print/map_print2/collocation/Lagrange_differentiation_matrix.m:
--------------------------------------------------------------------------------
1 | function D = Lagrange_differentiation_matrix(nodes, interpolation_weights)
2 | deltas = nodes - nodes' + eye(length(nodes));
3 | Q = prod(deltas, 2) ./ deltas;
4 | D = interpolation_weights' .* Q + diag(interpolation_weights .* (sum(Q, 2) - 2 * diag(Q)));
5 | end
--------------------------------------------------------------------------------
/tools/map_print/map_print2/collocation/Lagrange_interpolation.m:
--------------------------------------------------------------------------------
1 | function x_interp = Lagrange_interpolation(ti, xi, t_interp, interpolation_weights)
2 |
3 | assert(size(ti, 2) == 1);
4 | assert(size(xi, 2) == 1);
5 | assert(size(interpolation_weights, 2) == 1);
6 | assert(size(t_interp, 1) == 1);
7 |
8 | x_interp = sum(interpolation_weights .* xi ./ (t_interp - ti),1) ./ sum(interpolation_weights ./ (t_interp - ti),1);
9 | assert( ~any(isnan(x_interp)) );
10 |
11 | end
--------------------------------------------------------------------------------
/tools/map_print/map_print2/collocation/Lagrange_interpolation_matrix.m:
--------------------------------------------------------------------------------
1 | function interpolation_matrix = Lagrange_interpolation_matrix(ti, t_interp, interpolation_weights)
2 |
3 | assert(size(ti, 2) == 1);
4 | assert(size(interpolation_weights, 2) == 1);
5 | assert(size(t_interp, 1) == 1);
6 |
7 |
8 | deltas = (t_interp - ti)';
9 | interpolation_matrix = nan(size(deltas));
10 |
11 | for j = 1:length(ti)
12 | deltas_copy = deltas;
13 |
14 | deltas_copy(:,j) = [];
15 | interpolation_matrix(:,j) = interpolation_weights(j) * prod(deltas_copy,2);
16 | end
17 |
18 | end
--------------------------------------------------------------------------------
/tools/map_print/map_print2/collocation/Lagrange_interpolation_weights.m:
--------------------------------------------------------------------------------
1 | function w = Lagrange_interpolation_weights(nodes)
2 | assert(size(nodes, 2) == 1);
3 | P = nodes - nodes' + eye(length(nodes));
4 | w = 1./prod(P,2);
5 | end
--------------------------------------------------------------------------------
/tools/map_print/map_print2/collocation/collocation_constants.m:
--------------------------------------------------------------------------------
1 | function [nodes, w_integration, D, w_interp] = collocation_constants(node_name, N, a, b, as_sym)
2 |
3 |
4 |
5 | if strcmp(node_name, 'LGL')
6 | [nodes, w_integration] = quadrature_LGL(N);
7 | elseif strcmp(node_name, 'CGL')
8 | [nodes, w_integration] = quadrature_CGL(N);
9 | end
10 |
11 | assert(all(diff(double(nodes))>0));
12 | %assert(abs(sum(double(w_integration)) - 2) < 1e-14);
13 |
14 |
15 | nodes = 0.5*(b-a)*(nodes+1) + a;
16 | w_integration = w_integration * 0.5 * (b-a);
17 |
18 | w_interp = Lagrange_interpolation_weights(nodes);
19 | D = Lagrange_differentiation_matrix(nodes, w_interp);
20 |
21 | if nargin ~= 5 || ~as_sym
22 | w_integration = double(w_integration);
23 | nodes = double(nodes);
24 | D = double(D);
25 | w_interp = double(w_interp);
26 | end
27 |
28 | end
--------------------------------------------------------------------------------
/tools/map_print/map_print2/collocation/quadrature_CGL.m:
--------------------------------------------------------------------------------
1 | function [x,w] = quadrature_CGL(N)
2 |
3 |
4 | % Chebyshev-Gauss-Lobatto nodes
5 | x=vpa(cos(pi*(0:N)/N)');
6 |
7 | w = nan(size(x));
8 | x = flipud(x);
9 |
10 | end
--------------------------------------------------------------------------------
/tools/map_print/map_print2/map-sketch.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/map_print/map_print2/map-sketch.jpg
--------------------------------------------------------------------------------
/tools/map_print/map_print2/prepress.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/map_print/map_print2/prepress.pdf
--------------------------------------------------------------------------------
/tools/map_print/map_print2/startup.m:
--------------------------------------------------------------------------------
1 | addpath('C:\Users\janis\casadi-windows-matlabR2016a-v3.4.5')
2 | addpath('collocation')
--------------------------------------------------------------------------------
/tools/map_print/maxxprint_druckdaten_anforderungen.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/map_print/maxxprint_druckdaten_anforderungen.pdf
--------------------------------------------------------------------------------
/tools/map_print/sub_lane_graph/graph_inversion.m:
--------------------------------------------------------------------------------
1 | function lane_graph = graph_inversion(original_lane_graph)
2 | lane_graph = original_lane_graph;
3 |
4 | end_node_index = num2cell([original_lane_graph.edges.end_node_index]);
5 | [lane_graph.edges.start_node_index] = end_node_index{:};
6 |
7 | start_node_index = num2cell([original_lane_graph.edges.start_node_index]);
8 | [lane_graph.edges.end_node_index] = start_node_index{:};
9 |
10 | for i_edge = 1:numel(lane_graph.edges)
11 | lane_graph.edges(i_edge).path = flip(original_lane_graph.edges(i_edge).path);
12 | end
13 | end
14 |
15 |
--------------------------------------------------------------------------------
/tools/map_print/sub_lane_graph/sub_graph_selection.m:
--------------------------------------------------------------------------------
1 | function lane_graph = sub_graph_selection(original_lane_graph, selected_nodes)
2 | %UNTITLED2 Summary of this function goes here
3 | % Detailed explanation goes here
4 | lane_graph.nodes = original_lane_graph.nodes(selected_nodes,:);
5 | lane_graph.edges = [];
6 | n_edges = size(original_lane_graph.edges,2);
7 | for i_edge = 1:n_edges
8 | % Check if edge connects two selected nodes
9 | current_edge = original_lane_graph.edges(i_edge);
10 | start_node = current_edge.start_node_index;
11 | end_node = current_edge.end_node_index;
12 | is_start_node_sel = any(selected_nodes == start_node);
13 | is_end_node_sel = any(selected_nodes == end_node);
14 | if (is_start_node_sel && is_end_node_sel)
15 | edge = current_edge;
16 | edge.start_node_index = find(selected_nodes == start_node);
17 | edge.end_node_index = find(selected_nodes == end_node);
18 | lane_graph.edges = [lane_graph.edges edge];
19 | end
20 | end
21 |
22 |
--------------------------------------------------------------------------------
/tools/map_print/sub_lane_graph/visualize_lane_graph.m:
--------------------------------------------------------------------------------
1 | function visualize_lane_graph(lane_graph)
2 | n_nodes = size(lane_graph.nodes,1);
3 | figure
4 | hold on
5 | for i_node = 1:n_nodes
6 | plot(lane_graph.nodes(i_node,1),lane_graph.nodes(i_node,2),'bo');
7 | textstr = num2str(i_node);
8 | text(lane_graph.nodes(i_node,1),lane_graph.nodes(i_node,2),textstr);
9 | end
10 | n_edges = size(lane_graph.edges,2);
11 | for i_edge = 1:n_edges
12 | plot(lane_graph.edges(i_edge).path(:,1), ...
13 | lane_graph.edges(i_edge).path(:,2), ...
14 | 'r');
15 | end
16 | axis equal
--------------------------------------------------------------------------------
/tools/reference_trajectory_map2_test_loop/Doxygen_reference_trajectory.hpp:
--------------------------------------------------------------------------------
1 | //Doxygen software group and file descriptions for camera calibration
2 |
3 | /**
4 | * \defgroup reference_trajectory_test Reference Trajectory Test
5 | * \brief TODO
6 | * \ingroup tools
7 | */
8 |
9 | /**
10 | * \page ref_tr_t_page Reference Trajectory Test
11 | * \subpage ref_tr_t_anim
12 | * \subpage ref_tr_t_create
13 | * \subpage ref_tr_t_export
14 | * \subpage ref_tr_t_load
15 | * \subpage ref_tr_t_opt
16 | * \ingroup reference_trajectory_test
17 | */
18 |
19 | /**
20 | * \page ref_tr_t_anim animate_trajectory.m
21 | * \brief TODO
22 | */
23 |
24 | /**
25 | * \page ref_tr_t_create create_reference_path.m
26 | * \brief TODO
27 | */
28 |
29 | /**
30 | * \page ref_tr_t_export export_trajectory.m
31 | * \brief TODO
32 | */
33 |
34 | /**
35 | * \page ref_tr_t_load load_collision_map.m
36 | * \brief TODO
37 | */
38 |
39 | /**
40 | * \page ref_tr_t_opt optimize_speed_profile.m
41 | * \brief TODO
42 | */
--------------------------------------------------------------------------------
/tools/reference_trajectory_map2_test_loop/animate_trajectory.m:
--------------------------------------------------------------------------------
1 | function animate_trajectory
2 |
3 | reference_path = dlmread('reference_path.csv');
4 | path_x = reference_path(:,1);
5 | path_y = reference_path(:,2);
6 |
7 | opt_result = load('optimization_checkpoint.mat');
8 |
9 | clf
10 | hold on
11 | plot(path_x, path_y);
12 | axis equal
13 | h = plot(0,0);
14 | plot(4.5*[0 0 1 1 0],4*[0 1 1 0 0],'k','LineWidth',4)
15 |
16 | position_fn = full(opt_result.s);
17 |
18 | k = 1;
19 | while true
20 | vehicle_x = [];
21 | vehicle_y = [];
22 |
23 | circle_c = [0.1*cos(linspace(0,2*pi,100)) nan];
24 | circle_s = [0.1*sin(linspace(0,2*pi,100)) nan];
25 |
26 | for i = 1:opt_result.p.nVeh
27 |
28 |
29 | t_idx = opt_result.p.delta_H_veh * i + k;
30 | s = circshift(position_fn, t_idx, 1);
31 | s_idx = s(1) / opt_result.p.ds + 1;
32 | s_idx = round(s_idx);
33 |
34 | tmp_x = circshift(path_x, s_idx, 1);
35 | tmp_y = circshift(path_y, s_idx, 1);
36 |
37 | vehicle_x = [vehicle_x (circle_c+tmp_x(1))];
38 | vehicle_y = [vehicle_y (circle_s+tmp_y(1))];
39 | end
40 |
41 | set(h, 'XData', vehicle_x);
42 | set(h, 'YData', vehicle_y);
43 | drawnow
44 | k = k + 2;
45 | end
46 |
47 | end
48 |
49 |
--------------------------------------------------------------------------------
/tools/reference_trajectory_map2_test_loop/load_collision_map.m:
--------------------------------------------------------------------------------
1 | function [collision_map, collision_dist_sq] = load_collision_map
2 |
3 | reference_path = dlmread('reference_path.csv');
4 | path_x = reference_path(:,1);
5 | path_y = reference_path(:,2);
6 |
7 | [PX1, PX2] = meshgrid(path_x, path_x);
8 | [PY1, PY2] = meshgrid(path_y, path_y);
9 |
10 | collision_dist_sq = (PX2 - PX1).^2 + (PY2 - PY1).^2;
11 | collision_map = exp(-collision_dist_sq / 0.1);
12 |
13 | %imagesc(flipud(collision_map))
14 | end
15 |
16 |
--------------------------------------------------------------------------------
/tools/reference_trajectory_map2_test_loop/optimization_checkpoint.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/reference_trajectory_map2_test_loop/optimization_checkpoint.mat
--------------------------------------------------------------------------------
/tools/vehicle_dynamics_identification_and_mpc/documentation/main.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/vehicle_dynamics_identification_and_mpc/documentation/main.pdf
--------------------------------------------------------------------------------
/tools/vehicle_dynamics_identification_and_mpc/optimize_parameters_delay_grid.m:
--------------------------------------------------------------------------------
1 | function optimize_parameters_delay_grid
2 | for n_delay_steps_IPS = 0:1
3 | for n_delay_steps_local = 0:1
4 | for n_delay_steps_steering = 2:8
5 | for n_delay_steps_motor = 0:8
6 |
7 | file_name = sprintf('output/optimal_parameters_d_ips_%i_d_local_%i_d_steer_%i_d_mot_%i.mat', ...
8 | n_delay_steps_IPS, n_delay_steps_local, n_delay_steps_steering, n_delay_steps_motor);
9 |
10 |
11 | if exist(file_name, 'file')
12 | fprintf('Skipping "%s"\n', file_name);
13 | else
14 | optimize_parameters(file_name, n_delay_steps_IPS, n_delay_steps_local, n_delay_steps_steering, n_delay_steps_motor);
15 | end
16 |
17 | end
18 | end
19 | end
20 | end
21 | end
--------------------------------------------------------------------------------
/tools/vehicle_dynamics_identification_and_mpc/read_rti_csv.m:
--------------------------------------------------------------------------------
1 | function result = read_rti_csv(filepath)
2 |
3 | fID = fopen(filepath, 'r');
4 |
5 | line1 = fgetl(fID);
6 | line2 = fgetl(fID);
7 |
8 | fclose(fID);
9 |
10 | column_header = strsplit(line2,',');
11 |
12 | for i = 1:length(column_header)
13 | column_header{i} = strrep(column_header{i},'.','_');
14 | end
15 |
16 | data = dlmread(filepath, ',',2,0);
17 |
18 | assert(size(data,2) == length(column_header));
19 |
20 | result = struct;
21 |
22 | for i = 1:length(column_header)
23 | result.(column_header{i}) = data(:,i);
24 | end
25 |
26 | end
27 |
28 |
--------------------------------------------------------------------------------
/tools/vehicle_dynamics_identification_and_mpc/readme.md:
--------------------------------------------------------------------------------
1 | Measurement data were recorded with RTI's rtirecord version 5.3.1.
2 | Measurement data in `recording_vehicles_2_3_test_loop.dat` and `recording_vehicles_2_3_test_loop_b.dat`.
3 |
4 | In order to execute parameter identification with RTI 6.0.0:
5 | 1. Convert to `.mat`-file with `recording_to_mat.bash`
6 | 2. Prepare measurements with `prepare_measurement_data.m`
7 | 3. Optimize parameters with `optimize_parameters_delay_grid.m`
8 | 4. Evaluate results with `optimize_parameters_delay_grid_eval.m`
--------------------------------------------------------------------------------
/tools/vehicle_dynamics_identification_and_mpc/recording_to_mat.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | rticonverter -cfgFile LEGACY_CONVERTER.xml -cfgName test_loop
4 | rticonverter -cfgFile LEGACY_CONVERTER.xml -cfgName test_loop_b
5 |
6 | matlab -sd . -batch sql_to_mat
--------------------------------------------------------------------------------
/tools/vehicle_dynamics_identification_and_mpc/recording_vehicles_2_3_test_loop.dat_0_0:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/vehicle_dynamics_identification_and_mpc/recording_vehicles_2_3_test_loop.dat_0_0
--------------------------------------------------------------------------------
/tools/vehicle_dynamics_identification_and_mpc/recording_vehicles_2_3_test_loop_b.dat_0_0:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/embedded-software-laboratory/cpm_lab/6b571c51022a25e59755b77f2df8209f74511fb8/tools/vehicle_dynamics_identification_and_mpc/recording_vehicles_2_3_test_loop_b.dat_0_0
--------------------------------------------------------------------------------
/tools/vehicle_dynamics_identification_and_mpc/sql_to_mat.m:
--------------------------------------------------------------------------------
1 | addpath('../../lab_control_center/recording/visualization');
2 | databyveh = preprocessing(0, './output/recording_vehicles_2_3_test_loop.dat');
3 | save('recording_vehicles_2_3_test_loop.mat', 'databyveh');
4 | databyveh = preprocessing(0, './output/recording_vehicles_2_3_test_loop_b.dat');
5 | save('recording_vehicles_2_3_test_loop_b.mat', 'databyveh');
6 | delete('dds_json_sample.mat')
7 | delete('dds_record.mat')
--------------------------------------------------------------------------------
/tools/vehicle_dynamics_identification_and_mpc/vehicle_dynamics.m:
--------------------------------------------------------------------------------
1 | function dx = vehicle_dynamics(x,u,p)
2 |
3 | %% See 'vehicle_dynamics.png/tex' for documentation.
4 |
5 | px = x(:,1);
6 | py = x(:,2);
7 | yaw = x(:,3);
8 | v = x(:,4);
9 |
10 | f = u(:,1);
11 | delta_ref = u(:,2);
12 | V = u(:,3);
13 |
14 | delta = delta_ref + p(9);
15 |
16 | dx = 0*x;
17 |
18 | dx(:,1) = p(1) .* v .* (1 + p(2) .* delta.^2) .* cos(yaw + p(3) .* delta + p(10));
19 | dx(:,2) = p(1) .* v .* (1 + p(2) .* delta.^2) .* sin(yaw + p(3) .* delta + p(10));
20 | dx(:,3) = p(4) .* v .* delta;
21 | dx(:,4) = p(5) .* v + (p(6) + p(7) .* V) .* sign(f) .* (abs(f).^(p(8)));
22 |
23 | end
--------------------------------------------------------------------------------
/tools/vehicle_dynamics_identification_and_mpc/vehicle_dynamics_no_bat.m:
--------------------------------------------------------------------------------
1 | function dx = vehicle_dynamics_no_bat(x,u,p)
2 |
3 | %% See 'vehicle_dynamics.png/tex' for documentation.
4 | % Cross check with vehicle paper
5 |
6 | px = x(:,1);
7 | py = x(:,2);
8 | yaw = x(:,3);
9 | v = x(:,4);
10 |
11 | f = u(:,1);
12 | delta_ref = u(:,2);
13 |
14 | delta = delta_ref + p(8);
15 |
16 | dx = 0*x;
17 |
18 | dx(:,1) = p(1) .* v .* (1 + p(2) .* delta.^2) .* cos(yaw + p(3) .* delta + p(9));
19 | dx(:,2) = p(1) .* v .* (1 + p(2) .* delta.^2) .* sin(yaw + p(3) .* delta + p(9));
20 | dx(:,3) = p(4) .* v .* delta;
21 | dx(:,4) = p(5) .* v + p(6) .* sign(f) .* (abs(f).^(p(7)));
22 |
23 | end
24 |
25 |
--------------------------------------------------------------------------------
/update_nucs.bash:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | #Get command line arguments
3 | for i in "$@"
4 | do
5 | case $i in
6 | -vi=*|--vehicle_ids=*)
7 | vehicle_ids="${i#*=}"
8 | shift # past argument=value
9 | ;;
10 | -va=*|--vehicle_amount=*)
11 | vehicle_amount="${i#*=}"
12 | shift # past argument=value
13 | ;;
14 | -pw=*|--password=*)
15 | password="${i#*=}"
16 | shift # past argument=value
17 | ;;
18 | *)
19 | # unknown option
20 | ;;
21 | esac
22 | done
23 |
24 | #Check for existence of required command line arguments
25 | if ( [ -z "$vehicle_ids" ] && [ -z "$vehicle_amount" ] ) || [ -z "$password" ]
26 | then
27 | echo "Invalid use, enter vehicle IDs or amount and a password"
28 | exit 1
29 | fi
30 |
31 | #If vehicle amount was set, create vehicle id list in vehicle_ids from that, style: 1,...,vehicle_amount
32 | if !([ -z "$vehicle_amount" ])
33 | then
34 | vehicle_ids=$(seq -s, 1 1 ${vehicle_amount})
35 | fi
36 |
37 | IFS=,
38 | for val in $vehicle_ids;
39 | do
40 | ip=$(printf "192.168.1.2%02d" ${val})
41 | echo $ip
42 | sshpass -p $password ssh -t controller@$ip "echo ${password} | sudo -S apt-get update;sudo apt-get upgrade"
43 | done
--------------------------------------------------------------------------------