Add the state lattice motion planning features

autowarefoundation · Aug 26, 2015 · 29e1f68 · 29e1f68
1 parent 6202e05
commit 29e1f68
Show file tree

Hide file tree

Showing 19 changed files with 4,373 additions and 171 deletions.
diff --git 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
@@ -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: <Fixed 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: <Fixed 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
@@ -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 ##