diff --git a/README.md b/README.md index 41ebb9fe64c..2047df48b81 100644 --- a/README.md +++ b/README.md @@ -14,19 +14,20 @@ Open-source software for autonomous driving - Qt 5.2.1 or higher - CUDA(Optional) - FlyCapture2(optional) +- Armadillo ### Install dependencies for Ubuntu 14.04 indigo ``` % sudo apt-get install ros-indigo-desktop-full ros-indigo-nmea-msgs ros-indigo-sound-play -% sudo apt-get install libnlopt-dev freeglut3-dev qtbase5-dev libqt5opengl5-dev libssh2-1-dev +% sudo apt-get install libnlopt-dev freeglut3-dev qtbase5-dev libqt5opengl5-dev libssh2-1-dev libarmadillo-dev ``` ### Install dependencies for Ubuntu 13.10 indigo and Ubuntu 13.04 hydro ``` % sudo apt-get install ros-hydro-desktop-full ros-indigo-nmea-msgs ros-hydro-sound-play -% sudo apt-get install libnlopt-dev freeglut3-dev libssh2-1-dev +% sudo apt-get install libnlopt-dev freeglut3-dev libssh2-1-dev libarmadillo-dev ``` ### Install Velodyne Driver dependencies diff --git a/ros/src/.config/rviz/demo.rviz b/ros/src/.config/rviz/demo.rviz index 6cf6d47fb11..6f409e157b0 100644 --- a/ros/src/.config/rviz/demo.rviz +++ b/ros/src/.config/rviz/demo.rviz @@ -4,12 +4,9 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /Status1 - - /PointCloud21 - - /Image1 + - /MarkerArray1/Namespaces1 Splitter Ratio: 0.5 - Tree Height: 865 + Tree Height: 589 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -28,80 +25,59 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Visualization Manager: Class: "" Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 + - Class: rviz/Marker Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: + Marker Topic: /vscan_linelist + Name: Marker + Namespaces: + {} + Queue Size: 100 Value: true - - Alpha: 0.5 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 9.35255 - Min Value: -11.618 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 + - Class: rviz/MarkerArray Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 135 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.01 - Style: Points - Topic: /velodyne_points - Use Fixed Frame: true - Use rainbow: true + Marker Topic: /vector_map + Name: MarkerArray + Namespaces: + cross walk: true + curb: true + gutter: true + lane: false + pole: true + road edge: true + road sign: true + road surface mark: true + signal: true + stop line: true + streetlight: true + utilitypole: true + white line: true + zebrazone: true + Queue Size: 100 Value: true - - Class: rviz/Image + - Class: rviz/Marker Enabled: true - Image Topic: /image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw + Marker Topic: /cubic_splines_viz + Name: Marker + Namespaces: + "": true + Queue Size: 100 Value: true - Class: rviz/Marker Enabled: true - Marker Topic: /vscan_linelist + Marker Topic: /sim_prius_model Name: Marker Namespaces: - vscan_linelist: true + sim_prius_model: true Queue Size: 100 Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: velodyne + Fixed Frame: world Frame Rate: 30 Name: root Tools: @@ -121,33 +97,28 @@ Visualization Manager: Value: true Views: Current: - Class: rviz/Orbit - Distance: 33.9424 + Angle: 0 + Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Focal Point: - X: 28.3026 - Y: 6.95075 - Z: -3.52082 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.209802 - Target Frame: - Value: Orbit (rviz) - Yaw: 3.31858 + Scale: 10 + Target Frame: sim_base_link + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 Saved: ~ Window Geometry: Displays: - collapsed: false - Height: 1374 - Hide Left Dock: false + collapsed: true + Height: 1026 + Hide Left Dock: true Hide Right Dock: true - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000032e000004affc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc00000043000004af0000010001000021fa000000020100000003fb0000000a0049006d0061006700650100000000ffffffff0000000000000000fb0000000a0049006d0061006700650100000000ffffffff0000005b00fffffffb000000100044006900730070006c00610079007301000000000000013c0000010f00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000044fc0100000002fb0000000800540069006d0065010000000000000a000000025800fffffffb0000000800540069006d00650100000000000004500000000000000000000006cc000004af00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000032e00000372fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc00000028000003720000000000fffffffa000000000100000003fb0000000a0049006d0061006700650100000000ffffffff0000000000000000fb0000000a0049006d0061006700650100000000ffffffff0000000000000000fb000000100044006900730070006c00610079007300000000000000013c0000010f00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000372000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064f00000044fc0100000002fb0000000800540069006d006501000000000000064f000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000064f0000037200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -156,6 +127,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 2560 - X: -10 - Y: 19 + Width: 1615 + X: 55 + Y: -14 diff --git a/ros/src/computing/planning/motion/packages/driving_planner/CMakeLists.txt b/ros/src/computing/planning/motion/packages/driving_planner/CMakeLists.txt index cb37be88cff..86825c1ef3f 100644 --- a/ros/src/computing/planning/motion/packages/driving_planner/CMakeLists.txt +++ b/ros/src/computing/planning/motion/packages/driving_planner/CMakeLists.txt @@ -1,121 +1,86 @@ cmake_minimum_required(VERSION 2.8.3) project(driving_planner) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp + std_msgs + pcl_ros + pcl_conversions + sensor_msgs + tf + gnss + message_generation + runtime_manager waypoint_follower ) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - +################################################ +## Find OpenMP in order to parallelize loops ## +################################################ -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +OPTION (USE_OpenMP "Use OpenMP" ON) +IF(USE_OpenMP) + FIND_PACKAGE(OpenMP) + IF(OPENMP_FOUND) + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + ENDIF() +ENDIF() ################################################ ## Declare ROS messages, services and actions ## ################################################ -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) +add_message_files( + FILES + lane.msg + waypoint.msg + dtlane.msg +) -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) ################################### ## catkin specific configuration ## ################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES driving_planner - CATKIN_DEPENDS roscpp waypoint_follower -# DEPENDS system_lib + INCLUDE_DIRS include + LIBRARIES libtraj_gen + CATKIN_DEPENDS roscpp std_msgs tf runtime_manager waypoint_follower + DEPENDS gnss ) ########### ## Build ## ########### -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) +SET(CMAKE_CXX_FLAGS "-std=c++0x -O2 -g -Wall -Wno-unused-result -DROS ${CMAKE_CXX_FLAGS}") + include_directories( + include ${catkin_INCLUDE_DIRS} ) -## Declare a cpp library -# add_library(driving_planner -# src/${PROJECT_NAME}/driving_planner.cpp -# ) - -## Declare a cpp executable -# add_executable(driving_planner_node src/driving_planner_node.cpp) -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(driving_planner_node driving_planner_generate_messages_cpp) +add_library(libtraj_gen lib/libtraj_gen.cpp) +target_link_libraries(libtraj_gen ${catkin_LIBRARIES} /usr/lib/libarmadillo.so) -## Specify libraries to link a library or executable target against -# target_link_libraries(driving_planner_node -# ${catkin_LIBRARIES} -# ) +add_executable(lattice_trajectory_gen nodes/lattice_trajectory_gen/lattice_trajectory_gen.cpp) +target_link_libraries(lattice_trajectory_gen gnss libwaypoint_follower libtraj_gen ${catkin_LIBRARIES}) +add_dependencies(lattice_trajectory_gen +waypoint_follower_generate_messages_cpp +runtime_manager_generate_messages_cpp) +add_executable(lattice_twist_convert nodes/lattice_twist_convert/lattice_twist_convert.cpp) +target_link_libraries(lattice_twist_convert libwaypoint_follower libtraj_gen ${catkin_LIBRARIES}) +add_dependencies(lattice_twist_convert +waypoint_follower_generate_messages_cpp) - ############# ## Install ## diff --git a/ros/src/computing/planning/motion/packages/driving_planner/include/libtraj_gen.h b/ros/src/computing/planning/motion/packages/driving_planner/include/libtraj_gen.h new file mode 100755 index 00000000000..c1512241b59 --- /dev/null +++ b/ros/src/computing/planning/motion/packages/driving_planner/include/libtraj_gen.h @@ -0,0 +1,236 @@ +/* + * libtraj_gen.h + * Trajectory Generator Library + * + * Created by Matthew O'Kelly on 7/17/15. + * Copyright (c) 2015 Matthew O'Kelly. All rights reserved. + * mokelly@seas.upenn.edu + * +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * 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. + * + * * Neither the name of Autoware nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * 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. + * +*/ + +#ifndef TRAJECTORYGENERATOR_H +#define TRAJECTORYGENERATOR_H + +// ---------DEFINE MODE---------// +//#define GEN_PLOT_FILES +//#define DEBUG_OUTPUT +//#define DEBUG +//#define ONE_ITER +//#define STANDALONE +#define USE_HEURISTIC_INIT +#define CUBIC_STABLE + +// --------UNSTABLE MODES-------// +//#define QUINTIC_STABLE +//#define FIRST ORDER + + +// ------------BOOLEAN----------// +#define TRUE 1 +#define FALSE 0 + +// ------------CONSTANTS----------// +// Constants for forward simulation of ego vehicle +// Maximum curvature (radians) +#define kmax (0.1900) +// Minimum curvature (radians) +#define kmin (-0.1900) +// Maximum rate of curvature (radians/second) +#define dkmax (0.1021) +// Minimum rate of curvature (radians/second) +#define dkmin (-0.1021) +// Maximum acceleration (meters/second^2) +#define dvmax (2.000) +// Maximum deceleration (meters/second^2) +#define dvmin (-6.000) +// Control latency (seconds) +#define tdelay (0.0800) + //#define tdelay (0.03) +// Speed control logic a coefficient +#define ascl (0.1681) +// Speed control logic b coefficient +#define bscl (-0.0049) +// Speed control logic threshold (meters/second) +#define vscl (4.000) +// Max curvature for speed (radians) +#define kvmax (0.1485) +// Speed control logic safety factor +#define sf (1.000) + +// ------------TERMINATION CRITERIA----------// +// User defined allowable errors for goal state approximation +// Allowable crosstrack error (meters) +#define crosstrack_e (0.001) +// Allowable inline error (meters) +#define inline_e (0.001) +// Allowable heading error (radians) +#define heading_e (0.1) +// Allowable curvature error (meters^-1) +#define curvature_e (0.005) +// General error, ill-defined heuristic (unitless) +#define general_e (0.05) + +// ------------PARAMETER PERTURBATION----------// +// User defined perturbations for estimation of partial derivatives +// Perturbation for a +#define h_sx (0.001) +// Perturbation for b +#define h_sy (0.001) +// Perturbation for d +#define h_theta (0.001) +// Perturbation for s +#define h_k (0.001) +// If all parameters are perturbed equally, heuristic (unitless) +#define h_global (0.001) + +// --------INTEGRATION STEP SIZE CONTROL------// +// Set time step +#define step_size (0.0001) +// Set lightweight timestep for plotting, used in genLineStrip +#define plot_step_size (0.1) + +//#define step_size (0.05) + +// ------------LOG FILES----------// +// Open files for data logging, define globally so all functions may access: +using namespace std; +ofstream fmm_sx; +ofstream fmm_sy; +ofstream fmm_v; +ofstream fmm_theta; +ofstream fmm_kappa; + +// --------DATA STRUCTURES-------// +union State +{ + struct + { + double sx; + double sy; + double theta; + double kappa; + double v; + double vdes; + double timestamp; + }; + + double state_value[7]; +}; + +union Parameters +{ + struct + { + double a; + double b; + double c; + double d; + double s; + }; + + double param_value[5]; + + +}; + +union Spline +{ + struct + { + //double kappa_0; + double s; + double kappa_1; + double kappa_2; + //double kappa_2; + //double kappa_3; + + double kappa_0; + double kappa_3; + bool success; + + }; + + double spline_value[6]; +}; + +union Command +{ + struct + { + double kappa; + double v; + }; + double cmd_index[2]; +}; + + +// ------------FUNCTION DECLARATIONS----------// + +// initParams is used to generate the initial guess for the trajectory +union Spline initParams(union State veh, union State goal); + +// speedControlLogic prevents the vehicle from exceeding dynamic limits +union State speedControlLogic(union State veh_next); + +// responseToControlInputs computes the vehicles next state consider control delay +union State responseToControlInputs(union State veh, union State veh_next, double dt); + +// getCurvatureCommand computes curvature based on the selection of trajectory parameters +double getCurvatureCommand(union Spline curvature, double dt, double v, double t); + +// getVelocityCommand computes the next velocity command, very naieve right now. +double getVelocityCommand(double v_goal, double v); + +// motionModel computes the vehicles next state, it calls speedControlLogic, responseToControlInputs, getCurvatureCommand and +// and getVelocityCommand +union State motionModel(union State veh, union State goal, union Spline curvature, double dt, double horizon, int flag); + +// checkConvergence determines if the current final state is close enough to the goal state +bool checkConvergence(union State veh_next, union State goal); + +// pDerivEstimate computes one column of the Jacobian +union State pDerivEstimate(union State veh, union State veh_next, union State goal, union Spline curvature, int p_id, double h, double dt, double horizon, int stateIndex); + +// generateCorrection inverts the Jacobian and updates the spline parameters +union Spline generateCorrection(union State veh, union State veh_next, union State goal, union Spline curvature, double dt, double horizon); + +// nextState is used by the robot to compute commands once an adequate set of parameters has been found +union State nextState(union State veh, union Spline curvature, double vdes, double dt, double elapsedTime); + +// trajectoryGenerator is like a "main function" used to iterate through a series of goal states +union Spline trajectoryGenerator(double sx, double sy, double theta, double v, double kappa); + +// plotTraj is used by rViz to compute points for line strip, it is a lighter weight version of nextState +union State genLineStrip(union State veh, union Spline curvature, double vdes, double t); + + +#endif // TRAJECTORYGENERATOR_H + diff --git a/ros/src/computing/planning/motion/packages/driving_planner/launch/lattice_trajectory_gen.launch b/ros/src/computing/planning/motion/packages/driving_planner/launch/lattice_trajectory_gen.launch new file mode 100644 index 00000000000..e7a43c5c162 --- /dev/null +++ b/ros/src/computing/planning/motion/packages/driving_planner/launch/lattice_trajectory_gen.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/ros/src/computing/planning/motion/packages/driving_planner/lib/libtraj_gen.cpp b/ros/src/computing/planning/motion/packages/driving_planner/lib/libtraj_gen.cpp new file mode 100755 index 00000000000..b9548dfeeed --- /dev/null +++ b/ros/src/computing/planning/motion/packages/driving_planner/lib/libtraj_gen.cpp @@ -0,0 +1,889 @@ +/* + * libtraj_gen.cpp + * Trajectory Generator Library + * + * Created by Matthew O'Kelly on 7/17/15. + * Copyright (c) 2015 Matthew O'Kelly. All rights reserved. + * mokelly@seas.upenn.edu + * +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * 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. + * + * * Neither the name of Autoware nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * 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. + * +*/ + + + +/* + * This file contains the vehicle model for use in predicting the ego vehicle's motion + * It also contains a heuristic for initial trajetories and + * a gradient descent method for refining them... + * + * First some constants are defined, these will be specific to your vehicle + * Nine functions are included: speedControlLogic(), + * responsetoControlInputs(), and most + * importantly, the vehicle's motionModel() etc... + * + * motionModel() calls responseToControlInputs() + * responseToControlInputs calls speedControlLogic() + * + * For a standalone build use the following command: g++ trajectorygenerator.cpp -o trajectorygenerator -O2 -larmadillo + * Dependancies: Armadillo, http://arma.sourceforge.net/ + * +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "libtraj_gen.h" +// #include "trajectorygenerator.h" + + + +using namespace std; + + + +//-----------------------------FUNCTIONS------------------------------------// + +// ------------INITIALIZE PARAMETERS----------// +// Heuristic to compute initial paramters given +// INPUT: Initial state, and a goal state +// OUTPUT: Estimated control parameters +// Use #define USE_HEURISTIC_INIT if using Nagy 2001 method +// This is necessary for cubic splines +// Use #define FIRST_ORDER if working with first order splines + +union Spline initParams(union State veh, union State goal) +{ + + // Local variables for init and goal: + double sx_f = goal.sx; + double sy_f = goal.sy; + double theta_f = goal.theta; + double kappa_0 = veh.kappa; + double kappa_f = goal.kappa; + + // Initialize output + union Spline curvature; + + // Convenience + double d_theta = abs(theta_f); + + // Heuristic for initial guess from Nagy and Kelly, 2001 + // 'Trajectory Generation for Car-like Robots using + // Cubic Curvature Polynomials' + #ifdef USE_HEURISTIC_INIT + // Order of computation matters, need to compute d before s and so on... + + // Commented out, because unused for cubic splines + //double c = 0.0; + + // Note we must cast to double to avoid ambiguity + double d = sqrt((double)pow(sx_f,2) +(double) pow(sy_f,2)); + + double s = d * ((pow(d_theta,2))/5.0 + 1.0) + (2.0/5.0)*d_theta; + + // Commented out because unused for cubic splines + //double a = (6* (theta_f/(pow(s,2)))) - (2*kappa_0/s) + (4*kappa_f/s); + + double b = (3/(pow(s,2))) * (kappa_0 + kappa_f) + (6*theta_f/(pow(s,3))); + + double si=0.00; + curvature.kappa_0 = veh.kappa; + curvature.kappa_1=(1.00/49.00)*(8.00*b*si - 8.00*b*curvature.s - 26.00*curvature.kappa_0 - curvature.kappa_3); + curvature.kappa_2=0.25*(curvature.kappa_3 -2.00*curvature.kappa_0 +5.00*curvature.kappa_1); + curvature.kappa_3 = goal.kappa; + curvature.s = s; + + #endif + + // Alternative heuristic initial guess for first order spline + // From Thomas Howard's dissertation page 38 + #ifdef FIRST_ORDER + double d = sqrt((double)pow(sx_f,2) +(double) pow(sy_f,2)); + double s = d * ((pow(d_theta,2))/5 + 1) + (2/5) *d_theta; + union Spline curvature; + double si=0.00; + curvature.kappa_0 = veh.kappa; + curvature.kappa_1 = 0.00; + curvature.kappa_2 = 0.00; + curvature.kappa_3 = goal.kappa; + curvature.s = s; + #endif + + curvature.success=TRUE; + + // Return the initialized curvature union + return curvature; + +} + + +// ------------SPEED CONTROL----------// +// Function for computing safe (feasible) speed and curvature +// INPUT: Next vehicle state +// OUTPUT: Updated next vehicle state + +union State speedControlLogic(union State veh_next) +{ + + // Calculate speed (look up from next state vector) + double vcmd = abs(veh_next.v); + double kappa_next = veh_next.kappa; + + // Compute safe speed + double compare_v=(kappa_next-ascl)/bscl; + double vcmd_max = max((double)vscl, compare_v); + + // Compute safe curvature + double compare_kappa = ascl + (bscl * vcmd); + double kmax_scl = min((double) kmax, compare_kappa); + + // Check if max curvature for speed is exceeded + if(kappa_next >= kmax_scl) + { + // Check for safe speed + vcmd = sf * vcmd_max; + } + + // Update velocity command, this is not quite equivalent to Ferguson, Howard, & Liukhachev + veh_next.v = vcmd; + + // Return vehicle state + return veh_next; +} + +// ------------RESPONSE TO CONTROL----------// +// Function for computing the vehicles response to control inputs +// Mainly deals with delays associated with controls +// Also considers upper and lower safety bounds +// INPUT: Current vehicle state, next vehicle state, sampling time +// OUTPUT: Next vehicle state + +union State responseToControlInputs(union State veh, union State veh_next, double dt) +{ + + // Local variables: + double kappa = veh.kappa; + double kappa_next = veh_next.kappa; + double v = veh.v; + double v_next = veh_next.v; + + // Compute curvature rate command + double kdot = (kappa_next - kappa)/dt; + + // Check against upper bound on curvature rate + kdot = min(kdot, (double) dkmax); + + // Check against lower bound on curvature rate + kdot = max(kdot, (double) dkmin); + + // Call the speedControlLogic function + veh_next = speedControlLogic(veh_next); + + // Compute curvature at the next vehicle state + kappa_next = kappa + kdot*dt; + + // Check upper bound on curvature + kappa_next = min(kappa_next, (double) kmax); + + // Check lower bound on curvature + kappa_next = max(kappa_next, (double) kmin); + + // Compute acceleration command + double vdot = (v_next -v)/dt; + + // Check for upper bound on acceleration + vdot = min(vdot, (double) dvmax); + + // Check for lower bound on acceleration + vdot = max(vdot, (double) dvmin); + + // Compute velocity at next state + veh_next.v = v + vdot*dt; + + // Return next vehicle state + return veh_next; +} + +// ------------COMPUTE CURVATURE----------// +// Computes the curvature at the next state +// Still messy but will clean shortly... + +double getCurvatureCommand(union Spline curvature, double dt, double v, double t) +{ + // Local variables for curvature constants + double kappa_0 = curvature.kappa_0; + double kappa_1 = curvature.kappa_1; + double kappa_2 = curvature.kappa_2; + double kappa_3 = curvature.kappa_3; + double s = curvature.s; + + // Init next command + double k_next_cmd =0.0; + + // Assume position at t=0, si=0 + double si = 0.0; + + // Estimate distance traveled on spiral + double st = v*t; + + // These equations can be found in McNaughton, page 76 + // They make the knot points equally spaced. + // They apply to stable cubic paths + #ifdef CUBIC_STABLE + double a =0; + double b =0; + double c= 0; + double d = 0; + a = kappa_0; + b = (-0.50)*(-2*kappa_3 + 11*kappa_0 - 18*kappa_1 + 9*kappa_2)/(s-si); + c = (4.50)*(-kappa_3 + 2*kappa_0 - 5*kappa_1 +4*kappa_2)/(pow((s-si), 2)); + d = (-4.50)*(-kappa_3 + kappa_0 - 3*kappa_1 + 3*kappa_2)/(pow((s-si), 3)); + k_next_cmd = a + b*st + c*pow(st,2) + d*pow(st,3); + #endif + + // These equations can be found in McNaughton, page 77 + // They make the knot points equally spaced. + // They apply to stable quintic paths + #ifdef QUINTIC_STABLE + double a = kappa_0; + double b = kappa_1; + double c = kappa_2/2.0; + double d = -(575*kappa_0 -648*kappa_3 +81*kappa_4 -8*kappa_5 +170*kappa_1*st +22*kappa_2*pow(st,2)/ (8*pow(st,3)); + double e = 9*(37*kappa_0 -45*kappa_3 +9*kappa_4 -kappa_5 +10*kappa_1*st +kappa_2*pow(st,2))/(2*pow(st,4)); + double f = -9*(85*kappa_0 -108*kappa_3 +27*kappa_4 -4*kappa_5 +22*kappa_1*st +2*kappa_2*pow(st,2))/(8*pow(st,5)); + k_next_cmd = a + b*st + c*pow(st,2) + d*pow(st,3) + e*pow(st,4) +f*pow(st,5); + #endif + + // Compute next command from Howard page 39 + #ifdef FIRST_ORDER + k_next_cmd = kappa_0 + ((kappa_1 - kappa_0)/s) * (st); + #endif + + // Return value + return k_next_cmd; + +} + +// ------------COMPUTE VELOCITY----------// +// Updates the vehicles velocity based on velocity profile +// Currently assumes constant +/// Will update to include ramp... + +double getVelocityCommand(double v_goal, double v) +{ + #ifdef DEBUG + cout << "Function: getVelocityCommand()"<< endl; + #endif + // Some function based on parameterization of velocity profile + // Set to constant for now + double v_next_cmd=v_goal; + return v_next_cmd; +} + +// ------------MOTION MODEL----------// +// Computes update to vehicle state +// INPUT: Current vehicle state, parameterized control inputs, sampling time +// OUTPUT: Next vehicle state + +union State motionModel(union State veh, union State goal, union Spline curvature, double dt, double horizon, int flag) +{ + // Initialized the elapsed time to 0.0 s + double t =0.0; + // Setup local data structures for holding + // the previous and next vehicle states + union State veh_next; + union State veh_temp; + // Set veh_temp to the current state + veh_temp=veh; + // Compute the stop time for the simulation + horizon = curvature.s/goal.v; + + while(t < horizon) + { + // Read state vector + double sx = veh_temp.sx; + double sy = veh_temp.sy; + double v = veh_temp.v; + double theta = veh_temp.theta; + double kappa = veh_temp.kappa; + double v_goal= goal.v; + + // Compute change in 2D x-position (this is x_dot) + double sx_next = sx + (v * cos(theta) * dt); + + veh_next.sx = sx_next; + + + // Compute change in 2D y-position (this is y_dot) + double sy_next = sy + (v * sin(theta) * dt); + + veh_next.sy = sy_next; + + // Compute change in 2D orientation (this is theta_dot) + double theta_next = theta + (v * kappa * dt); + + veh_next.theta = theta_next; + + // Get curvature command + double kappa_next = getCurvatureCommand(curvature, dt , veh.v,t); + + veh_next.kappa = kappa_next; + + // Get velocity command + double v_next = getVelocityCommand(v_goal, v); + + veh_next.v = v_next; + + // Get acceleration command, not used yet... + //double a_next_cmd = 0; + + + // Update the next vehicle state + veh_next = responseToControlInputs(veh, veh_next, dt); + + // Increment the timestep + t=t+dt; + + // Update veh_temp + veh_temp=veh_next; + + // Write to log file if we are testing + if(flag==1) + { + fmm_sx < +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "runtime_manager/ConfigLaneFollower.h" +#include "waypoint_follower/libwaypoint_follower.h" +#include "libtraj_gen.h" +#include "vehicle_socket/CanInfo.h" + +#define DEBUG_TRAJECTORY_GEN +#define WHEEL_ANGLE_MAX (31.28067) +#define WHEEL_TO_STEERING (STEERING_ANGLE_MAX/WHEEL_ANGLE_MAX) +#define STEERING_ANGLE_MAX (666.00000) +#define WHEEL_BASE (2.70) + +#define DEBUG_TRAJECTORY_GEN + +static const int LOOP_RATE = 10; //Hz +static const double LOOK_AHEAD_THRESHOLD_CALC_RATIO = 3.0; // the next waypoint must be outside of this threshold. +static const double MINIMUM_LOOK_AHEAD_THRESHOLD = 4.0; // the next waypoint must be outside of this threshold. +static const double EVALUATION_THRESHOLD = 1.0; //meter +static const std::string MAP_FRAME = "map"; +static const std::string SIM_BASE_FRAME = "sim_base_link"; +static const std::string BASE_FRAME = "base_link"; +static ros::Publisher marker_pub; +static int SPLINE_INDEX=0; + +//define class +class PathPP: public Path +{ +private: + int next_waypoint_; + + int param_flag_; //0 = waypoint, 1 = Dialog + double lookahead_threshold_; //meter + double initial_velocity_; //km/h + +public: + PathPP() + { + param_flag_ = 0; + lookahead_threshold_ = 4.0; + initial_velocity_ = 5.0; + next_waypoint_ = -1; + } + void setConfig(const runtime_manager::ConfigLaneFollowerConstPtr &config); + double getCmdVelocity(); + double getLookAheadThreshold(int waypoint); + int getNextWaypoint(); + double calcRadius(int waypoint); + bool evaluateWaypoint(int next_waypoint); + void displayTrajectory(tf::Vector3 center, double radius); + double getNextWaypointVelocity(int next_waypoint); + union Spline waypointTrajectory(union State veh, union State goal, union Spline curvature, int next_waypoint); + union State computeWaypointGoal(int next_waypoint); + union State computeVeh(int old_time, double old_theta, int next_waypoint); + void drawSpline(union Spline curvature, union State veh, int flag, int selected); + + +}; +PathPP _path_pp; + +static bool _sim_mode = false; +static geometry_msgs::PoseStamped _current_pose; // current pose by the global plane. +static double _current_velocity; +static double _current_angular_velocity; +static double _can_info_curvature; +static double _prev_velocity = 0; +static ros::Publisher _vis_pub; +static ros::Publisher _traj_pub; +static ros::Publisher _stat_pub; +static bool _waypoint_set = false; +static bool _pose_set = false; + +void PathPP::setConfig(const runtime_manager::ConfigLaneFollowerConstPtr &config) +{ + initial_velocity_ = config->velocity; + param_flag_ = config->param_flag; + lookahead_threshold_ = config->lookahead_threshold; +} + +//get velocity of the closest waypoint or from config +double PathPP::getCmdVelocity() +{ + + if (param_flag_) + { + ROS_INFO_STREAM("dialog : " << initial_velocity_ << " km/h (" << kmph2mps(initial_velocity_) << " m/s )"); + return kmph2mps(initial_velocity_); + } + if (current_path_.waypoints.empty()) + { + return 0; + } + + double velocity = current_path_.waypoints[closest_waypoint_].twist.twist.linear.x; + return velocity; +} + +double PathPP::getNextWaypointVelocity(int next_waypoint) +{ + if (current_path_.waypoints.empty()) + { + ROS_INFO_STREAM("waypoint : not loaded path"); + return 0; + } + + double velocity = current_path_.waypoints[next_waypoint].twist.twist.linear.x; + return velocity; +} + +double PathPP::getLookAheadThreshold(int waypoint) +{ + if (param_flag_) + return lookahead_threshold_; + + double current_velocity_mps = current_path_.waypoints[waypoint].twist.twist.linear.x; + if (current_velocity_mps * LOOK_AHEAD_THRESHOLD_CALC_RATIO < MINIMUM_LOOK_AHEAD_THRESHOLD) + return MINIMUM_LOOK_AHEAD_THRESHOLD; + else if (getDistance(waypoint) < 0.5) + return current_velocity_mps * LOOK_AHEAD_THRESHOLD_CALC_RATIO; + else + return current_velocity_mps * LOOK_AHEAD_THRESHOLD_CALC_RATIO + getDistance(waypoint) * 3.0; +} + +///////////////////////////////////////////////////////////////// +// obtain the next "effective" waypoint. +// the vehicle drives itself toward this waypoint. +///////////////////////////////////////////////////////////////// +int PathPP::getNextWaypoint() +{ + // if waypoints are not given, do nothing. + if (!getPathSize()) + return -1; + + double lookahead_threshold = getLookAheadThreshold(closest_waypoint_); + + //ROS_INFO_STREAM("threshold = " << lookahead_threshold); + // look for the next waypoint. + for (int i = closest_waypoint_; i < getPathSize(); i++) + { + + //if threshold is distance of previous waypoint + if (next_waypoint_ > 0) + { + if (getDistance(next_waypoint_) > lookahead_threshold) + { + //ROS_INFO_STREAM("threshold = " << lookahead_threshold); + return next_waypoint_; + } + } + + // if there exists an effective waypoint + if (getDistance(i) > lookahead_threshold) + { + + //if param flag is waypoint + if (!param_flag_) + { + + if (evaluateWaypoint(i) == true) + { + //ROS_INFO_STREAM("threshold = " << lookahead_threshold); + next_waypoint_ = i; + return i; + } + else + { + //restart search from closest_waypoint + i = closest_waypoint_; + + //threshold shortening + if (lookahead_threshold > MINIMUM_LOOK_AHEAD_THRESHOLD) + { + // std::cout << "threshold correction" << std::endl; + lookahead_threshold -= lookahead_threshold / 10; + //ROS_INFO_STREAM("fixed threshold = " << lookahead_threshold); + } + else + { + lookahead_threshold = MINIMUM_LOOK_AHEAD_THRESHOLD; + } + } + } + else + { + //ROS_INFO_STREAM("threshold = " << lookahead_threshold); + next_waypoint_ = i; + return i; + } + } + } + + // if the program reaches here, it means we lost the waypoint. + return -1; +} + +bool PathPP::evaluateWaypoint(int next_waypoint) +{ + + double radius = calcRadius(next_waypoint); + // std::cout << "radius "<< radius << std::endl; + if (radius < 0) + radius = (-1) * radius; + + tf::Vector3 center; + + //calculate circle of trajectory + if (transformWaypoint(next_waypoint).getY() > 0) + center = tf::Vector3(0, 0 + radius, 0); + else + center = tf::Vector3(0, 0 - radius, 0); + + displayTrajectory(center, radius); + + //evaluation + double evaluation = 0; + for (int j = closest_waypoint_ + 1; j < next_waypoint; j++) + { + tf::Vector3 tf_waypoint = transformWaypoint(j); + tf_waypoint.setZ(0); + double dt_diff = fabs(tf::tfDistance(center, tf_waypoint) - fabs(radius)); + // std::cout << dt_diff << std::endl; + if (dt_diff > evaluation) + evaluation = dt_diff; + } + + if (evaluation < EVALUATION_THRESHOLD) + return true; + else + return false; + +} + +// display the trajectory by markers. +void PathPP::displayTrajectory(tf::Vector3 center, double radius) +{ + + geometry_msgs::Point point; + tf::Vector3 inv_center = transform_.inverse() * center; + + point.x = inv_center.getX(); + point.y = inv_center.getY(); + point.z = inv_center.getZ(); + + visualization_msgs::Marker traj; + traj.header.frame_id = MAP_FRAME; + traj.header.stamp = ros::Time(); + traj.ns = "trajectory_marker"; + traj.id = 0; + traj.type = visualization_msgs::Marker::SPHERE; + traj.action = visualization_msgs::Marker::ADD; + traj.pose.position = point; + traj.scale.x = radius * 2; + traj.scale.y = radius * 2; + traj.scale.z = 1.0; + traj.color.a = 0.3; + traj.color.r = 1.0; + traj.color.g = 0.0; + traj.color.b = 0.0; + traj.frame_locked = true; + _traj_pub.publish(traj); +} + +double PathPP::calcRadius(int waypoint) +{ + return pow(getDistance(waypoint), 2) / (2 * transformWaypoint(waypoint).getY()); +} + +static void ConfigCallback(const runtime_manager::ConfigLaneFollowerConstPtr config) +{ + _path_pp.setConfig(config); +} + +static void OdometryPoseCallback(const nav_msgs::OdometryConstPtr &msg) +{ + if (_sim_mode) + { + _current_velocity = msg->twist.twist.linear.x; + _current_angular_velocity = msg->twist.twist.angular.z; + _current_pose.header = msg->header; + _current_pose.pose = msg->pose.pose; + + tf::Transform inverse; + tf::poseMsgToTF(msg->pose.pose, inverse); + _path_pp.setTransform(inverse.inverse()); + _pose_set = true; + // std::cout << "transform2 (" << _transform2.getOrigin().x() << " " << _transform2.getOrigin().y() << " " << _transform2.getOrigin().z() << ")" << std::endl; + } + +} + +static void NDTCallback(const geometry_msgs::PoseStampedConstPtr &msg) +{ + if (!_sim_mode) + { + _current_pose.header = msg->header; + _current_pose.pose = msg->pose; + tf::Transform inverse; + tf::poseMsgToTF(msg->pose, inverse); + _path_pp.setTransform(inverse.inverse()); + _pose_set = true; + } +} + +static void estVelCallback(const std_msgs::Float32ConstPtr &msg) +{ + //_current_velocity = msg->data; +} + +static void WayPointCallback(const waypoint_follower::laneConstPtr &msg) +{ + _path_pp.setPath(msg); + _waypoint_set = true; + ROS_INFO_STREAM("waypoint subscribed"); +} + +static void canInfoCallback(const vehicle_socket::CanInfoConstPtr &msg) +{ + double steering_wheel_angle = msg->angle; + _current_velocity = (msg->speed)*(1000.00/3600); + steering_wheel_angle = steering_wheel_angle*(3.1496/180.00); + _can_info_curvature = (steering_wheel_angle / (double) WHEEL_TO_STEERING) / WHEEL_BASE; + ROS_INFO_STREAM("Steering Wheel Angle: "< 0 || radius < 0) + { + twist.angular.z = current_velocity / radius; + } + else + { + twist.angular.z = 0; + } + return twist; +} + + + +///////////////////////////////////////////////////////////////// +// Safely stop the vehicle. +///////////////////////////////////////////////////////////////// +static geometry_msgs::Twist stopControl() +{ + geometry_msgs::Twist twist; + + double lookahead_distance = _path_pp.getDistance(_path_pp.getPathSize() - 1); + + double stop_interval = 3; + if (lookahead_distance < stop_interval) + { + twist.linear.x = 0; + twist.angular.z = 0; + return twist; + } + + twist.linear.x = DecelerateVelocity(lookahead_distance, _prev_velocity); + + if (twist.linear.x < 1.0) + { + twist.linear.x = 0; + } + + double radius = _path_pp.calcRadius(_path_pp.getPathSize() - 1); + + if (radius > 0 || radius < 0) + { + twist.angular.z = twist.linear.x / radius; + } + else + { + twist.angular.z = 0; + } + return twist; + +} + +////////////////////////////////////////////////////////////////////////////////////////////////// +// M. O'Kelly code begins here, +// Suggested: need to clean up unused functions above... don't have time. +///////////////////////////////////////////////////////////////////////////////////////////////// + +///////////////////////////////////////////////////////////////// +// Compute the goal state of the vehicle +///////////////////////////////////////////////////////////////// +union State computeWaypointGoal(int next_waypoint) +{ + union State l_goal; + + // Get the next waypoint position with respect to the vehicles frame + l_goal.sx = _path_pp.transformWaypoint(next_waypoint).getX(); + l_goal.sy = _path_pp.transformWaypoint(next_waypoint).getY(); + + // Get the next next waypoint position + double waypoint_lookahead_1_x = _path_pp.transformWaypoint(next_waypoint+1).getX(); + double waypoint_lookahead_1_y = _path_pp.transformWaypoint(next_waypoint+1).getY(); + + // Get the next next next waypoint + double waypoint_lookahead_2_x = _path_pp.transformWaypoint(next_waypoint+2).getX(); + double waypoint_lookahead_2_y = _path_pp.transformWaypoint(next_waypoint+2).getY(); + + // Compute dX and dY relative to lookahead + double dX_1 = waypoint_lookahead_1_x - l_goal.sx; + double dY_1 = waypoint_lookahead_1_y - l_goal.sy; + + // Compute dX and dY relative to lookahead and next lookahead + double dX_2 = waypoint_lookahead_2_x - waypoint_lookahead_1_x; + double dY_2 = waypoint_lookahead_2_y - waypoint_lookahead_1_y; + + // Estimate desired orientation of the vehicle at next waypoint + l_goal.theta = atan(dY_1/dX_1); + + // Should we use lookahead and next waypoint + // or current and next as below: + //l_goal.theta = atan(l_goal.sy/l_goal.sx); + + + // Estimate the desired orientation at the next next waypoint + double theta_lookahead = atan(dY_2/dX_2); + + // Estimate the arc length + double ds = sqrt(pow(waypoint_lookahead_1_x - waypoint_lookahead_2_x, 2) + pow(waypoint_lookahead_1_x - waypoint_lookahead_2_y, 2)); + + // Angle + double angle = (theta_lookahead-l_goal.theta)/2.00; + + // Estimate Kappa + double estimate = 2.00*sin(angle)/ds; + + l_goal.kappa = estimate; + + // Note we limit kappa from being too extreme + // 10.0 was arbitrary, we really need a better curvature estimate + l_goal.kappa = min((double)kmax/10.0, l_goal.kappa); + + l_goal.kappa = max ((double)kmin/10.0, l_goal.kappa); + + // Get the desired velocity at the next waypoint + l_goal.v = _path_pp.getNextWaypointVelocity(next_waypoint); + + return l_goal; +} + +///////////////////////////////////////////////////////////////// +// Compute current state of the vehicle +///////////////////////////////////////////////////////////////// +union State computeVeh(int old_time, double old_theta, int next_waypoint) +{ + union State l_veh; + + // Goal is computed relative to vehicle coordinate frame + l_veh.sx=0.0; + l_veh.sy=0.0; + + // Compute yaw (orientation) relative to the world coordinate frame + // Necessary to compute curvature, but note that in the local coordinate frame yaw is still 0.0 + tf::Quaternion q(_current_pose.pose.orientation.x,_current_pose.pose.orientation.y,_current_pose.pose.orientation.z,_current_pose.pose.orientation.w); + tf::Matrix3x3 m(q); + double roll,pitch,yaw; + m.getRPY(roll,pitch,yaw); + + // Check this later: + //double local_theta=yaw; + + // Because we are on the coordinate system of the base frame + l_veh.theta = 0.0; + + // Get the current velocity of the vehicle + l_veh.v = _current_velocity; + + // Get the desired velocity + l_veh.vdes = _path_pp.getNextWaypointVelocity(next_waypoint); + ROS_INFO_STREAM("Desired Velocity: "<< l_veh.vdes); + + // Not using timing related stuff for now... + // In order to estimate the curvature we need to compute the arc length we have travelled on + // Thus we need to estimate dt + // l_veh.timestamp = _current_pose.header.stamp.nsec; + //double localElapsedTime=((double)l_veh.timestamp-old_time)/1000000000.00; + + // ds is the change in position along the arc + //double ds = localElapsedTime*veh.v; + + // Should get the current steering angle instead + // Steering angle ~= kappa + // Ask Ohta-san? + double w = _current_angular_velocity; + ROS_INFO_STREAM("Current omega: " <0) + { + line_strip.scale.x = 0.08; + line_strip.color.r = 1.0; + line_strip.color.a = 1.0; + } + + // Set the color and transparency (green and solid) if selected + else + { + line_strip.scale.x = 0.1; + line_strip.color.g= 1.0; + line_strip.color.a = 1.0; + } + + + // Init temp state for storing results of genLineStrip + union State temp; + + // Init time + double sim_time = 0.0; + + // Figure out sim time horizon + double horizon = curvature.s/veh.vdes; + + // Init points + geometry_msgs::Point p; + + // Create veritices + while(sim_time("twist_raw", 10); + _vis_pub = nh.advertise("next_waypoint_mark", 1); + _traj_pub = nh.advertise("trajectory_mark", 0); + _stat_pub = nh.advertise("wf_stat", 0); + + // Publish the curvature information: + ros::Publisher spline_parameters_pub = nh.advertise("spline", 10); + ros::Publisher state_parameters_pub = nh.advertise("state", 10); + + // Publish the trajectory visualization + marker_pub = nh.advertise("cubic_splines_viz", 10); + + // Subscribe to the following topics: + // waypoints, odometry, localization, velocity estimate, lane follower (MOK not sure of the purpose) + //ros::Subscriber waypoint_subcscriber = nh.subscribe("base_waypoint", 10, WayPointCallback); + ros::Subscriber waypoint_subcscriber = nh.subscribe("safety_waypoint", 10, WayPointCallback); + ros::Subscriber odometry_subscriber = nh.subscribe("odom_pose", 10, OdometryPoseCallback); + ros::Subscriber ndt_subscriber = nh.subscribe("control_pose", 10, NDTCallback); + ros::Subscriber estimated_vel_subscriber = nh.subscribe("estimated_vel", 10, estVelCallback); + //ros::Subscriber config_subscriber = nh.subscribe("config/lane_follower", 10, ConfigCallback); + ros::Subscriber config_subscriber = nh.subscribe("config/waypoint_follower", 10, ConfigCallback); + ros::Subscriber can_info = nh.subscribe("can_info", 10, canInfoCallback); + + // Local variable for geometry messages + geometry_msgs::TwistStamped twist; + + // Set the loop rate unit is Hz + ros::Rate loop_rate(LOOP_RATE); + + // Initialize endflag to false + bool endflag = false; + + // Set up arrays for perturb and flag to enable easy + // parallelization via OpenMP pragma + double perturb[30]; + perturb[0]=-3.00; + + int flag[30]; + flag[0]=1; + + for(int i=1; i<30; i++) + { + perturb[i] = perturb[i-1] + 0.2; + flag[i-1] = flag[i]+1; + } + bool initFlag = FALSE; + union Spline prev_curvature; + union State veh_fmm; + + // Here we go.... + while (ros::ok()) + { + std_msgs::Bool _lf_stat; + + ros::spinOnce(); + + // Wait for waypoints (in Runtime Manager) and pose to be set (in RViz) + if (_waypoint_set == false || _pose_set == false) + { + ROS_INFO_STREAM("topic waiting..."); + loop_rate.sleep(); + continue; + } + + // Get the closest waypoinmt + int closest_waypoint = _path_pp.getClosestWaypoint(); + static int prev_closest_waypoint = 0; + if (closest_waypoint < 0) + { + closest_waypoint = prev_closest_waypoint+5; + prev_closest_waypoint = closest_waypoint; + } + + + ROS_INFO_STREAM("closest waypoint = " << closest_waypoint); + + // Check to make sure that the vehicle has not reached the end + // of the mission + if (!endflag) + { + // If the current waypoint has a valid index + if (closest_waypoint > 0) + { + // Return the next waypoint + int next_waypoint = _path_pp.getNextWaypoint(); + ROS_INFO_STREAM("Next waypoint: "< 0) + { + // Display and publish waypoint information + displayNextWaypoint(next_waypoint); + _lf_stat.data = true; + _stat_pub.publish(_lf_stat); + + // Determine the desired state of the vehicle at the next waypoint + union State goal = computeWaypointGoal(next_waypoint); + + // Estimate the current state of the vehicle + union State veh = computeVeh(old_time, old_theta, next_waypoint); + + if(initFlag==TRUE && prev_curvature.success==TRUE) + { + veh_fmm = nextState(veh, prev_curvature, veh.vdes, 0.2, 0); + //veh.kappa = veh_fmm.kappa; + ROS_INFO_STREAM("est kappa: " <0) + { + drawSpline(curvature, veh, 0,0); + SPLINE_INDEX++; + ROS_INFO_STREAM("Spline published to RVIZ"); + } + + // This is a messy for loop which generates extra trajectories for visualization + // Likely will change when valid cost map arrives. + // Note: pragma indicates parallelization for OpenMP + + // Setup variables + union State tempGoal= goal; + int i; + union Spline extra; + + // Tell OpenMP how to parallelize (keep i private because it is a counter) + #pragma omp parallel for private(i) + + // Index through all the predefined perturbations from waypoint + for(i=1; i<31; i++) + { + // Shift the y-coordinate of the goal + tempGoal.sy = tempGoal.sy + perturb[i-1]; + + // Compute new spline + extra= waypointTrajectory(veh, tempGoal, curvature, next_waypoint); + + // Display trajectory + if(veh.v>5.00) + { + drawSpline(extra, veh, i,1); + } + } + + + // Update the elapsed time + //double elapsedTime=(veh.timestamp-old_time)/1000000000.00; + + // Update previous time and orientation measurements + old_time= veh.timestamp; + old_theta=veh.theta; + + } + + // If the next way point is not available + else + { + ROS_INFO_STREAM("Lost waypoint!"); + _lf_stat.data = false; + _stat_pub.publish(_lf_stat); + twist.twist.linear.x = 0; + twist.twist.angular.z = 0; + } + + // Check whether vehicle is close to the final destination + if (next_waypoint > _path_pp.getPathSize() - 5) + { + endflag = true; + } + } + + // Else if the closest waypoint is not detected + /*else + { + ROS_INFO_STREAM("Closest waypoint cannot be detected!"); + _lf_stat.data = false; + _stat_pub.publish(_lf_stat); + twist.twist.linear.x = 0; + twist.twist.angular.z = 0; + }*/ + + } + + // If endflag is true + else + { + twist.twist = stopControl(); + _lf_stat.data = false; + _stat_pub.publish(_lf_stat); + + // After stopped or fed out, let's get ready for the restart. + if (twist.twist.linear.x == 0) + { + ROS_INFO_STREAM("Trajectory generation ended!"); + _lf_stat.data = false; + _stat_pub.publish(_lf_stat); + endflag = false; + } + } + _prev_velocity = twist.twist.linear.x; + loop_rate.sleep(); + } + + return 0; +} diff --git a/ros/src/computing/planning/motion/packages/driving_planner/nodes/lattice_twist_convert/lattice_twist_convert.cpp b/ros/src/computing/planning/motion/packages/driving_planner/nodes/lattice_twist_convert/lattice_twist_convert.cpp new file mode 100644 index 00000000000..845c36d96a9 --- /dev/null +++ b/ros/src/computing/planning/motion/packages/driving_planner/nodes/lattice_twist_convert/lattice_twist_convert.cpp @@ -0,0 +1,227 @@ +/* + * command_converter.cpp + * Command Converter for Cubic Spline Trajectory Generation + * + * Created by Matthew O'Kelly on 7/17/15. + * Copyright (c) 2015 Matthew O'Kelly. All rights reserved. + * mokelly@seas.upenn.edu + * +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * 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. + * + * * Neither the name of Autoware nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * 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. + * +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "runtime_manager/ConfigLaneFollower.h" +#include "waypoint_follower/libwaypoint_follower.h" +#include "libtraj_gen.h" +#include + + +///////////////////////////////////////////////////////////////// +// Global variables +///////////////////////////////////////////////////////////////// + +// Set update rate +static const int LOOP_RATE = 15; //Hz + +// Next state time difference +static const double next_time = 1.00/LOOP_RATE; + +// Global vairable to hold curvature +union Spline curvature; + +// Global variable to hold vehicle state +union State veh; + +// Global var +union State veh_temp; + +// Global variable to keep track of when the last message was recieved: +double start_time; + +// Global variable keep track of current time in this node: +double current_time; + +// Global flag to indicate a new state +bool newState = FALSE; + +// Global flag to indicate a new spline +bool newSpline =FALSE; + + +///////////////////////////////////////////////////////////////// +// Callback function declarations +///////////////////////////////////////////////////////////////// + +// Callback function to get control parameters +void splineCallback(const std_msgs::Float64MultiArray::ConstPtr& spline); + +// Callback function to get state parameters +void stateCallback(const std_msgs::Float64MultiArray::ConstPtr& state); + + +///////////////////////////////////////////////////////////////// +// Main loop +///////////////////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + //fmm_omega.open("fmm_omega.dat"); + + ROS_INFO_STREAM("Command converter begin: "); + ROS_INFO_STREAM("Loop Rate: " << next_time); + + // Set up ROS + ros::init(argc, argv, "command_converter"); + + // Make handles + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + + // Publish the following topics: + // Commands + ros::Publisher cmd_velocity_publisher = nh.advertise("twist_raw", 10); + + // Subscribe to the following topics: + // Curvature parameters and state parameters + ros::Subscriber spline_parameters = nh.subscribe("spline", 10, splineCallback); + ros::Subscriber state_parameters = nh.subscribe("state", 10, stateCallback); + + // Setup message to hold commands + geometry_msgs::TwistStamped twist; + + // Setup the loop rate in Hz + ros::Rate loop_rate(LOOP_RATE); + bool endflag = false; + static double vdes; + + while (ros::ok()) + { + std_msgs::Bool _lf_stat; + + + ros::spinOnce(); + current_time = ros::Time::now().toSec(); + double elapsedTime = current_time - start_time; + + // Make sure we haven't finished the mission yet + if(endflag==FALSE) + { + if(newState == TRUE) + { + vdes = veh.vdes; + // This computes the next command + veh_temp = nextState(veh, curvature, vdes, next_time, elapsedTime + 0.1); + } + + // Set velocity + twist.twist.linear.x=vdes; + + // Ensure kappa is reasonable + veh_temp.kappa = min(kmax, veh_temp.kappa); + veh_temp.kappa = max (kmin, veh_temp.kappa); + + // Set angular velocity + twist.twist.angular.z=vdes*veh_temp.kappa; + } + + // If we have finished the mission clean up + else + { + ROS_INFO_STREAM("End mission"); + } + + // Publish messages + cmd_velocity_publisher.publish(twist); + loop_rate.sleep(); + } + return 0; +} + + +///////////////////////////////////////////////////////////////// +// Call back function for state update +///////////////////////////////////////////////////////////////// + +void stateCallback(const std_msgs::Float64MultiArray::ConstPtr& state) +{ + int i = 0; + + for(std::vector::const_iterator it = state->data.begin(); it != state->data.end(); ++it) + { + veh.state_value[i] = *it; + i++; + } + + newState = TRUE; + + return; +} + +///////////////////////////////////////////////////////////////// +// Callback function for spline update +///////////////////////////////////////////////////////////////// + +void splineCallback(const std_msgs::Float64MultiArray::ConstPtr& spline) +{ + if(ros::ok()) + { + start_time= ros::Time::now().toSec(); + } + + // Reset elapsed time to 0, if called... + double start_time = ros::Time::now().toSec(); + int i = 0; + + for(std::vector::const_iterator it = spline->data.begin(); it != spline->data.end(); ++it) + { + curvature.spline_value[i] = *it; + i++; + } + + if(newState == TRUE) + { + newSpline = TRUE; + } + + return; +} diff --git a/ros/src/computing/planning/motion/packages/driving_planner/package.xml b/ros/src/computing/planning/motion/packages/driving_planner/package.xml index 3a65835118f..2ee100c76f4 100644 --- a/ros/src/computing/planning/motion/packages/driving_planner/package.xml +++ b/ros/src/computing/planning/motion/packages/driving_planner/package.xml @@ -7,7 +7,7 @@ - h_ohta + mokelly @@ -42,7 +42,17 @@ catkin roscpp waypoint_follower + std_msgs + tf + gnss + message_generation + runtime_manager + runtime_manager + message_runtime roscpp + std_msgs + tf + gnss waypoint_follower @@ -51,4 +61,4 @@ - \ No newline at end of file + diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt b/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt index ff27390b94d..edf661c64b2 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt +++ b/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt @@ -13,6 +13,19 @@ find_package(catkin REQUIRED COMPONENTS runtime_manager ) +################################################ +## Find OpenMP in order to parallelize loops ## +################################################ + +OPTION (USE_OpenMP "Use OpenMP" ON) +IF(USE_OpenMP) + FIND_PACKAGE(OpenMP) + IF(OPENMP_FOUND) + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + ENDIF() +ENDIF() + ################################################ ## Declare ROS messages, services and actions ## ################################################ @@ -56,12 +69,26 @@ add_dependencies(libwaypoint_follower waypoint_follower_generate_messages_cpp runtime_manager_generate_messages_cpp) +#add_library(libtraj_gen lib/libtraj_gen.cpp) +#target_link_libraries(libtraj_gen ${catkin_LIBRARIES} /usr/lib/libarmadillo.so) + add_executable(pure_pursuit nodes/pure_pursuit/pure_pursuit.cpp) target_link_libraries(pure_pursuit gnss libwaypoint_follower ${catkin_LIBRARIES}) add_dependencies(pure_pursuit waypoint_follower_generate_messages_cpp runtime_manager_generate_messages_cpp) +#add_executable(trajectory_generator nodes/trajectory_generator/trajectory_generator.cpp) +#target_link_libraries(trajectory_generator gnss libwaypoint_follower libtraj_gen ${catkin_LIBRARIES}) +#add_dependencies(trajectory_generator +#waypoint_follower_generate_messages_cpp +#runtime_manager_generate_messages_cpp) + +#add_executable(command_converter nodes/command_converter/command_converter.cpp) +#target_link_libraries(command_converter libwaypoint_follower libtraj_gen ${catkin_LIBRARIES}) +#add_dependencies(command_converter +#waypoint_follower_generate_messages_cpp) + add_executable(odom_gen nodes/odom_gen/odom_gen.cpp) target_link_libraries(odom_gen libwaypoint_follower ${catkin_LIBRARIES}) @@ -77,4 +104,4 @@ add_executable(velocity_set nodes/velocity_set/velocity_set.cpp) target_link_libraries(velocity_set ${catkin_LIBRARIES}) add_dependencies(velocity_set waypoint_follower_generate_messages_cpp) - + diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/odom_gen/odom_gen.cpp b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/odom_gen/odom_gen.cpp index ca6373ff935..520802863cc 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/odom_gen/odom_gen.cpp +++ b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/odom_gen/odom_gen.cpp @@ -157,7 +157,7 @@ int main(int argc, char **argv) geometry_msgs::Pose pose; double th = 0; - ros::Rate loop_rate(10); // 10Hz + ros::Rate loop_rate(50); // 50Hz while (ros::ok()) { ros::spinOnce(); //check subscribe topic diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit.cpp b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/pure_pursuit/pure_pursuit.cpp old mode 100644 new mode 100755 diff --git a/ros/src/computing/planning/motion/packages/waypoint_maker/nodes/waypoint_loader/waypoint_loader.cpp b/ros/src/computing/planning/motion/packages/waypoint_maker/nodes/waypoint_loader/waypoint_loader.cpp index fbd795bad04..800b6b4fcf6 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_maker/nodes/waypoint_loader/waypoint_loader.cpp +++ b/ros/src/computing/planning/motion/packages/waypoint_maker/nodes/waypoint_loader/waypoint_loader.cpp @@ -35,7 +35,6 @@ #include #include #include - #include "waypoint_follower/libwaypoint_follower.h" struct WP diff --git a/ros/src/util/packages/runtime_manager/scripts/computing.yaml b/ros/src/util/packages/runtime_manager/scripts/computing.yaml index 5554ffcee4f..c4470e05e0e 100644 --- a/ros/src/util/packages/runtime_manager/scripts/computing.yaml +++ b/ros/src/util/packages/runtime_manager/scripts/computing.yaml @@ -191,6 +191,18 @@ subs : - name : Motion Planning subs : - name : driving_planner + subs : + - name : lattice_trajectory_gen + cmd : roslaunch driving_planner lattice_trajectory_gen.launch + param: lane_follower_trajgen + gui : + velocity: + depend : param_flag + lookahead_threshold: + depend : param_flag + stat_topic : [ lf ] + - name : lattice_twist_convert + cmd : rosrun driving_planner lattice_twist_convert - name : waypoint_maker subs : @@ -222,7 +234,7 @@ subs : - name : collision_avoid cmd : roslaunch waypoint_follower collision_avoid.launch - name : twist_through - cmd : roslaunch waypoint_follower twist_through + cmd : rosrun waypoint_follower twist_through - name : wf_simulator cmd : roslaunch waypoint_follower wf_simulator.launch param: wf_simulator @@ -550,6 +562,31 @@ params : kind : checkbox v : False rosparam: /pure_pursuit/sim_mode + - name : lane_follower_trajgen + topic : /config/lane_follower + msg : ConfigLaneFollower + vars : + - name : param_flag + kind : radio_box + choices: + - Waypoint + - Dialog + v : 1 + - name : velocity + label : Velocity + min : 0 + max : 60 + v : 5.0 + - name : lookahead_threshold + label : Lookahead Threshold + min : 0 + max : 30 + v : 4.0 + - name : sim_mode + label : 'Simulation Mode' + kind : checkbox + v : False + rosparam: /lattice_trajectory_gen/sim_mode - name : wf_simulator vars : - name : use_pose diff --git a/ros/src/util/packages/runtime_manager/scripts/computing_backup.yaml b/ros/src/util/packages/runtime_manager/scripts/computing_backup.yaml new file mode 100644 index 00000000000..a137495fad9 --- /dev/null +++ b/ros/src/util/packages/runtime_manager/scripts/computing_backup.yaml @@ -0,0 +1,912 @@ +name : Computing +subs : +- subs : + - name : Localization + subs : + - name : gnss_localizer + subs : + - name : fix2tfpose + cmd : rosrun gnss_localizer fix2tfpose + gui : + stat_topic : [ gnss ] + - name : nmea2tfpose + #cmd : rosrun gnss_localizer nmea2tfpose + cmd : roslaunch gnss_localizer nmea2tfpose.launch + gui : + stat_topic : [ gnss ] + - name : ndt_localizer + subs : + - name : ndt_mapping + cmd : roslaunch ndt_localizer ndt_mapping.launch + param: ndt_mapping + gui : + dialog : MyDialogNdtMapping + stat_topic : [ ndt ] + - name : ndt_matching + cmd : roslaunch ndt_localizer ndt_matching.launch + param: ndt + gui : + stat_topic : [ ndt ] + - name : pos_marker + cmd : rosrun ndt_localizer pos_marker + + - name : Detection + subs : + - name : cv_detector + subs : + - name : dpm_ocv + cmd : roslaunch runtime_manager dpm_ocv.launch + param: dpm_ocv + gui : + flags : [ open_dialog ] + config_dialog : MyDialogCarPedestrian + car_pedestrian_obj_key : { car : car_dpm, pedestrian : pedestrian_dpm } + open_dialog : MyDialogDpm + use_gpu : + flags : [ nl ] + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + + - name : dpm_ttic + cmd : roslaunch cv_tracker dpm_ttic.launch + param: dpm_ttic + gui : + flags : [ open_dialog ] + config_dialog : MyDialogCarPedestrian + car_pedestrian_obj_key : { car : car_dpm, pedestrian : pedestrian_dpm } + open_dialog : MyDialogDpm + use_gpu : + flags : [ nl ] + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + part_model_car : + flags : [ hline ] + + - name : range_fusion + cmd : roslaunch cv_tracker ranging.launch + param: car_fusion + gui : + flags : [ open_dialog ] + config_dialog : MyDialogCarPedestrian + car_pedestrian_obj_key : { car : car_fusion, pedestrian : pedestrian_fusion } + config_dialog_only : [ min_low_height, max_low_height, max_height, min_points, dispersion ] + open_dialog_only : [ car, pedestrian ] + open_dialog_ok_label : Start + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + + - name : kf_track + cmd : roslaunch cv_tracker kf_tracking.launch + param: car_kf + gui : + flags : [ open_dialog ] + config_dialog : MyDialogCarPedestrian + car_pedestrian_obj_key : { car : car_kf, pedestrian : pedestrian_kf } + config_dialog_only : + - initial_lifespan + - default_lifespan + - noise_covariance + - measurement_noise_covariance + - error_estimate_covariance + - percentage_of_overlapping + - orb_features + - use_orb + open_dialog_only : [ car, pedestrian ] + open_dialog_ok_label : Start + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + + - name : obj_reproj + cmd : roslaunch cv_tracker reprojection.launch + param: obj_reproj + gui : + flags : [ open_dialog, no_link ] + open_dialog_ok_label : Start + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + + - name : lidar_detector + subs : + - name : euclidean_clusterer + cmd : roslaunch lidar_tracker euclidean_clustering.launch + - name : obj_fusion + cmd : roslaunch lidar_tracker obj_fusion.launch + param: obj_fusion + gui : + flags : [ open_dialog, no_link ] + open_dialog_ok_label : Start + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + + - name : road_wizard + subs : + - name : feat_proj + cmd : rosrun road_wizard feat_proj + - name : region_tlr + cmd : roslaunch road_wizard traffic_light_recognition.launch + + +- subs : + - name : Mission Planning + subs : + - name : lane_planner + subs : + - name : lane_change + cmd : rosrun lane_planner lane_change + - name : lane_navi + cmd : rosrun lane_planner lane_navi + - name : lane_rule + cmd : rosrun lane_planner lane_rule + param: lane_rule + - name : lane_stop + cmd : rosrun lane_planner lane_stop + param: lane_stop + gui : + dialog: MyDialogLaneStop + - name : parking_planner + subs : + - name : XXX + cmd : echo "XXX" + + - name : Motion Planning + subs : + - name : driving_planner + subs : + - name : trajectory_generator + cmd : roslaunch driving_planner trajectory_generator.launch + param: lane_follower_trajgen + gui : + velocity: + depend : param_flag + lookahead_threshold: + depend : param_flag + stat_topic : [ lf ] + - name : command_converter + cmd : rosrun driving_planner command_converter + - name : obstacle_avoidance + cmd : rosrun driving_planner obstacle_avoidance + - name : safety_driving + cmd : rosrun driving_planner safety_driving + - name : waypoint_maker + subs : + - name : waypoint_loader + cmd : roslaunch waypoint_maker waypoint_loader.launch + param: waypoint_loader + gui : + waypoint_filename : + prop : 1 + - name : waypoint_saver + cmd : roslaunch waypoint_maker waypoint_saver.launch + param: waypoint_saver + gui : + save_filename : + prop : 1 + - name : waypoint_follower + subs : + - name : pure_pursuit + cmd : roslaunch waypoint_follower pure_pursuit.launch + param: lane_follower + gui : + velocity: + depend : param_flag + lookahead_threshold: + depend : param_flag + stat_topic : [ lf ] + - name : distance_keep + cmd : roslaunch waypoint_follower distance_keep.launch + - name : twist_through + cmd : rosrun waypoint_follower twist_through + - name : car_simulator + cmd : roslaunch waypoint_follower car_simulator.launch + param: car_simulator + gui : + use_pose: + flags : [ no_category, nl ] + x: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category ] + y: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category ] + z: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category, nl ] + roll: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category ] + pitch: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category ] + yaw: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category, nl ] + +buttons: + car_dpm : + param : car_dpm + pedestrian_dpm : + param : pedestrian_dpm + car_fusion : + param : car_fusion + pedestrian_fusion : + param : pedestrian_fusion + car_kf : + param : car_kf + pedestrian_kf : + param : pedestrian_kf + +params : + - name : dpm_ocv + vars : + - name : use_gpu + label : Use GPU + kind : radio_box + choices : [ 'False', 'True' ] + choices_type : str + choices_style: h + v : 'False' + cmd_param: + dash : '' + delim : ':=' + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + - name : model_file_car + label : 'model_file (Car)' + kind : path + v : $(rospack find cv_tracker)/data/car_2008.xml + cmd_param: + dash : '' + delim : ':=' + - name : model_file_pedestrian + label : 'model_file (Pedestrian)' + kind : path + v : $(rospack find cv_tracker)/data/person.xml + cmd_param: + dash : '' + delim : ':=' + + - name : dpm_ttic + vars : + - name : use_gpu + label : Use GPU + kind : radio_box + choices : [ 'False', 'True' ] + choices_type : str + choices_style: h + v : 'False' + cmd_param: + dash : '' + delim : ':=' + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + - name : comp_model_car + label : 'comp_model (Car)' + kind : path + v : $(rospack find cv_tracker)/data/car_comp.csv + cmd_param: + dash : '' + delim : ':=' + - name : root_model_car + label : 'root_model (Car)' + kind : path + v : $(rospack find cv_tracker)/data/car_root.csv + cmd_param: + dash : '' + delim : ':=' + - name : part_model_car + label : 'part_model (Car)' + kind : path + v : $(rospack find cv_tracker)/data/car_part.csv + cmd_param: + dash : '' + delim : ':=' + - name : comp_model_pedestrian + label : 'comp_model (Pedestrian)' + kind : path + v : $(rospack find cv_tracker)/data/person_comp.csv + cmd_param: + dash : '' + delim : ':=' + - name : root_model_pedestrian + label : 'root_model (Pedestrian)' + kind : path + v : $(rospack find cv_tracker)/data/person_root.csv + cmd_param: + dash : '' + delim : ':=' + - name : part_model_pedestrian + label : 'part_model (Pedestrian)' + kind : path + v : $(rospack find cv_tracker)/data/person_part.csv + cmd_param: + dash : '' + delim : ':=' + + - name : car_dpm + topic : /config/car_dpm + msg : ConfigCarDpm + vars : + - name : score_threshold + label : Score Threshold + min : -2 + max : 2 + v : -0.5 + - name : group_threshold + label : Group Threshold + min : 0 + max : 1 + v : 0.1 + - name : Lambda + label : Lambda + min : 1 + max : 20 + v : 10 + - name : num_cells + label : Num Cells + min : 2 + max : 10 + v : 8 + - name : num_bins + label : Num Bins + min : 2 + max : 10 + v : 9 + + - name : pedestrian_dpm + topic : /config/pedestrian_dpm + msg : ConfigPedestrianDpm + vars : + - name : score_threshold + label : Score Threshold + min : -2 + max : 2 + v : 0.6 + - name : group_threshold + label : Group Threshold + min : 0 + max : 1 + v : 0.3 + - name : Lambda + label : Lambda + min : 1 + max : 20 + v : 10 + - name : num_cells + label : Num Cells + min : 2 + max : 10 + v : 8 + - name : num_bins + label : Num Bins + min : 2 + max : 10 + v : 9 + + - name : ndt + topic : /config/ndt + msg : ConfigNdt + vars : + - name : init_pos_gnss + kind : radio_box + choices: + - Initial Pos + - GNSS + v : 1 + - name : x + label : 'x:' + v : 0.0 + - name : y + label : 'y:' + v : 0.0 + - name : z + label : 'z:' + v : 0.0 + - name : roll + label : 'roll:' + v : 0.0 + - name : pitch + label : 'pitch:' + v : 0.0 + - name : yaw + label : 'yaw:' + v : 0.0 + - name : resolution + label : Resolution + min : 0 + max : 10 + v : 1.0 + - name : step_size + label : Step Size + min : 0 + max : 1 + v : 0.1 + - name : trans_esp + label : Trans ESP + min : 0 + max : 0.1 + v : 0.01 + - name : leaf_size + label : Leaf Size + min : 0 + max : 10 + v : 2.0 + - name : angle_error + label : Angle Error + min : -180.0 + max : 180.0 + step : 0.1 + v : 0 + - name : shift_x + label : Shift X + min : -2.0 + max : 2.0 + v : 0 + - name : shift_y + label : Shift Y + min : -2.0 + max : 2.0 + v : 0 + - name : shift_z + label : Shift Z + min : -2.0 + max : 2.0 + v : 0 + + - name : lane_follower + topic : /config/lane_follower + msg : ConfigLaneFollower + vars : + - name : param_flag + kind : radio_box + choices: + - Waypoint + - Dialog + v : 1 + - name : velocity + label : Velocity + min : 0 + max : 60 + v : 5.0 + - name : lookahead_threshold + label : Lookahead Threshold + min : 0 + max : 30 + v : 4.0 + - name : sim_mode + label : 'Simulation Mode' + kind : checkbox + v : False + rosparam: /pure_pursuit/sim_mode + - name : lane_follower_trajgen + topic : /config/lane_follower + msg : ConfigLaneFollower + vars : + - name : param_flag + kind : radio_box + choices: + - Waypoint + - Dialog + v : 1 + - name : velocity + label : Velocity + min : 0 + max : 60 + v : 5.0 + - name : lookahead_threshold + label : Lookahead Threshold + min : 0 + max : 30 + v : 4.0 + - name : sim_mode + label : 'Simulation Mode' + kind : checkbox + v : False + rosparam: /trajectory_generator/sim_mode + - name : car_simulator + vars : + - name : use_pose + kind : radio_box + choices : + - Initial Pos + - NDT + - GNSS + choices_type: str + v : Initial Pos + rosparam: /odom_gen/use_pose + - name : x + label : 'X:' + v : 0.0 + rosparam: /odom_gen/initial_pos_x + - name : y + label : 'Y:' + v : 0.0 + rosparam: /odom_gen/initial_pos_y + - name : z + label : 'Z:' + v : 0.0 + rosparam: /odom_gen/initial_pos_z + - name : roll + label : 'Roll:' + v : 0.0 + rosparam: /odom_gen/initial_pos_roll + - name : pitch + label : 'Pitch:' + v : 0.0 + rosparam: /odom_gen/initial_pos_pitch + - name : yaw + label : 'Yaw:' + v : 0.0 + rosparam: /odom_gen/initial_pos_yaw + + - name : car_kf + topic : /config/car_kf + msg : ConfigCarKf + vars : + - name : initial_lifespan + label : Initial Lifespan + min : 1 + max : 100 + v : 4 + - name : default_lifespan + label : Default Lifespan + min : 1 + max : 100 + v : 8 + - name : noise_covariance + label : Noise Covariance + min : 1 + max : 1000 + v : 1.0 + - name : measurement_noise_covariance + label : Measurement Noise Covariance + min : 1 + max : 1000 + v : 25.0 + - name : error_estimate_covariance + label : Error estimate covariance + min : 1 + max : 10000000 + v : 1000000.0 + - name : percentage_of_overlapping + label : Percentage of Overlapping + min : 0 + max : 100 + v : 0.0 + - name : orb_features + label : Number of ORB features + min : 500 + max : 5000 + v : 2000 + - name : use_orb + label : Use ORB Matching? + min : 0 + max : 1 + v : 0 + + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + + - name : pedestrian_kf + topic : /config/pedestrian_kf + msg : ConfigPedestrianKf + vars : + - name : initial_lifespan + label : Initial Lifespan + min : 1 + max : 100 + v : 2 + - name : default_lifespan + label : Default Lifespan + min : 1 + max : 100 + v : 10 + - name : noise_covariance + label : Noise Covariance + min : 1 + max : 1000 + v : 1.0 + - name : measurement_noise_covariance + label : Measurement Noise Covariance + min : 1 + max : 1000 + v : 25.0 + - name : error_estimate_covariance + label : Error estimate covariance + min : 1 + max : 10000000 + v : 1000000.0 + - name : percentage_of_overlapping + label : Percentage of Overlapping + min : 0 + max : 100 + v : 0.0 + - name : orb_features + label : Number of ORB features + min : 500 + max : 5000 + v : 2000 + - name : use_orb + label : Use ORB Matching? + min : 0 + max : 1 + v : 0 + + - name : obj_reproj + vars : + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + + - name : obj_fusion + vars : + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + + - name : traffic_light + vars : + - name : file + kind : path + path_type : dir + v : /tmp + cmd_param : + dash : '' + delim : ':=' + + - name : lane_rule + topic : /config/lane_rule + msg : ConfigLaneRule + vars : + - name : ruled_waypoint + kind : path + path_type : save + v : /tmp/ruled_waypoint.csv + rosparam : /lane_rule/ruled_waypoint_csv + - name : velocity + label : Velocity (km/h) + min : 0 + max : 200 + v : 40.0 + - name : difference_around_signal + label : Difference around Signal (km/h) + min : 0 + max : 20 + v : 2.0 + - name : number_of_zeros + label : Number of Zeros + min : 1 + max : 100 + v : 1 + + - name : lane_stop + topic : /traffic_light + msg : traffic_light + vars : + - name : traffic_light + v : 0 + + - name : waypoint_saver + vars : + - name : save_filename + kind : path + path_type : save + v : '' + cmd_param : + only_enable : True + var_name : save_finename + dash : '' + delim : ':=' + - name : interval + label : Interval + v : 4.0 + cmd_param : + only_enable : True + dash : '' + delim : ':=' + + - name : waypoint_loader + topic : /config/waypoint_loader + msg : ConfigWaypointLoader + vars : + - name : ruled_waypoint + kind : path + v : /tmp/ruled_waypoint.csv + rosparam : /waypoint_loader/ruled_waypoint_csv + - name : velocity + label : Velocity (km/h) + min : 0 + max : 200 + v : 40.0 + - name : difference_around_signal + label : Difference around Signal (km/h) + min : 0 + max : 20 + v : 2.0 + - name : number_of_zeros + label : Number of Zeros + min : 1 + max : 100 + v : 1 + + - name : ndt_mapping + topic : /config/ndt_mapping + msg : ConfigNdtMapping + flags : [ no_init_update ] + vars : + - name : resolution + label : Resolution + min : 0.0 + max : 10.0 + v : 1.0 + - name : step_size + label : Step Size + min : 0.0 + max : 1.0 + v : 0.1 + - name : trans_eps + label : Trans EPS + min : 0.0 + max : 0.1 + v : 0.01 + - name : leaf_size + label : Leaf Size + min : 0.0 + max : 10.0 + v : 1.0 + + - name : car_fusion + topic : /config/car_fusion + msg : ConfigCarFusion + vars : + - name : min_low_height + label : Minimum Height(Lower Bound) + min : -2 + max : 0 + v : -1.5 + - name : max_low_height + label : Minimum Height(Upper Bound) + min : -2 + max : 0 + v : -1.0 + - name : max_height + label : Maximum Height + min : 0 + max : 10 + v : 2.0 + - name : min_points + label : Min Points in Box + min : 1 + max : 100 + v : 2 + - name : dispersion + label : Coefficient of Variation + min : 0.0 + max : 5.0 + v : 1.0 + + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + + - name : pedestrian_fusion + topic : /config/pedestrian_fusion + msg : ConfigPedestrianFusion + vars : + - name : min_low_height + label : Minimum Height(Lower Bound) + min : -2 + max : 0 + v : -1.5 + - name : max_low_height + label : Minimum Height(Upper Bound) + min : -2 + max : 0 + v : -1.0 + - name : max_height + label : Maximum Height + min : 0 + max : 10 + v : 2.0 + - name : min_points + label : Min Points in Box + min : 1 + max : 100 + v : 2 + - name : dispersion + label : Coefficient of Variation + min : 0.0 + max : 5.0 + v : 1.0 diff --git a/ros/src/util/packages/runtime_manager/scripts/computing_old.yaml b/ros/src/util/packages/runtime_manager/scripts/computing_old.yaml new file mode 100644 index 00000000000..dad2c591052 --- /dev/null +++ b/ros/src/util/packages/runtime_manager/scripts/computing_old.yaml @@ -0,0 +1,912 @@ +name : Computing +subs : +- subs : + - name : Localization + subs : + - name : gnss_localizer + subs : + - name : fix2tfpose + cmd : rosrun gnss_localizer fix2tfpose + gui : + stat_topic : [ gnss ] + - name : nmea2tfpose + #cmd : rosrun gnss_localizer nmea2tfpose + cmd : roslaunch gnss_localizer nmea2tfpose.launch + gui : + stat_topic : [ gnss ] + - name : ndt_localizer + subs : + - name : ndt_mapping + cmd : roslaunch ndt_localizer ndt_mapping.launch + param: ndt_mapping + gui : + dialog : MyDialogNdtMapping + stat_topic : [ ndt ] + - name : ndt_matching + cmd : roslaunch ndt_localizer ndt_matching.launch + param: ndt + gui : + stat_topic : [ ndt ] + - name : pos_marker + cmd : rosrun ndt_localizer pos_marker + + - name : Detection + subs : + - name : cv_detector + subs : + - name : dpm_ocv + cmd : roslaunch runtime_manager dpm_ocv.launch + param: dpm_ocv + gui : + flags : [ open_dialog ] + config_dialog : MyDialogCarPedestrian + car_pedestrian_obj_key : { car : car_dpm, pedestrian : pedestrian_dpm } + open_dialog : MyDialogDpm + use_gpu : + flags : [ nl ] + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + + - name : dpm_ttic + cmd : roslaunch cv_tracker dpm_ttic.launch + param: dpm_ttic + gui : + flags : [ open_dialog ] + config_dialog : MyDialogCarPedestrian + car_pedestrian_obj_key : { car : car_dpm, pedestrian : pedestrian_dpm } + open_dialog : MyDialogDpm + use_gpu : + flags : [ nl ] + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + part_model_car : + flags : [ hline ] + + - name : range_fusion + cmd : roslaunch cv_tracker ranging.launch + param: car_fusion + gui : + flags : [ open_dialog ] + config_dialog : MyDialogCarPedestrian + car_pedestrian_obj_key : { car : car_fusion, pedestrian : pedestrian_fusion } + config_dialog_only : [ min_low_height, max_low_height, max_height, min_points, dispersion ] + open_dialog_only : [ car, pedestrian ] + open_dialog_ok_label : Start + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + + - name : kf_track + cmd : roslaunch cv_tracker kf_tracking.launch + param: car_kf + gui : + flags : [ open_dialog ] + config_dialog : MyDialogCarPedestrian + car_pedestrian_obj_key : { car : car_kf, pedestrian : pedestrian_kf } + config_dialog_only : + - initial_lifespan + - default_lifespan + - noise_covariance + - measurement_noise_covariance + - error_estimate_covariance + - percentage_of_overlapping + - orb_features + - use_orb + open_dialog_only : [ car, pedestrian ] + open_dialog_ok_label : Start + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + + - name : obj_reproj + cmd : roslaunch cv_tracker reprojection.launch + param: obj_reproj + gui : + flags : [ open_dialog, no_link ] + open_dialog_ok_label : Start + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + + - name : lidar_detector + subs : + - name : euclidean_clusterer + cmd : roslaunch lidar_tracker euclidean_clustering.launch + - name : obj_fusion + cmd : roslaunch lidar_tracker obj_fusion.launch + param: obj_fusion + gui : + flags : [ open_dialog, no_link ] + open_dialog_ok_label : Start + car : + user_category : Detection Target + pedestrian : + border : 16 + flags : [ left ] + + - name : road_wizard + subs : + - name : feat_proj + cmd : rosrun road_wizard feat_proj + - name : region_tlr + cmd : roslaunch road_wizard traffic_light_recognition.launch + + +- subs : + - name : Mission Planning + subs : + - name : lane_planner + subs : + - name : lane_change + cmd : rosrun lane_planner lane_change + - name : lane_navi + cmd : rosrun lane_planner lane_navi + - name : lane_rule + cmd : rosrun lane_planner lane_rule + param: lane_rule + - name : lane_stop + cmd : rosrun lane_planner lane_stop + param: lane_stop + gui : + dialog: MyDialogLaneStop + - name : parking_planner + subs : + - name : XXX + cmd : echo "XXX" + + - name : Motion Planning + subs : + - name : driving_planner + subs : + - name : obstacle_avoidance + cmd : rosrun driving_planner obstacle_avoidance + - name : safety_driving + cmd : rosrun driving_planner safety_driving + - name : waypoint_maker + subs : + - name : waypoint_loader + cmd : roslaunch waypoint_maker waypoint_loader.launch + param: waypoint_loader + gui : + waypoint_filename : + prop : 1 + - name : waypoint_saver + cmd : roslaunch waypoint_maker waypoint_saver.launch + param: waypoint_saver + gui : + save_filename : + prop : 1 + - name : waypoint_follower + subs : + - name : trajectory_generator + cmd : roslaunch waypoint_follower trajectory_generator.launch + param: lane_follower_trajgen + gui : + velocity: + depend : param_flag + lookahead_threshold: + depend : param_flag + stat_topic : [ lf ] + - name : pure_pursuit + cmd : roslaunch waypoint_follower pure_pursuit.launch + param: lane_follower + gui : + velocity: + depend : param_flag + lookahead_threshold: + depend : param_flag + stat_topic : [ lf ] + - name : command_converter + cmd : rosrun waypoint_follower command_converter + - name : distance_keep + cmd : roslaunch waypoint_follower distance_keep.launch + - name : twist_through + cmd : rosrun waypoint_follower twist_through + - name : car_simulator + cmd : roslaunch waypoint_follower car_simulator.launch + param: car_simulator + gui : + use_pose: + flags : [ no_category, nl ] + x: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category ] + y: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category ] + z: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category, nl ] + roll: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category ] + pitch: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category ] + yaw: + depend : use_pose + depend_bool : 'lambda v : v == "Initial Pos"' + flags : [ no_category, nl ] + +buttons: + car_dpm : + param : car_dpm + pedestrian_dpm : + param : pedestrian_dpm + car_fusion : + param : car_fusion + pedestrian_fusion : + param : pedestrian_fusion + car_kf : + param : car_kf + pedestrian_kf : + param : pedestrian_kf + +params : + - name : dpm_ocv + vars : + - name : use_gpu + label : Use GPU + kind : radio_box + choices : [ 'False', 'True' ] + choices_type : str + choices_style: h + v : 'False' + cmd_param: + dash : '' + delim : ':=' + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + - name : model_file_car + label : 'model_file (Car)' + kind : path + v : $(rospack find cv_tracker)/data/car_2008.xml + cmd_param: + dash : '' + delim : ':=' + - name : model_file_pedestrian + label : 'model_file (Pedestrian)' + kind : path + v : $(rospack find cv_tracker)/data/person.xml + cmd_param: + dash : '' + delim : ':=' + + - name : dpm_ttic + vars : + - name : use_gpu + label : Use GPU + kind : radio_box + choices : [ 'False', 'True' ] + choices_type : str + choices_style: h + v : 'False' + cmd_param: + dash : '' + delim : ':=' + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + - name : comp_model_car + label : 'comp_model (Car)' + kind : path + v : $(rospack find cv_tracker)/data/car_comp.csv + cmd_param: + dash : '' + delim : ':=' + - name : root_model_car + label : 'root_model (Car)' + kind : path + v : $(rospack find cv_tracker)/data/car_root.csv + cmd_param: + dash : '' + delim : ':=' + - name : part_model_car + label : 'part_model (Car)' + kind : path + v : $(rospack find cv_tracker)/data/car_part.csv + cmd_param: + dash : '' + delim : ':=' + - name : comp_model_pedestrian + label : 'comp_model (Pedestrian)' + kind : path + v : $(rospack find cv_tracker)/data/person_comp.csv + cmd_param: + dash : '' + delim : ':=' + - name : root_model_pedestrian + label : 'root_model (Pedestrian)' + kind : path + v : $(rospack find cv_tracker)/data/person_root.csv + cmd_param: + dash : '' + delim : ':=' + - name : part_model_pedestrian + label : 'part_model (Pedestrian)' + kind : path + v : $(rospack find cv_tracker)/data/person_part.csv + cmd_param: + dash : '' + delim : ':=' + + - name : car_dpm + topic : /config/car_dpm + msg : ConfigCarDpm + vars : + - name : score_threshold + label : Score Threshold + min : -2 + max : 2 + v : -0.5 + - name : group_threshold + label : Group Threshold + min : 0 + max : 1 + v : 0.1 + - name : Lambda + label : Lambda + min : 1 + max : 20 + v : 10 + - name : num_cells + label : Num Cells + min : 2 + max : 10 + v : 8 + - name : num_bins + label : Num Bins + min : 2 + max : 10 + v : 9 + + - name : pedestrian_dpm + topic : /config/pedestrian_dpm + msg : ConfigPedestrianDpm + vars : + - name : score_threshold + label : Score Threshold + min : -2 + max : 2 + v : 0.6 + - name : group_threshold + label : Group Threshold + min : 0 + max : 1 + v : 0.3 + - name : Lambda + label : Lambda + min : 1 + max : 20 + v : 10 + - name : num_cells + label : Num Cells + min : 2 + max : 10 + v : 8 + - name : num_bins + label : Num Bins + min : 2 + max : 10 + v : 9 + + - name : ndt + topic : /config/ndt + msg : ConfigNdt + vars : + - name : init_pos_gnss + kind : radio_box + choices: + - Initial Pos + - GNSS + v : 1 + - name : x + label : 'x:' + v : 0.0 + - name : y + label : 'y:' + v : 0.0 + - name : z + label : 'z:' + v : 0.0 + - name : roll + label : 'roll:' + v : 0.0 + - name : pitch + label : 'pitch:' + v : 0.0 + - name : yaw + label : 'yaw:' + v : 0.0 + - name : resolution + label : Resolution + min : 0 + max : 10 + v : 1.0 + - name : step_size + label : Step Size + min : 0 + max : 1 + v : 0.1 + - name : trans_esp + label : Trans ESP + min : 0 + max : 0.1 + v : 0.01 + - name : leaf_size + label : Leaf Size + min : 0 + max : 10 + v : 2.0 + - name : angle_error + label : Angle Error + min : -180.0 + max : 180.0 + step : 0.1 + v : 0 + - name : shift_x + label : Shift X + min : -2.0 + max : 2.0 + v : 0 + - name : shift_y + label : Shift Y + min : -2.0 + max : 2.0 + v : 0 + - name : shift_z + label : Shift Z + min : -2.0 + max : 2.0 + v : 0 + + - name : lane_follower + topic : /config/lane_follower + msg : ConfigLaneFollower + vars : + - name : param_flag + kind : radio_box + choices: + - Waypoint + - Dialog + v : 1 + - name : velocity + label : Velocity + min : 0 + max : 60 + v : 5.0 + - name : lookahead_threshold + label : Lookahead Threshold + min : 0 + max : 30 + v : 4.0 + - name : sim_mode + label : 'Simulation Mode' + kind : checkbox + v : False + rosparam: /pure_pursuit/sim_mode + - name : lane_follower_trajgen + topic : /config/lane_follower + msg : ConfigLaneFollower + vars : + - name : param_flag + kind : radio_box + choices: + - Waypoint + - Dialog + v : 1 + - name : velocity + label : Velocity + min : 0 + max : 60 + v : 5.0 + - name : lookahead_threshold + label : Lookahead Threshold + min : 0 + max : 30 + v : 4.0 + - name : sim_mode + label : 'Simulation Mode' + kind : checkbox + v : False + rosparam: /trajectory_generator/sim_mode + - name : car_simulator + vars : + - name : use_pose + kind : radio_box + choices : + - Initial Pos + - NDT + - GNSS + choices_type: str + v : Initial Pos + rosparam: /odom_gen/use_pose + - name : x + label : 'X:' + v : 0.0 + rosparam: /odom_gen/initial_pos_x + - name : y + label : 'Y:' + v : 0.0 + rosparam: /odom_gen/initial_pos_y + - name : z + label : 'Z:' + v : 0.0 + rosparam: /odom_gen/initial_pos_z + - name : roll + label : 'Roll:' + v : 0.0 + rosparam: /odom_gen/initial_pos_roll + - name : pitch + label : 'Pitch:' + v : 0.0 + rosparam: /odom_gen/initial_pos_pitch + - name : yaw + label : 'Yaw:' + v : 0.0 + rosparam: /odom_gen/initial_pos_yaw + + - name : car_kf + topic : /config/car_kf + msg : ConfigCarKf + vars : + - name : initial_lifespan + label : Initial Lifespan + min : 1 + max : 100 + v : 4 + - name : default_lifespan + label : Default Lifespan + min : 1 + max : 100 + v : 8 + - name : noise_covariance + label : Noise Covariance + min : 1 + max : 1000 + v : 1.0 + - name : measurement_noise_covariance + label : Measurement Noise Covariance + min : 1 + max : 1000 + v : 25.0 + - name : error_estimate_covariance + label : Error estimate covariance + min : 1 + max : 10000000 + v : 1000000.0 + - name : percentage_of_overlapping + label : Percentage of Overlapping + min : 0 + max : 100 + v : 0.0 + - name : orb_features + label : Number of ORB features + min : 500 + max : 5000 + v : 2000 + - name : use_orb + label : Use ORB Matching? + min : 0 + max : 1 + v : 0 + + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + + - name : pedestrian_kf + topic : /config/pedestrian_kf + msg : ConfigPedestrianKf + vars : + - name : initial_lifespan + label : Initial Lifespan + min : 1 + max : 100 + v : 2 + - name : default_lifespan + label : Default Lifespan + min : 1 + max : 100 + v : 10 + - name : noise_covariance + label : Noise Covariance + min : 1 + max : 1000 + v : 1.0 + - name : measurement_noise_covariance + label : Measurement Noise Covariance + min : 1 + max : 1000 + v : 25.0 + - name : error_estimate_covariance + label : Error estimate covariance + min : 1 + max : 10000000 + v : 1000000.0 + - name : percentage_of_overlapping + label : Percentage of Overlapping + min : 0 + max : 100 + v : 0.0 + - name : orb_features + label : Number of ORB features + min : 500 + max : 5000 + v : 2000 + - name : use_orb + label : Use ORB Matching? + min : 0 + max : 1 + v : 0 + + - name : obj_reproj + vars : + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + + - name : obj_fusion + vars : + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + + - name : traffic_light + vars : + - name : file + kind : path + path_type : dir + v : /tmp + cmd_param : + dash : '' + delim : ':=' + + - name : lane_rule + topic : /config/lane_rule + msg : ConfigLaneRule + vars : + - name : ruled_waypoint + kind : path + path_type : save + v : /tmp/ruled_waypoint.csv + rosparam : /lane_rule/ruled_waypoint_csv + - name : velocity + label : Velocity (km/h) + min : 0 + max : 200 + v : 40.0 + - name : difference_around_signal + label : Difference around Signal (km/h) + min : 0 + max : 20 + v : 2.0 + - name : number_of_zeros + label : Number of Zeros + min : 1 + max : 100 + v : 1 + + - name : lane_stop + topic : /traffic_light + msg : traffic_light + vars : + - name : traffic_light + v : 0 + + - name : waypoint_saver + vars : + - name : save_filename + kind : path + path_type : save + v : '' + cmd_param : + only_enable : True + var_name : save_finename + dash : '' + delim : ':=' + - name : interval + label : Interval + v : 4.0 + cmd_param : + only_enable : True + dash : '' + delim : ':=' + + - name : waypoint_loader + topic : /config/waypoint_loader + msg : ConfigWaypointLoader + vars : + - name : ruled_waypoint + kind : path + v : /tmp/ruled_waypoint.csv + rosparam : /waypoint_loader/ruled_waypoint_csv + - name : velocity + label : Velocity (km/h) + min : 0 + max : 200 + v : 40.0 + - name : difference_around_signal + label : Difference around Signal (km/h) + min : 0 + max : 20 + v : 2.0 + - name : number_of_zeros + label : Number of Zeros + min : 1 + max : 100 + v : 1 + + - name : ndt_mapping + topic : /config/ndt_mapping + msg : ConfigNdtMapping + flags : [ no_init_update ] + vars : + - name : resolution + label : Resolution + min : 0.0 + max : 10.0 + v : 1.0 + - name : step_size + label : Step Size + min : 0.0 + max : 1.0 + v : 0.1 + - name : trans_eps + label : Trans EPS + min : 0.0 + max : 0.1 + v : 0.01 + - name : leaf_size + label : Leaf Size + min : 0.0 + max : 10.0 + v : 1.0 + + - name : car_fusion + topic : /config/car_fusion + msg : ConfigCarFusion + vars : + - name : min_low_height + label : Minimum Height(Lower Bound) + min : -2 + max : 0 + v : -1.5 + - name : max_low_height + label : Minimum Height(Upper Bound) + min : -2 + max : 0 + v : -1.0 + - name : max_height + label : Maximum Height + min : 0 + max : 10 + v : 2.0 + - name : min_points + label : Min Points in Box + min : 1 + max : 100 + v : 2 + - name : dispersion + label : Coefficient of Variation + min : 0.0 + max : 5.0 + v : 1.0 + + - name : car + label : Car + kind : checkbox + v : True + cmd_param: + dash : '' + delim : ':=' + - name : pedestrian + label : Pedestrian + kind : checkbox + v : False + cmd_param: + dash : '' + delim : ':=' + + - name : pedestrian_fusion + topic : /config/pedestrian_fusion + msg : ConfigPedestrianFusion + vars : + - name : min_low_height + label : Minimum Height(Lower Bound) + min : -2 + max : 0 + v : -1.5 + - name : max_low_height + label : Minimum Height(Upper Bound) + min : -2 + max : 0 + v : -1.0 + - name : max_height + label : Maximum Height + min : 0 + max : 10 + v : 2.0 + - name : min_points + label : Min Points in Box + min : 1 + max : 100 + v : 2 + - name : dispersion + label : Coefficient of Variation + min : 0.0 + max : 5.0 + v : 1.0