From 4dfcb8b99f5e58b6b295e4ccd4c36118c73ab54d Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Tue, 28 May 2019 08:26:45 -0500 Subject: [PATCH] As/111 live traffic new detector (#16) * First run at replacing region_tlr_tensorflow with new version. * Fixing order-of-operations CMake bug in TLR. * Fixing another order-of-operations issue in TLR. * Adding main function and fixing srv install. * Fixing launch file for new region_tlr_tensorflow. * Making tensorflow_tlr.py a class. * python global variable fixes in tlr * python self instance ref fixes --- ros/src/.config/rviz/default.rviz | 690 ------------------ .../trafficlight_recognizer/CMakeLists.txt | 62 +- .../trafficlight_recognizer/interface.yaml | 7 +- ...raffic_light_recognition_tensorflow.launch | 6 +- .../region_tlr_tensorflow.cpp | 535 ++++++++++++++ .../region_tlr_tensorflow.h | 80 ++ .../trafficlight_recognizer/__init__.py | 2 +- .../region_tlr_tensorflow.py | 128 ---- .../trafficlight_recognizer/tensorflow_tlr.py | 69 ++ .../nodes/roi_pub/roi_pub.cpp | 332 --------- .../srv/RecognizeLightState.srv | 4 + 11 files changed, 737 insertions(+), 1178 deletions(-) delete mode 100644 ros/src/.config/rviz/default.rviz create mode 100644 ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/region_tlr_tensorflow.cpp create mode 100644 ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/region_tlr_tensorflow.h delete mode 100755 ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/region_tlr_tensorflow.py create mode 100755 ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/tensorflow_tlr.py delete mode 100644 ros/src/computing/perception/detection/trafficlight_recognizer/nodes/roi_pub/roi_pub.cpp create mode 100644 ros/src/computing/perception/detection/trafficlight_recognizer/srv/RecognizeLightState.srv diff --git a/ros/src/.config/rviz/default.rviz b/ros/src/.config/rviz/default.rviz deleted file mode 100644 index 6f4dcded0b2..00000000000 --- a/ros/src/.config/rviz/default.rviz +++ /dev/null @@ -1,690 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Camera1/Visibility1 - - /Points Raw1 - - /Points Raw1/Status1 - - /Local Waypoints1/Status1 - - /Vector Map CenterLines1/Namespaces1 - - /Global Path1/Namespaces1 - - /Local Rollouts1/Namespaces1 - - /GlobalPathAnimation1/Status1 - - /OverlayImage1 - - /OK1 - - /WARN1 - - /ERROR1 - - /FATAL1 - Splitter Ratio: 0.695804 - Tree Height: 369 - - 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: Points Map - - Class: integrated_viewer/ImageViewerPlugin - Image topic: ----- - Lane topic: ----- - Name: ImageViewerPlugin - Point size: 3 - Point topic: ----- - Rect topic: ----- - - Class: autoware_rviz_debug/DecisionMakerPanel - Name: DecisionMakerPanel -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - 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: - Value: false - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - map: - Value: true - sim_base_link: - Value: true - sim_lidar: - Value: true - world: - Value: true - Marker Scale: 5 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - map: - sim_base_link: - sim_lidar: - {} - Update Interval: 0 - Value: true - - Alpha: 0.0500000007 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points Map - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 2 - Size (m): 0.00999999978 - Style: Points - Topic: /points_map - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /vector_map - Name: Vector Map - Namespaces: - cross_road: true - cross_walk: true - road_sign: true - signal: true - stop_line: true - white_line: true - Queue Size: 100 - Value: true - - Class: rviz/Camera - Enabled: false - Image Rendering: overlay - Image Topic: /image_raw - Name: Camera - Overlay Alpha: 0.400000006 - Queue Size: 10 - Transport Hint: raw - Unreliable: false - Value: false - Visibility: - A* Sim Obstacle: true - Behavior State: true - Control Pose: true - Current Pose: true - Detection Range: true - ERROR: true - FATAL: true - Global Path: true - Global Waypoints: true - GlobalPathAnimation: true - Grid: true - Image: true - Local Rollouts: true - Local Waypoints: true - MarkerArray: true - Next Waypoint Mark: true - OK: true - Occupancy Grid Map: true - OverlayImage: true - OverlayText: true - PP Trajectory Mark: true - Points Cluster: true - Points Map: true - Points Raw: true - Safety Box: true - Simulated Obstacle: true - Stixel: true - TF: false - Tracked Contours: true - Value: true - Vector Map: true - Vector Map CenterLines: true - Vehicle Model: true - Velocity (km/h): true - Vscan Points: true - WARN: true - Waypoint Guide: true - Zoom Factor: 1 - - Alpha: 0.300000012 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 11.8534737 - Min Value: 9.73868942 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 137 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points Raw - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 5 - Size (m): 0.00999999978 - Style: Points - Topic: /points_raw - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 0.5 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 255 - Min Color: 0; 0; 0 - Min Intensity: 255 - Name: Vscan Points - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.00999999978 - Style: Points - Topic: /vscan_points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.100000001 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: false - Head Length: 2 - Head Radius: 2 - Name: Control Pose - Shaft Length: 2 - Shaft Radius: 1 - Shape: Arrow - Topic: /control_pose - Unreliable: false - Value: false - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.100000001 - Class: rviz/Pose - Color: 255; 170; 255 - Enabled: false - Head Length: 2 - Head Radius: 2 - Name: Current Pose - Shaft Length: 2 - Shaft Radius: 1 - Shape: Arrow - Topic: /ndt_pose - Unreliable: false - Value: false - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /detection_range - Name: Detection Range - Namespaces: - Crosswalk Detection: true - Decelerate Detection: true - Stop Detection: true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /next_target_mark - Name: Next Waypoint Mark - Namespaces: - next_target_marker: true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /trajectory_circle_mark - Name: PP Trajectory Mark - Namespaces: - trajectory_circle_marker: true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /vscan_linelist - Name: Stixel - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: Vehicle Model - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Points Cluster - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 5 - Size (m): 0.00999999978 - Style: Points - Topic: /points_cluster - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /local_waypoints_mark - Name: Local Waypoints - Namespaces: - local_path_marker: true - local_point_marker: true - local_waypoint_velocity: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /global_waypoints_mark - Name: Global Waypoints - Namespaces: - global_change_flag_lane_1: true - global_lane_waypoint_orientation_marker_1: true - global_velocity_lane_1: true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /waypoint_guide - Name: Waypoint Guide - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /astar_sim_obstacle - Name: A* Sim Obstacle - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 0.699999988 - Class: rviz/Map - Color Scheme: map - Draw Behind: true - Enabled: true - Name: Occupancy Grid Map - Topic: /grid_map_filter_visualization/distance_transform - Unreliable: false - Use Timestamp: false - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /vector_map_center_lines_rviz - Name: Vector Map CenterLines - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /global_waypoints_rviz - Name: Global Path - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /local_trajectories - Name: Local Rollouts - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /detected_polygons - Name: Tracked Contours - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /behavior_state - Name: Behavior State - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /AnimateGlobalPlan - Name: GlobalPathAnimation - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /safety_border - Name: Safety Box - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: jsk_rviz_plugin/BoundingBoxArray - Enabled: true - Name: Simulated Obstacle - Topic: /dp_planner_tracked_boxes - Unreliable: false - Value: true - alpha: 0.800000012 - color: 25; 255; 0 - coloring: Value - line width: 0.00999999978 - only edge: false - show coords: false - - Buffer length: 100 - Class: jsk_rviz_plugin/Plotter2D - Enabled: true - Name: Velocity (km/h) - Show Value: true - Topic: /linear_velocity_viz - Value: true - auto color change: false - auto scale: true - background color: 0; 0; 0 - backround alpha: 0 - border: true - foreground alpha: 0.699999988 - foreground color: 85; 255; 0 - height: 80 - left: 40 - linewidth: 1 - max color: 255; 0; 0 - max value: 1 - min value: -1 - show caption: true - text size: 8 - top: 30 - update interval: 0.0399999991 - width: 80 - - Align Bottom: false - Background Alpha: 0.800000012 - Background Color: 0; 0; 0 - Class: jsk_rviz_plugin/OverlayText - Enabled: true - Foreground Alpha: 0.800000012 - Foreground Color: 25; 255; 240 - Name: OverlayText - Overtake Color Properties: false - Overtake Position Properties: false - Topic: /ndt_monitor/ndt_info_text - Value: true - font: DejaVu Sans Mono - height: 128 - left: 0 - line width: 2 - text size: 12 - top: 0 - width: 128 - - Class: rviz/Image - Enabled: false - Image Topic: /image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - - Class: jsk_rviz_plugin/OverlayImage - Enabled: false - Name: OverlayImage - Topic: /image_raw - Value: false - alpha: 0.800000012 - height: 128 - keep aspect ratio: true - left: 128 - top: 128 - width: 640 - - Align Bottom: false - Background Alpha: 0.800000012 - Background Color: 0; 0; 0 - Class: jsk_rviz_plugin/OverlayText - Enabled: true - Foreground Alpha: 0.800000012 - Foreground Color: 25; 255; 240 - Name: OK - Overtake Color Properties: false - Overtake Position Properties: false - Topic: /health_aggregator/ok_text - Value: true - font: DejaVu Sans Mono - height: 128 - left: 0 - line width: 2 - text size: 12 - top: 0 - width: 128 - - Align Bottom: false - Background Alpha: 0.800000012 - Background Color: 0; 0; 0 - Class: jsk_rviz_plugin/OverlayText - Enabled: true - Foreground Alpha: 0.800000012 - Foreground Color: 25; 255; 240 - Name: WARN - Overtake Color Properties: false - Overtake Position Properties: false - Topic: /health_aggregator/warn_text - Value: true - font: DejaVu Sans Mono - height: 128 - left: 0 - line width: 2 - text size: 12 - top: 0 - width: 128 - - Align Bottom: false - Background Alpha: 0.800000012 - Background Color: 0; 0; 0 - Class: jsk_rviz_plugin/OverlayText - Enabled: true - Foreground Alpha: 0.800000012 - Foreground Color: 25; 255; 240 - Name: ERROR - Overtake Color Properties: false - Overtake Position Properties: false - Topic: /health_aggregator/error_text - Value: true - font: DejaVu Sans Mono - height: 128 - left: 0 - line width: 2 - text size: 12 - top: 0 - width: 128 - - Align Bottom: false - Background Alpha: 0.800000012 - Background Color: 0; 0; 0 - Class: jsk_rviz_plugin/OverlayText - Enabled: true - Foreground Alpha: 0.800000012 - Foreground Color: 25; 255; 240 - Name: FATAL - Overtake Color Properties: false - Overtake Position Properties: false - Topic: /health_aggregator/fatal_text - Value: true - font: DejaVu Sans Mono - height: 128 - left: 0 - line width: 2 - text size: 12 - top: 0 - width: 128 - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /waypoint_saver_marker - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - 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: 0.0299999975 - 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: 13.4510489 - Target Frame: base_link - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - DecisionMakerPanel: - collapsed: false - Displays: - collapsed: false - Height: 896 - Hide Left Dock: false - Hide Right Dock: true - Image: - collapsed: false - ImageViewerPlugin: - collapsed: false - QMainWindow State: 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 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1215 - X: 65 - Y: 24 diff --git a/ros/src/computing/perception/detection/trafficlight_recognizer/CMakeLists.txt b/ros/src/computing/perception/detection/trafficlight_recognizer/CMakeLists.txt index d94e516e1e3..52b40954a64 100644 --- a/ros/src/computing/perception/detection/trafficlight_recognizer/CMakeLists.txt +++ b/ros/src/computing/perception/detection/trafficlight_recognizer/CMakeLists.txt @@ -33,6 +33,7 @@ find_package(catkin REQUIRED COMPONENTS vector_map camera_info_manager image_transport + message_generation ) find_package(OpenCV REQUIRED) @@ -70,6 +71,18 @@ include_directories( ${Eigen3_INCLUDE_DIRS} ) +catkin_python_setup() + +add_service_files( + FILES RecognizeLightState.srv + ) + +generate_messages( + DEPENDENCIES + std_msgs + sensor_msgs + ) + catkin_package( INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS} CATKIN_DEPENDS @@ -87,6 +100,7 @@ catkin_package( vector_map_server visualization_msgs waypoint_follower + message_runtime LIBRARIES libcontext ) @@ -213,22 +227,6 @@ target_link_libraries(roi_extractor add_dependencies(region_tlr ${catkin_EXPORTED_TARGETS}) -### roi_pub ### -include_directories( - ${catkin_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} -) - -add_executable(roi_pub - nodes/roi_pub/roi_pub.cpp - ) - -target_link_libraries(roi_pub - ${catkin_LIBRARIES} - ${OpenCV_LIBS} - libcontext -) - ### label_maker ### find_package(Qt5Core REQUIRED) find_package(Qt5Gui REQUIRED) @@ -275,7 +273,7 @@ target_link_libraries(label_maker ) -install(TARGETS region_tlr feat_proj tlr_tuner roi_extractor roi_pub label_maker libcontext tl_switch +install(TARGETS region_tlr feat_proj tlr_tuner roi_extractor label_maker libcontext tl_switch ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -390,8 +388,34 @@ if (CUDA_FOUND) find_package(TensorFlow QUIET) if (TENSORFLOW_FOUND) - catkin_python_setup() - catkin_install_python(PROGRAMS nodes/region_tlr_tensorflow/trafficlight_recognizer/region_tlr_tensorflow.py + add_executable(region_tlr_tensorflow + nodes/region_tlr_tensorflow/region_tlr_tensorflow.cpp + nodes/region_tlr_tensorflow/region_tlr_tensorflow.h + ) + + target_include_directories(region_tlr_tensorflow PRIVATE + ${catkin_INCLUDE_DIRS} + ${OpenCV_LIBRARIES} + ) + + target_link_libraries(region_tlr_tensorflow + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + libcontext + ) + + add_dependencies(region_tlr_tensorflow + ${catkin_EXPORTED_TARGETS} + ${trafficlight_recognizer_EXPORTED_TARGETS} + ) + + install(TARGETS region_tlr_tensorflow + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + catkin_install_python(PROGRAMS nodes/region_tlr_tensorflow/trafficlight_recognizer/tensorflow_tlr.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) else () message("'CuDNN' or 'tensorflow' is not installed. 'region_tlr_tensorflow' will not be built.") diff --git a/ros/src/computing/perception/detection/trafficlight_recognizer/interface.yaml b/ros/src/computing/perception/detection/trafficlight_recognizer/interface.yaml index 762eddc98b2..b7256de6f54 100644 --- a/ros/src/computing/perception/detection/trafficlight_recognizer/interface.yaml +++ b/ros/src/computing/perception/detection/trafficlight_recognizer/interface.yaml @@ -13,14 +13,11 @@ publish: [/light_color, /sound_player, /tlr_result, /tlr_superimpose_image] subscribe: [/image_raw, /roi_signal] - name: /region_tlr_tensorflow - publish: [/light_color, /sound_player] - subscribe: [/tlr_roi_image] + publish: [/light_color, /sound_player, /tlr_roi_image, /tlr_result, /tlr_superimpose_image] + subscribe: [/image_raw, /roi_signal] - name: /roi_extractor publish: [] subscribe: [/image_raw, /roi_signal] -- name: /roi_pub - publish: [/tlr_roi_image, /tlr_result, /tlr_superimpose_image] - subscribe: [/image_raw, /roi_signal, /config/superimpose] - name: /light_color_switch publish: [/light_color] subscribe: [/camera_light_color, /ams_light_color] diff --git a/ros/src/computing/perception/detection/trafficlight_recognizer/launch/traffic_light_recognition_tensorflow.launch b/ros/src/computing/perception/detection/trafficlight_recognizer/launch/traffic_light_recognition_tensorflow.launch index bdfbc6f0f36..0ec50e94497 100644 --- a/ros/src/computing/perception/detection/trafficlight_recognizer/launch/traffic_light_recognition_tensorflow.launch +++ b/ros/src/computing/perception/detection/trafficlight_recognizer/launch/traffic_light_recognition_tensorflow.launch @@ -3,18 +3,18 @@ - + - + - + diff --git a/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/region_tlr_tensorflow.cpp b/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/region_tlr_tensorflow.cpp new file mode 100644 index 00000000000..95cfcadb90a --- /dev/null +++ b/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/region_tlr_tensorflow.cpp @@ -0,0 +1,535 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "Context.h" +#include "region_tlr_tensorflow.h" + +RegionTLRTensorFlowROSNode::RegionTLRTensorFlowROSNode() : + image_topic_name_("/image_raw"), + kAdvertiseInLatch_(true), + kTrafficLightRed(0), + kTrafficLightGreen(1), + kTrafficLightUnknown(2), + kStringRed("red signal"), + kStringGreen("green signal"), + kStringUnknown("") +{ +} + +RegionTLRTensorFlowROSNode::~RegionTLRTensorFlowROSNode() +{ +} + +void RegionTLRTensorFlowROSNode::RunRecognition() +{ + GetROSParam(); // get execution parameters from ROS parameter server + StartSubscribersAndPublishers(); + ROS_INFO("Node initialized, waiting for signals from feat_proj..."); + ros::spin(); +} + +void RegionTLRTensorFlowROSNode::ImageRawCallback(const sensor_msgs::ImageConstPtr &msg) +{ + auto se_frame = cv_bridge::toCvShare(msg); + + if (msg->encoding == "bgr8") + cv::cvtColor(se_frame->image, frame_, cv::COLOR_BGR2RGB); + else if (msg->encoding == "bayer_bggr8") + cv::cvtColor(se_frame->image, frame_, cv::COLOR_BayerRG2RGB); + else if (msg->encoding == "bayer_rggb8") + cv::cvtColor(se_frame->image, frame_, cv::COLOR_BayerBG2RGB); + else + ROS_WARN("Received image of unhandled encoding type."); + + frame_header_ = msg->header; +} + +void RegionTLRTensorFlowROSNode::ROISignalCallback(const autoware_msgs::Signals::ConstPtr &extracted_pos) +{ + static ros::Time previous_timestamp; + + // Abort this callback if a new image is not available + if (frame_.empty() || + frame_header_.stamp == previous_timestamp) + return; + + // Acquire signal position on the image + Context::SetContexts(contexts_, extracted_pos, frame_.rows, frame_.cols); + + // Recognize the color of the traffic light + int counter = 0; + for (Context &context: contexts_) + { + if (context.topLeft.x > context.botRight.x) + continue; + + counter++; + + // Extract ROI + cv::Mat roi = frame_(cv::Rect(context.topLeft, context.botRight)).clone(); + cv_bridge::CvImage converter; + converter.header = frame_header_; + converter.encoding = sensor_msgs::image_encodings::RGB8; + converter.image = roi; + sensor_msgs::ImagePtr roi_img = converter.toImageMsg(); + + // Publish the ROI image for top signal in vector (top signal has largest estimated + // radius in every signals projected in a image) + if (counter == 1) + { + roi_image_publisher.publish(converter.toImageMsg()); + } + + // Get current state of traffic light from current frame + trafficlight_recognizer::RecognizeLightState srv; + srv.request.roi_image = *roi_img; + LightState current_state = LightState::UNDEFINED; + + double confidence = 0.0; + if (srv_client.call(srv)) + { + ROS_INFO("light state: %d (%f)", srv.response.class_id, srv.response.confidence); + current_state = (LightState)srv.response.class_id; + confidence = srv.response.confidence; + } + else + { + ROS_ERROR("Failed to call service recognize_light_state"); + return; + } + + // The state of the traffic light WON'T be changed + // unless the new state is found at least change_state_threshold_ times + if (confidence >= score_threshold_) + DetermineState(current_state, context); + } + + if (extracted_pos->Signals.size() == 0) + std::cout << "No signals in the image" << std::endl; + else + { + // Publish recognition result + PublishTrafficLight(contexts_); + } + + PublishString(contexts_); + PublishMarkerArray(contexts_); + PublishImage(contexts_); + + // Save timestamp of this frame so that same frame has never been process again + previous_timestamp = frame_header_.stamp; +} + +void RegionTLRTensorFlowROSNode::GetROSParam() +{ + ros::NodeHandle private_node_handle("~"); + + private_node_handle.param("image_raw_topic", image_topic_name_, "/image_raw"); + ROS_INFO("image_raw_topic: %s", image_topic_name_.c_str()); + + private_node_handle.param("change_state_threshold", change_state_threshold_, 2); + ROS_INFO("change_state_threshold: %d", change_state_threshold_); + + private_node_handle.param("score_threshold", score_threshold_, 0.6); + ROS_INFO("score_threshold: %f", score_threshold_); +} + +void RegionTLRTensorFlowROSNode::StartSubscribersAndPublishers() +{ + ros::NodeHandle node_handle; + + // Register subscribers + image_transport::ImageTransport it(node_handle); + image_subscriber = it.subscribe(image_topic_name_, + 1, + &RegionTLRTensorFlowROSNode::ImageRawCallback, + this); + + roi_signal_subscriber = node_handle.subscribe("/roi_signal", + 1, + &RegionTLRTensorFlowROSNode::ROISignalCallback, + this); + + // Register publishers + signal_state_publisher = node_handle.advertise("light_color", 1); + signal_state_string_publisher = node_handle.advertise("sound_player", 1); + marker_publisher = node_handle.advertise("tlr_result", 1, kAdvertiseInLatch_); + superimpose_image_publisher = node_handle.advertise("tlr_superimpose_image", 1); + roi_image_publisher = node_handle.advertise("tlr_roi_image", 1); +} + +/*! + * DetermineState works as a latch to reduce the chance of sudden changes in the state of the traffic light, caused by + * misclassifications in the detector. To change the traffic light state, the new candidate should be found at + * least kChangeStateThreshold times. + * @param current_state the current state of the traffic light as reported by the classifier. + * @param in_out_signal_context the object containing the data of the current Traffic Light instance. + */ +void RegionTLRTensorFlowROSNode::DetermineState(LightState in_current_state, + Context& in_out_signal_context) +{ + //if reported state by classifier is different than the previously stored + if (in_current_state != in_out_signal_context.lightState) + { + //and also different from the previous difference + if (in_current_state != in_out_signal_context.newCandidateLightState) + { + //set classifier result as a candidate + in_out_signal_context.newCandidateLightState = in_current_state; + in_out_signal_context.stateJudgeCount = 0; + } + else + { + //if classifier returned the same result previously, then increase its confidence + in_out_signal_context.stateJudgeCount++; + if (in_out_signal_context.stateJudgeCount > change_state_threshold_) + in_out_signal_context.stateJudgeCount = change_state_threshold_; // prevent overflow + + //if new candidate has been found enough times, change state to the new candidate + if (in_out_signal_context.stateJudgeCount >= change_state_threshold_) + { + in_out_signal_context.lightState = in_current_state; + } + } + } +} + +void RegionTLRTensorFlowROSNode::PublishTrafficLight(std::vector contexts) +{ + autoware_msgs::TrafficLight topic; + static int32_t previous_state = kTrafficLightUnknown; + topic.traffic_light = kTrafficLightUnknown; + + for (const auto ctx: contexts) + { + switch (ctx.lightState) + { + case GREEN: + topic.traffic_light = kTrafficLightGreen; + break; + case YELLOW: + case RED: + topic.traffic_light = kTrafficLightRed; + break; + case UNDEFINED: + topic.traffic_light = kTrafficLightUnknown; + break; + } + + // Publish the first state in contexts, + // which has largest estimated radius of signal. + // This program assume that the signal which has the largest estimated radius + // equal the nearest one from camera. + if (topic.traffic_light != kTrafficLightUnknown) + { + break; + } + } + + // If state changes from previous one, publish it + static ros::Time prev_time = ros::Time::now(); + double timeout = 10.0; //seconds + if (topic.traffic_light != previous_state || ros::Time::now() - prev_time > ros::Duration(timeout)) + { + prev_time = ros::Time::now(); + signal_state_publisher.publish(topic); + previous_state = topic.traffic_light; + } +} + +void RegionTLRTensorFlowROSNode::PublishString(std::vector contexts) +{ + std_msgs::String topic; + static std::string previous_state = kStringUnknown; + topic.data = kStringUnknown; + for (const auto ctx: contexts) + { + switch (ctx.lightState) + { + case GREEN: + topic.data = kStringGreen; + break; + case YELLOW: + case RED: + topic.data = kStringRed; + break; + case UNDEFINED: + topic.data = kStringUnknown; + break; + } + + // Publish the first state in contexts, + // which has largest estimated radius of signal. + // This program assume that the signal which has the largest estimated radius + // equal the nearest one from camera. + if (topic.data != kStringUnknown) + { + break; + } + } + + // If state changes from previous one, publish it + if (topic.data != previous_state) + { + signal_state_string_publisher.publish(topic); + previous_state = topic.data; + } +} + +void RegionTLRTensorFlowROSNode::PublishMarkerArray(std::vector contexts) +{ + // Define color constants + std_msgs::ColorRGBA color_black; + color_black.r = 0.0f; + color_black.g = 0.0f; + color_black.b = 0.0f; + color_black.a = 1.0f; + + std_msgs::ColorRGBA color_red; + color_red.r = 1.0f; + color_red.g = 0.0f; + color_red.b = 0.0f; + color_red.a = 1.0f; + + std_msgs::ColorRGBA color_yellow; + color_yellow.r = 1.0f; + color_yellow.g = 1.0f; + color_yellow.b = 0.0f; + color_yellow.a = 1.0f; + + std_msgs::ColorRGBA color_green; + color_green.r = 0.0f; + color_green.g = 1.0f; + color_green.b = 0.0f; + color_green.a = 1.0f; + + // publish all result as ROS MarkerArray + for (const auto ctx: contexts) + { + visualization_msgs::MarkerArray signal_set; + visualization_msgs::Marker red_light, yellow_light, green_light; + + // Set the frame ID + red_light.header.frame_id = "map"; + yellow_light.header.frame_id = "map"; + green_light.header.frame_id = "map"; + + // Set the namespace and ID for this markers + red_light.ns = "tlr_result_red"; + red_light.id = ctx.signalID; + + yellow_light.ns = "tlr_result_yellow"; + yellow_light.id = ctx.signalID; + + green_light.ns = "tlr_result_green"; + green_light.id = ctx.signalID; + + // Set the markers type + red_light.type = visualization_msgs::Marker::SPHERE; + yellow_light.type = visualization_msgs::Marker::SPHERE; + green_light.type = visualization_msgs::Marker::SPHERE; + + // Set the pose of the markers + red_light.pose.position.x = ctx.redCenter3d.x; + red_light.pose.position.y = ctx.redCenter3d.y; + red_light.pose.position.z = ctx.redCenter3d.z; + red_light.pose.orientation.x = 0.0; + red_light.pose.orientation.y = 0.0; + red_light.pose.orientation.z = 0.0; + red_light.pose.orientation.w = 0.0; + + yellow_light.pose.position.x = ctx.yellowCenter3d.x; + yellow_light.pose.position.y = ctx.yellowCenter3d.y; + yellow_light.pose.position.z = ctx.yellowCenter3d.z; + yellow_light.pose.orientation.x = 0.0; + yellow_light.pose.orientation.y = 0.0; + yellow_light.pose.orientation.z = 0.0; + yellow_light.pose.orientation.w = 0.0; + + green_light.pose.position.x = ctx.greenCenter3d.x; + green_light.pose.position.y = ctx.greenCenter3d.y; + green_light.pose.position.z = ctx.greenCenter3d.z; + green_light.pose.orientation.x = 0.0; + green_light.pose.orientation.y = 0.0; + green_light.pose.orientation.z = 0.0; + green_light.pose.orientation.w = 0.0; + + // Set the scale of the markers. We assume lamp radius is 30cm in real world + red_light.scale.x = 0.3; + red_light.scale.y = 0.3; + red_light.scale.z = 0.3; + + yellow_light.scale.x = 0.3; + yellow_light.scale.y = 0.3; + yellow_light.scale.z = 0.3; + + green_light.scale.x = 0.3; + green_light.scale.y = 0.3; + green_light.scale.z = 0.3; + + // Set the color for each marker + switch (ctx.lightState) + { + case GREEN: + red_light.color = color_black; + yellow_light.color = color_black; + green_light.color = color_green; + break; + case YELLOW: + red_light.color = color_black; + yellow_light.color = color_yellow; + green_light.color = color_black; + break; + case RED: + red_light.color = color_red; + yellow_light.color = color_black; + green_light.color = color_black; + break; + case UNDEFINED: + red_light.color = color_black; + yellow_light.color = color_black; + green_light.color = color_black; + break; + } + + red_light.lifetime = ros::Duration(0.1); + yellow_light.lifetime = ros::Duration(0.1); + green_light.lifetime = ros::Duration(0.1); + + // Pack each light marker into one + signal_set.markers.push_back(red_light); + signal_set.markers.push_back(yellow_light); + signal_set.markers.push_back(green_light); + + // Publish + marker_publisher.publish(signal_set); + } +} + +void RegionTLRTensorFlowROSNode::PublishImage(std::vector contexts) +{ + // Copy the frame image for output + cv::Mat result_image; + cv::cvtColor(frame_, result_image, cv::COLOR_RGB2BGR); + + // Define information for written label + std::string label; + int kFontFace = cv::FONT_HERSHEY_COMPLEX_SMALL; + int kThickness = 1; + double kFontScale = 0.8; + int font_baseline = 0; + CvScalar label_color; + + int counter = 0; + for (const auto ctx: contexts) + { + // Draw superimpose result on image + + // circle(result_image, ctx.redCenter, ctx.lampRadius, CV_RGB(255, 0, 0), 1, 0); + // circle(result_image, ctx.yellowCenter, ctx.lampRadius, CV_RGB(255, 255, 0), 1, 0); + // circle(result_image, ctx.greenCenter, ctx.lampRadius, CV_RGB(0, 255, 0), 1, 0); + + // Draw recognition result on image + switch (ctx.lightState) + { + case GREEN: + label = "GREEN"; + label_color = CV_RGB(0, 255, 0); + break; + case YELLOW: + label = "YELLOW"; + label_color = CV_RGB(255, 255, 0); + break; + case RED: + label = "RED"; + label_color = CV_RGB(255, 0, 0); + break; + case UNDEFINED: + label = "UNKNOWN"; + label_color = CV_RGB(0, 0, 0); + } + + cv::rectangle(result_image, ctx.topLeft, ctx.botRight, label_color, 2); + + if (ctx.leftTurnSignal) + { + label += " LEFT"; + } + if (ctx.rightTurnSignal) + { + label += " RIGHT"; + } + + // Add lane ID text + label += " " + std::to_string(ctx.closestLaneId); + cv::Point label_origin = cv::Point(ctx.topLeft.x, ctx.botRight.y + font_baseline); + cv::putText(result_image, label, label_origin, kFontFace, kFontScale, label_color, kThickness); + } + + std_msgs::String topic; + for (const auto ctx: contexts) + { + switch (ctx.lightState) + { + case GREEN: + topic.data = kStringGreen; + label_color = CV_RGB(0, 255, 0); + break; + case YELLOW: + case RED: + topic.data = kStringRed; + label_color = CV_RGB(255, 0, 0); + break; + case UNDEFINED: + topic.data = kStringUnknown; + label_color = CV_RGB(0, 0, 0); + } + + // Publish the first state in contexts, + // which has largest estimated radius of signal. + // This program assume that the signal which has the largest estimated radius + // equal the nearest one from camera. + if (topic.data != kStringUnknown) + break; + } + + // Add text with state of closest signal + kFontFace = cv::FONT_HERSHEY_TRIPLEX; + kFontScale *= 3; + kThickness *= 3; + std::transform(topic.data.begin(), topic.data.end(), topic.data.begin(), ::toupper); + cv::Size textSize = cv::getTextSize(topic.data, kFontFace, kFontScale, kThickness, &font_baseline); + cv::Point label_origin((frame_.cols - textSize.width) / 2, 3 * (frame_.rows + textSize.height) / 4); + cv::putText(result_image, topic.data, label_origin, kFontFace, kFontScale, label_color, kThickness); + + // Publish superimpose result image + cv_bridge::CvImage converter; + converter.header = frame_header_; + converter.encoding = sensor_msgs::image_encodings::BGR8; + converter.image = result_image; + superimpose_image_publisher.publish(converter.toImageMsg()); +} + +int main(int argc, char *argv[]) +{ + // Initialize ros node + ros::init(argc, argv, "region_tlr_tensorflow_node"); + ros::NodeHandle n; + + // Create RegionTLRTensorFlowROSNode class object and do initialization + RegionTLRTensorFlowROSNode region_tlr_tensorflow_ros_node; + ros::service::waitForService("recognize_light_state", -1); + region_tlr_tensorflow_ros_node.srv_client = n.serviceClient("recognize_light_state"); + + // Start recognition process + region_tlr_tensorflow_ros_node.RunRecognition(); + + return 0; +} diff --git a/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/region_tlr_tensorflow.h b/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/region_tlr_tensorflow.h new file mode 100644 index 00000000000..ca6e2560303 --- /dev/null +++ b/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/region_tlr_tensorflow.h @@ -0,0 +1,80 @@ +#ifndef REGION_TLR_TENSORFLOW_H +#define REGION_TLR_TENSORFLOW_H + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "Context.h" +#include "autoware_msgs/Signals.h" + +class RegionTLRTensorFlowROSNode +{ +public: + RegionTLRTensorFlowROSNode(); + ~RegionTLRTensorFlowROSNode(); + + void RunRecognition(); + void ImageRawCallback(const sensor_msgs::ImageConstPtr &msg); + void ROISignalCallback(const autoware_msgs::Signals::ConstPtr &extracted_pos); + + // The vector of data structure to save traffic light state, position, ...etc + std::vector contexts_; + + // Service client + ros::ServiceClient srv_client; + +private: + void GetROSParam(); + void StartSubscribersAndPublishers(); + void DetermineState(LightState in_current_state, Context& in_out_signal_context); + void PublishTrafficLight(std::vector contexts); + void PublishString(std::vector contexts); + void PublishMarkerArray(std::vector contexts); + void PublishImage(std::vector contexts); + void SuperimposeCb(const std_msgs::Bool::ConstPtr &config_msg); + + // Execution parameter + std::string image_topic_name_; + double score_threshold_; + int change_state_threshold_;// The threshold of state detected times to accept the state change + bool swap_pole_lane_id; + + // Subscribers + image_transport::Subscriber image_subscriber; + ros::Subscriber roi_signal_subscriber; + ros::Subscriber superimpose_sub; + + // Publishers + ros::Publisher signal_state_publisher; + ros::Publisher signal_state_string_publisher; + ros::Publisher marker_publisher; + ros::Publisher superimpose_image_publisher; + ros::Publisher roi_image_publisher; + + // Flag to show topic will be published in latch manner + bool kAdvertiseInLatch_; + + // A frame image acquired from topic + cv::Mat frame_; + + // Timestamp of a frame in process + std_msgs::Header frame_header_; + + // constant values to pass recognition states to other nodes + const int32_t kTrafficLightRed; + const int32_t kTrafficLightGreen; + const int32_t kTrafficLightUnknown; + const std::string kStringRed; + const std::string kStringGreen; + const std::string kStringUnknown; +}; + +#endif // REGION_TLR_TENSORFLOW_H diff --git a/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/__init__.py b/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/__init__.py index 43ae9b13d74..e265ba34fd4 100644 --- a/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/__init__.py +++ b/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/__init__.py @@ -1 +1 @@ -from region_tlr_tensorflow import * +from tensorflow_tlr import * diff --git a/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/region_tlr_tensorflow.py b/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/region_tlr_tensorflow.py deleted file mode 100755 index a6308f17eaf..00000000000 --- a/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/region_tlr_tensorflow.py +++ /dev/null @@ -1,128 +0,0 @@ -#!/usr/bin/env python - -import rospy -from sensor_msgs.msg import Image -from std_msgs.msg import String -from cv_bridge import CvBridge -import cv2 -import numpy as np -import tensorflow as tf -from keras.models import load_model -from keras.preprocessing.image import img_to_array -from autoware_msgs.msg import TrafficLight - -USE_ALT_COLORSPACE = False -TARGET_SIZE = (32, 32) -NUM_CHANNELS = 3 -IDX_DEBOUNCE_THRESHOLD = 2 -CONFIDENCE_THRESHOLD = 0.6 -PUB_TIMEOUT = 10.0 # seconds -ROI_IMG_TIMEOUT = 1.0 # seconds -states = ['red', 'green', 'off'] -# internal_state_map = {0:'green', 1:'off', 2:'red', 3:'yellow' } -classifier_state_map = {0: 1, 1: 2, 2: 0, 3: 0} # note, treating yellow as red - -classifier_state_strings = {0: 'red signal', 1: 'green signal', 2: ''} - -config = tf.ConfigProto() -config.gpu_options.allow_growth = True -session = tf.Session(config=config) - -cv_bridge = CvBridge() -idx_debounce_count = 0 -last_pub_state_idx = -1 -last_debounce_state_idx = -1 -img_timer = None - -result_msg = TrafficLight() - - -def preprocess_image(img): - out_img = cv2.cvtColor(img, cv2.COLOR_BGR2YCrCb) - return out_img - - -def ROI_image_callback(image_msg): - global idx_debounce_count, \ - IDX_DEBOUNCE_THRESHOLD, \ - last_pub_state_idx, \ - last_debounce_state_idx, \ - last_pub_time, \ - img_timer - - if img_timer is not None: - img_timer.shutdown() - - img_timer = rospy.Timer(rospy.Duration(ROI_IMG_TIMEOUT), ImgTimeoutCallback) - last_ROI_img_time = rospy.get_rostime() - - # Resize the input image then feed it into the NN for prediction - cv_image = cv_bridge.imgmsg_to_cv2(image_msg, "passthrough") - - if USE_ALT_COLORSPACE: - cv_image = preprocess_image(cv_image) - - img = cv2.resize(cv_image, TARGET_SIZE, interpolation=cv2.INTER_LINEAR) - img = img.astype("float") / 255.0 - img = img_to_array(img) - img = np.expand_dims(img, axis=0) - - proba = nn_model.predict(img) - confidence = np.amax(proba) - predicted_state = nn_model.predict_classes(img) - - idx = -1 - if confidence >= CONFIDENCE_THRESHOLD: - # Get the prediction - idx = np.argmax(proba) - idx = classifier_state_map[idx] - - # Debounce the prediction before publishing - if idx == last_debounce_state_idx: - idx_debounce_count += 1 - if idx_debounce_count > IDX_DEBOUNCE_THRESHOLD: - idx_debounce_count = IDX_DEBOUNCE_THRESHOLD # prevent overflow - else: - last_debounce_state_idx = idx - idx_debounce_count = 0 - - if idx_debounce_count >= IDX_DEBOUNCE_THRESHOLD: - # Only consider publishing debounced states - if last_debounce_state_idx != last_pub_state_idx or \ - (rospy.get_rostime() - last_pub_time > rospy.Duration(PUB_TIMEOUT) and - last_pub_state_idx != 2): - # Only publish when state changes or a timeout occurs (but only want to repeat non-"unknown" states) - result_msg.traffic_light = last_debounce_state_idx - light_color_pub.publish(result_msg) - - if last_debounce_state_idx != last_pub_state_idx: - light_color_string_pub.publish(classifier_state_strings[last_debounce_state_idx]) - - last_pub_state_idx = last_debounce_state_idx - last_pub_time = rospy.get_rostime() - - -def ImgTimeoutCallback(event): - global last_pub_state_idx, last_pub_time - if last_pub_state_idx != 2: - result_msg.traffic_light = 2 - light_color_pub.publish(result_msg) - light_color_string_pub.publish(classifier_state_strings[2]) - last_pub_state_idx = 2 - last_pub_time = rospy.get_rostime() - -rospy.init_node('tlr_classifier') - -nn_model_path = rospy.get_param('~nn_model_path') -nn_model = load_model(nn_model_path) - -# Have to do this or the prediction in the callback fails -proba = nn_model.predict(np.zeros((1, TARGET_SIZE[0], TARGET_SIZE[1], NUM_CHANNELS))) - -rospy.Subscriber('tlr_roi_image', Image, ROI_image_callback, queue_size=1) -light_color_pub = rospy.Publisher('light_color', TrafficLight, queue_size=1) -light_color_string_pub = rospy.Publisher('sound_player', String, queue_size=1) - -last_pub_time = rospy.get_rostime() - -rospy.spin() diff --git a/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/tensorflow_tlr.py b/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/tensorflow_tlr.py new file mode 100755 index 00000000000..2c48c1f37f2 --- /dev/null +++ b/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/region_tlr_tensorflow/trafficlight_recognizer/tensorflow_tlr.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python + +from sensor_msgs.msg import Image +from std_msgs.msg import String, Float32 +from trafficlight_recognizer.srv import * +import cv2 +import numpy as np +import rospy +from cv_bridge import CvBridge +import tensorflow as tf +from keras.models import load_model +from keras.preprocessing.image import img_to_array + +class RegionTlrTensorFlow: + def __init__(self): + self.USE_ALT_COLORSPACE = False + self.TARGET_SIZE = (32, 32) + self.NUM_CHANNELS = 3 + self.CLASSIFIER_STATE_MAP = {0:0, 1:3, 2:2, 3:1} # map from trained NN ordering to Autoware ordering + + self.cv_bridge = CvBridge() + + def preprocess_image(self, img): + if self.USE_ALT_COLORSPACE: + out_img = cv2.cvtColor(img, cv2.COLOR_BGR2YCrCb) + else: + out_img = img.copy() + + out_img = cv2.resize(out_img, self.TARGET_SIZE, interpolation=cv2.INTER_LINEAR) + out_img = out_img.astype("float") / 255.0 # normalize to [0, 1] + out_img = img_to_array(out_img) + out_img = np.expand_dims(out_img, axis=0) # add dimension for batch index + + return out_img + + def recognize_light_state(self, req): + # Prepare the input image then feed it into the NN for prediction + cv_image = self.cv_bridge.imgmsg_to_cv2(req.roi_image, "passthrough") + img = self.preprocess_image(cv_image) + + # Make the class prediction for this image and get a confidence value + proba = self.nn_model.predict(img) + confidence = np.amax(proba) + class_id = self.CLASSIFIER_STATE_MAP[np.argmax(proba)] + return [class_id, confidence] + + def run(self): + rospy.init_node('tensorflow_tlr') + + # Workaround for cuDNN issue + config = tf.ConfigProto() + config.gpu_options.allow_growth = True + session = tf.Session(config=config) + + # Setup the neural network + self.nn_model_path = rospy.get_param('~nn_model_path') + self.nn_model = load_model(self.nn_model_path) + + # Have to do this or the prediction in the callback fails + proba = self.nn_model.predict(np.zeros((1, self.TARGET_SIZE[0], self.TARGET_SIZE[1], self.NUM_CHANNELS))) + + # Setup service + self.service = rospy.Service('recognize_light_state', RecognizeLightState, self.recognize_light_state) + + rospy.spin() + +if __name__ == "__main__": + region_tlr_tensorflow = RegionTlrTensorFlow() + region_tlr_tensorflow.run() diff --git a/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/roi_pub/roi_pub.cpp b/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/roi_pub/roi_pub.cpp deleted file mode 100644 index a9d314b2d18..00000000000 --- a/ros/src/computing/perception/detection/trafficlight_recognizer/nodes/roi_pub/roi_pub.cpp +++ /dev/null @@ -1,332 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -static const std::string WINDOW_NAME = "superimpose result"; - -// A frame image acquired from topic -cv::Mat frame_; - -std::string image_topic_name; -bool swap_pole_lane_id; - -// Timestamp of a frame in process -std_msgs::Header frame_header_; - -// Publishers -ros::Publisher roi_image_publisher; -ros::Publisher marker_publisher; -ros::Publisher superimpose_image_publisher; - -void PublishMarkerArray(std::vector contexts) -{ - // Define color constants - std_msgs::ColorRGBA color_black; - color_black.r = 0.0f; - color_black.g = 0.0f; - color_black.b = 0.0f; - color_black.a = 1.0f; - - std_msgs::ColorRGBA color_red; - color_red.r = 1.0f; - color_red.g = 0.0f; - color_red.b = 0.0f; - color_red.a = 1.0f; - - std_msgs::ColorRGBA color_yellow; - color_yellow.r = 1.0f; - color_yellow.g = 1.0f; - color_yellow.b = 0.0f; - color_yellow.a = 1.0f; - - std_msgs::ColorRGBA color_green; - color_green.r = 0.0f; - color_green.g = 1.0f; - color_green.b = 0.0f; - color_green.a = 1.0f; - - // publish all result as ROS MarkerArray - for (const auto ctx : contexts) - { - visualization_msgs::MarkerArray signal_set; - visualization_msgs::Marker red_light, yellow_light, green_light; - - // Set the frame ID - red_light.header.frame_id = "map"; - yellow_light.header.frame_id = "map"; - green_light.header.frame_id = "map"; - - // Set the namespace and ID for this markers - red_light.ns = "tlr_result_red"; - red_light.id = ctx.signalID; - - yellow_light.ns = "tlr_result_yellow"; - yellow_light.id = ctx.signalID; - - green_light.ns = "tlr_result_green"; - green_light.id = ctx.signalID; - - // Set the markers type - red_light.type = visualization_msgs::Marker::SPHERE; - yellow_light.type = visualization_msgs::Marker::SPHERE; - green_light.type = visualization_msgs::Marker::SPHERE; - - // Set the pose of the markers - red_light.pose.position.x = ctx.redCenter3d.x; - red_light.pose.position.y = ctx.redCenter3d.y; - red_light.pose.position.z = ctx.redCenter3d.z; - red_light.pose.orientation.x = 0.0; - red_light.pose.orientation.y = 0.0; - red_light.pose.orientation.z = 0.0; - red_light.pose.orientation.w = 0.0; - - yellow_light.pose.position.x = ctx.yellowCenter3d.x; - yellow_light.pose.position.y = ctx.yellowCenter3d.y; - yellow_light.pose.position.z = ctx.yellowCenter3d.z; - yellow_light.pose.orientation.x = 0.0; - yellow_light.pose.orientation.y = 0.0; - yellow_light.pose.orientation.z = 0.0; - yellow_light.pose.orientation.w = 0.0; - - green_light.pose.position.x = ctx.greenCenter3d.x; - green_light.pose.position.y = ctx.greenCenter3d.y; - green_light.pose.position.z = ctx.greenCenter3d.z; - green_light.pose.orientation.x = 0.0; - green_light.pose.orientation.y = 0.0; - green_light.pose.orientation.z = 0.0; - green_light.pose.orientation.w = 0.0; - - // Set the scale of the markers. We assume lamp radius is 30cm in real world - red_light.scale.x = 0.3; - red_light.scale.y = 0.3; - red_light.scale.z = 0.3; - - yellow_light.scale.x = 0.3; - yellow_light.scale.y = 0.3; - yellow_light.scale.z = 0.3; - - green_light.scale.x = 0.3; - green_light.scale.y = 0.3; - green_light.scale.z = 0.3; - - // Set the color for each marker - switch (ctx.lightState) - { - case GREEN: - red_light.color = color_black; - yellow_light.color = color_black; - green_light.color = color_green; - break; - case YELLOW: - red_light.color = color_black; - yellow_light.color = color_yellow; - green_light.color = color_black; - break; - case RED: - red_light.color = color_red; - yellow_light.color = color_black; - green_light.color = color_black; - break; - case UNDEFINED: - red_light.color = color_black; - yellow_light.color = color_black; - green_light.color = color_black; - break; - } - - red_light.lifetime = ros::Duration(0.1); - yellow_light.lifetime = ros::Duration(0.1); - green_light.lifetime = ros::Duration(0.1); - - // Pack each light marker into one - signal_set.markers.push_back(red_light); - signal_set.markers.push_back(yellow_light); - signal_set.markers.push_back(green_light); - - // Publish - marker_publisher.publish(signal_set); - } -} // void PublishMarkerArray() - -void PublishImage(std::vector contexts) -{ - // Copy the frame image for output - cv::Mat result_image = frame_.clone(); - cv::cvtColor(result_image, result_image, cv::COLOR_RGB2BGR); - - // Define information for written label - std::string label; - const int k_font_face = cv::FONT_HERSHEY_COMPLEX_SMALL; - const double k_font_scale = 0.8; - int font_baseline = 0; - CvScalar label_color; - - for (const auto ctx : contexts) - { - // Draw superimpose result on image - circle(result_image, ctx.redCenter, ctx.lampRadius, CV_RGB(255, 0, 0), 1, 0); - circle(result_image, ctx.yellowCenter, ctx.lampRadius, CV_RGB(255, 255, 0), 1, 0); - circle(result_image, ctx.greenCenter, ctx.lampRadius, CV_RGB(0, 255, 0), 1, 0); - - // Draw recognition result on image - switch (ctx.lightState) - { - case GREEN: - label = "GREEN"; - label_color = CV_RGB(0, 255, 0); - break; - case YELLOW: - label = "YELLOW"; - label_color = CV_RGB(255, 255, 0); - break; - case RED: - label = "RED"; - label_color = CV_RGB(255, 0, 0); - break; - case UNDEFINED: - label = "UNKNOWN"; - label_color = CV_RGB(0, 0, 0); - } - - if (ctx.leftTurnSignal) - { - label += " LEFT"; - } - if (ctx.rightTurnSignal) - { - label += " RIGHT"; - } - // add lane # text - label += " " + std::to_string(ctx.closestLaneId); - - cv::Point label_origin = cv::Point(ctx.topLeft.x, ctx.botRight.y + font_baseline); - - cv::putText(result_image, label, label_origin, k_font_face, k_font_scale, label_color); - } - - // Publish superimpose result image - cv_bridge::CvImage converter; - converter.header = frame_header_; - converter.encoding = sensor_msgs::image_encodings::BGR8; - converter.image = result_image; - superimpose_image_publisher.publish(converter.toImageMsg()); -} // void PublishImage() - -void superimposeCb(const std_msgs::Bool::ConstPtr& config_msg) -{ - bool show_superimpose_result = config_msg->data; - - if (show_superimpose_result) - { - cv::namedWindow(WINDOW_NAME, cv::WINDOW_NORMAL); - cv::startWindowThread(); - } - - if (!show_superimpose_result) - { - if (cvGetWindowHandle(WINDOW_NAME.c_str()) != NULL) - { - cv::destroyWindow(WINDOW_NAME); - cv::waitKey(1); - } - } -} // void superimposeCb() - -void imageCallback(const sensor_msgs::ImageConstPtr& msg) -{ - auto se_frame = cv_bridge::toCvShare(msg); - - if (msg->encoding == "bgr8") - cv::cvtColor(se_frame->image, frame_, cv::COLOR_BGR2RGB); - else if (msg->encoding == "bayer_bggr8") - cv::cvtColor(se_frame->image, frame_, cv::COLOR_BayerRG2RGB); - else if (msg->encoding == "bayer_rggb8") - cv::cvtColor(se_frame->image, frame_, cv::COLOR_BayerBG2RGB); - else - ROS_WARN("Received image of unhandled encoding type."); - - frame_header_ = msg->header; -} - -void roiSignalCallback(const autoware_msgs::Signals::ConstPtr& extracted_pos) -{ - static ros::Time previous_timestamp; - std::vector contexts_; - // If frame has not been prepared, abort this callback - if (frame_.empty() || frame_header_.stamp == previous_timestamp) - { - std::cout << "No Image" << std::endl; - return; - } - - // Acquire signal position on the image - // Context::SetContexts(contexts_, extracted_pos, frame_.rows, frame_.cols, swap_pole_lane_id); - Context::SetContexts(contexts_, extracted_pos, frame_.rows, frame_.cols); - - // Publish recognition result as some topic format - if (extracted_pos->Signals.size() == 0 || contexts_.size() == 0) - { - std::cout << "no signals in the image" << std::endl; - } - else - { - // Extract ROI for top signal in vector (top signal has largest estimated - // radius in every signals projected in a image) - cv::Mat roi = frame_(cv::Rect(contexts_.at(0).topLeft, contexts_.at(0).botRight)); - - cv_bridge::CvImage converter; - converter.header = frame_header_; - converter.encoding = sensor_msgs::image_encodings::RGB8; - converter.image = roi; - roi_image_publisher.publish(converter.toImageMsg()); - } - - PublishMarkerArray(contexts_); - PublishImage(contexts_); - - previous_timestamp = frame_header_.stamp; -} - -void getROSParams() -{ - ros::NodeHandle private_node_handle("~"); - private_node_handle.param("image_raw_topic", image_topic_name, "/image_raw"); - ROS_INFO("image_raw_topic: %s", image_topic_name.c_str()); - - private_node_handle.param("swap_pole_lane_id", swap_pole_lane_id, "false"); - ROS_INFO("swap_pole_lane_id: %s", swap_pole_lane_id ? "true" : "false"); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "tlr_roi_pub"); - ros::NodeHandle n; - - getROSParams(); - - // Register subscribers - image_transport::ImageTransport it(n); - image_transport::Subscriber image_subscriber = it.subscribe(image_topic_name, 1, &imageCallback, ros::VoidPtr()); - ros::Subscriber roi_signal_subscriber = n.subscribe("/roi_signal", 1, &roiSignalCallback); - ros::Subscriber superimpose_sub = n.subscribe("/config/superimpose", 1, &superimposeCb); - - // Register publishers - roi_image_publisher = n.advertise("tlr_roi_image", 1); - marker_publisher = n.advertise("tlr_result", 1, true); - superimpose_image_publisher = n.advertise("tlr_superimpose_image", 1); - - ros::spin(); - - return 0; -} diff --git a/ros/src/computing/perception/detection/trafficlight_recognizer/srv/RecognizeLightState.srv b/ros/src/computing/perception/detection/trafficlight_recognizer/srv/RecognizeLightState.srv new file mode 100644 index 00000000000..0ac28d4a223 --- /dev/null +++ b/ros/src/computing/perception/detection/trafficlight_recognizer/srv/RecognizeLightState.srv @@ -0,0 +1,4 @@ +sensor_msgs/Image roi_image +--- +uint8 class_id +float32 confidence