Path Planner w/ traffic sign support

competition_launch_files/circuit_race.launch

@@ -14,7 +14,11 @@
     <arg name="config_file" value="$(find rr_computer_vision)/config/circuit_race.yaml" />
   </include>
 
+  <!-- Path Planner Node -->
+  <include file="$(find rr_path_planner)/launch/path_planner.launch">
+    <arg name="config_file" value="$(find rr_path_planner)/config/circuit_race.yaml" />
+  </include>
+
   <!-- Mapper Node -->
   <include file="$(find rr_mapper)/launch/mapper.launch"/>
-
 </launch>

competition_launch_files/drag_race.launch

@@ -14,7 +14,11 @@
     <arg name="config_file" value="$(find rr_computer_vision)/config/drag_race.yaml" />
   </include>
 
-  <!-- Mapper Node -->
-  <include file="$(find rr_mapper)/launch/mapper.launch"/>
+  <!-- Path Planner Node -->
+  <include file="$(find rr_path_planner)/launch/path_planner.launch">
+    <arg name="config_file" value="$(find rr_path_planner)/config/drag_race.yaml" />
+  </include>
 
+   <!-- Mapper Node -->
+  <include file="$(find rr_mapper)/launch/mapper.launch"/>
 </launch>

competition_launch_files/obstacle_avoidance_challenge.launch

@@ -14,7 +14,11 @@
     <arg name="config_file" value="$(find rr_computer_vision)/config/obstacle_avoidance_challenge.yaml" />
   </include>
 
+  <!-- Path Planner Node -->
+  <include file="$(find rr_path_planner)/launch/path_planner.launch">
+    <arg name="config_file" value="$(find rr_path_planner)/config/obstacle_avoidance_challenge.yaml" />
+  </include>
+
   <!-- Mapper Node -->
   <include file="$(find rr_mapper)/launch/mapper.launch"/>
-
 </launch>

competition_launch_files/urban_road_challenge.launch

@@ -14,7 +14,11 @@
     <arg name="config_file" value="$(find rr_computer_vision)/config/urban_road_challenge.yaml" />
   </include>
 
+  <!-- Path Planner Node -->
+  <include file="$(find rr_path_planner)/launch/path_planner.launch">
+    <arg name="config_file" value="$(find rr_path_planner)/config/urban_road_challenge.yaml" />
+  </include>
+
   <!-- Mapper Node -->
   <include file="$(find rr_mapper)/launch/mapper.launch"/>
-
 </launch>

rr_common/CMakeLists.txt

@@ -18,6 +18,8 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
+add_compile_options(-std=c++11)
+
 ## Install project namespaced headers
 install(DIRECTORY include/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

rr_common/include/rr_topic_names.hpp

@@ -29,6 +29,15 @@ namespace rr_cv {
   static std::string lane_detection_occupancy_grid = rr_str + rr_cv_str + "lane_detection/occupancy_grid";
   static std::string horizontal_lane_monitor = rr_str + rr_cv_str + "lane_detection/horizontal_lane_crossed";
   static std::string traffic_sign_status = rr_str + rr_cv_str + "sign_detection/traffic_sign_status";
+
+  // Enum for publishing TrafficSign msg
+  enum sign_status: uint8_t
+  {
+    NONE,
+    LEFT,
+    RIGHT,
+    STRAIGHT
+  };
 }
 
 namespace rr_supervisor {

rr_common/rviz/test_path_planning.rviz

@@ -0,0 +1,143 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map1
+        - /Marker1
+        - /Image1
+      Splitter Ratio: 0.5
+    Tree Height: 775
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        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>
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /rr_bot/rr_mapper/occupancy_grid
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /Visualization/selected_path
+      Name: Marker
+      Namespaces:
+        Path Points: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /zed/zed_node/right/image_rect_color
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: 3.14000583
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Scale: -98.4878616
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz)
+      X: 0.831268549
+      Y: 2.03851771
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065030000046b000000be000001cf00000124000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1855
+  X: 65
+  Y: 24

rr_computer_vision/CMakeLists.txt

@@ -17,6 +17,8 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
 )
 
+find_package(OpenCV REQUIRED)
+
 add_message_files(
   FILES
   TrafficSign.msg
@@ -28,16 +30,13 @@ generate_messages(
 )
 
 catkin_package(
-  CATKIN_DEPENDS message_runtime
+  CATKIN_DEPENDS message_runtime std_msgs
 )
 
-find_package(OpenCV REQUIRED)
-
-catkin_package()
-
 include_directories(${OpenCV_INCLUDE_DIRS})
 include_directories(include ${catkin_INCLUDE_DIRS})
 include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+include_directories(rr_computer_vision ${catkin_EXPORTED_TARGETS})
 
 roslint_cpp(src/computer_vision.cpp include/computer_vision.hpp 
 src/lane_detection.cpp include/lane_detection.hpp
@@ -51,7 +50,7 @@ src/lane_detection.cpp include/lane_detection.hpp
 src/sign_detection.cpp include/sign_detection.hpp
 src/traffic_light.cpp include/traffic_light.hpp
 src/endline_detection.cpp include/endline_detection.hpp)
-add_dependencies(rr_computer_vision rr_computer_vision_generate_messages_cpp)
+add_dependencies(rr_computer_vision rr_computer_vision_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(rr_computer_vision ${catkin_LIBRARIES} ${OpenCV_LIBS})
+target_link_libraries(rr_computer_vision ${catkin_LIBRARIES} ${OpenCV_LIBS})

rr_computer_vision/include/sign_detection.hpp

@@ -52,15 +52,6 @@ class SignDetection
 
     // Haar cascade
     cv::CascadeClassifier sign_cascade;
-
-    // Enum for publishing TrafficSign msg
-    enum sign_status: uint8_t
-    {
-      NONE,
-      LEFT,
-      RIGHT,
-      STRAIGHT
-    };
 };
 
 #endif //SIGN_DETECTION_PROCESSOR_HPP

rr_computer_vision/launch/computer_vision.launch

@@ -15,7 +15,9 @@
   <param name="HaarCascadeFile" type="string" value="$(find rr_computer_vision)/classifier_dev/haar_cascade.xml"/>
 
   <!-- Launch ZED camera wrapper -->
+  <!--
   <include file="$(find zed_wrapper)/launch/zed.launch"> </include>
+  -->
 
   <!-- Computer Vision Node -->
   <node pkg="rr_computer_vision" type="rr_computer_vision" name="rr_computer_vision" output="screen"/>

rr_computer_vision/msg/TrafficSign.msg

@@ -1,3 +1,9 @@
 # Message specifies the status of the traffic sign
 
-uint8 traffic_sign_status
+# ENUM
+uint8 NONE = 0
+uint8 LEFT = 1
+uint8 RIGHT = 2
+uint8 STRAIGHT = 3
+
+uint8 traffic_sign_status

rr_computer_vision/src/computer_vision_main.cpp

@@ -29,9 +29,9 @@ int main(int argc, char** argv)
   // }
 
   // Once race has started, can instantiate the other objects
-  // LaneDetection lane_detection(nh);
+  LaneDetection lane_detection(nh);
   // EndlineDetection endline_detection(nh);
-  // SignDetection sign_detection(nh);
+  SignDetection sign_detection(nh);
   // ComputerVision computer_vision(nh);
 
   ros::spin();

rr_computer_vision/src/sign_detection.cpp

@@ -215,7 +215,7 @@ void SignDetection::ZedCameraImgCallback(const sensor_msgs::ImageConstPtr& right
         uint8_t arrow_status = CheckArrowDir(cv::Mat(right_img, best_bbox).clone());
         ++arrow_status_accumulator[arrow_status];
       }
-      sign_status_msg.traffic_sign_status = NONE;
+      sign_status_msg.traffic_sign_status = rr_cv::sign_status::NONE;
       sign_status_pub_.publish(sign_status_msg);
     }
 
@@ -224,7 +224,7 @@ void SignDetection::ZedCameraImgCallback(const sensor_msgs::ImageConstPtr& right
       // Only start checking arrow direction after 10 consecutive sign frames
       check_arrow = true;
     }
-    sign_status_msg.traffic_sign_status = NONE;
+    sign_status_msg.traffic_sign_status = rr_cv::sign_status::NONE;
     sign_status_pub_.publish(sign_status_msg);
   }
 
@@ -283,7 +283,7 @@ uint8_t SignDetection::CheckArrowDir(const cv::Mat& sign) {
   // Angle ranges 0-180 deg from the vertival axis going CW
   if (angle < 45 || angle > 135) {
     // Sign is straight
-    return STRAIGHT;
+    return rr_cv::sign_status::STRAIGHT;
   } else {
     // Sign is horizontal, need to apply secondary check, summary of check is:
     // Get a line perpendicular to the orientation of the arrow that passes through it's middle
@@ -330,9 +330,9 @@ uint8_t SignDetection::CheckArrowDir(const cv::Mat& sign) {
     // Return arrow direction with highest area
     uint8_t status_out;
     if (left_area > right_area) {
-      status_out = LEFT;
+      status_out = rr_cv::sign_status::LEFT;
     } else {
-      status_out = RIGHT;
+      status_out = rr_cv::sign_status::RIGHT;
     }
 
     return status_out;

rr_mapper/src/laser_mapper.cpp

@@ -279,7 +279,7 @@ void LaserMapper::CombineOccupancyGrid(nav_msgs::OccupancyGrid &to_grid,
   // TODO(jungwook): Handle Resolution Change
   if (to_grid.info.resolution != from_grid.info.resolution)
   {
-    ROS_ERROR("CombineOccupancyGrid: Resolution mismatch.");
+    ROS_ERROR("CombineOccupancyGrid: Resolution mismatch. TO: %f FROM: %f", to_grid.info.resolution, from_grid.info.resolution);
     return;
   }
 
@@ -469,4 +469,4 @@ std::vector<int> LaserMapper::RotateMap(std::vector<int> curr_map, double new_an
   }
 
   return rot_map;
-}
+}

rr_path_planner/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(catkin REQUIRED
   geometry_msgs
   roslint
   rr_common
+  rr_computer_vision
 )
 
 catkin_package(
@@ -21,15 +22,17 @@ catkin_package(
   nav_msgs 
   geometry_msgs
   rr_common
+  rr_computer_vision
   visualization_msgs
   DEPENDS system_lib
 )
 
 roslint_cpp(src/path_planner_main.cpp src/path_planner.cpp include/path_planner.h)
-
 include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
-add_executable(rr_path_planner src/path_planner_main.cpp)
+# include_directories(rr_path_planner ${catkin_EXPORTED_TARGETS})
+add_compile_options(-std=c++11)
 add_library(path_planner src/path_planner.cpp include/path_planner.h)
+add_executable(rr_path_planner src/path_planner_main.cpp)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(rr_path_planner path_planner ${catkin_LIBRARIES})