-Added support fot VMap colouring to Left Traffic signals (autowarefo…

…undation#988) -Added Lane number on tlr_superimpose
haopo2005 · Dec 18, 2017 · 330f015 · 330f015
1 parent 9f4bda6
commit 330f015
Show file tree

Hide file tree

Showing 5 changed files with 41 additions and 5 deletions.
diff --git a/ros/src/computing/perception/detection/packages/road_wizard/nodes/region_tlr/region_tlr.cpp b/ros/src/computing/perception/detection/packages/road_wizard/nodes/region_tlr/region_tlr.cpp
@@ -92,7 +92,7 @@ static void putResult_inText(cv::Mat *image, const std::vector<Context> &context
 {
 	std::string label;
 	const int fontFace = cv::FONT_HERSHEY_COMPLEX_SMALL;
-	const float fontScale = 1.0f;
+	const float fontScale = 0.8f;
 	const int fontThickness = 1;
 	int baseline = 0;
 	CvPoint textOrg;
@@ -130,6 +130,8 @@ static void putResult_inText(cv::Mat *image, const std::vector<Context> &context
 		{
 			label += " RIGHT";
 		}
+		//add lane # text
+		label +=" " + std::to_string(ctx.closestLaneId);
 
 		cv::getTextSize(label,
 		                fontFace,

diff --git a/...mputing/perception/detection/packages/road_wizard/nodes/region_tlr_ssd/region_tlr_ssd.cpp b/...mputing/perception/detection/packages/road_wizard/nodes/region_tlr_ssd/region_tlr_ssd.cpp
@@ -409,7 +409,7 @@ void RegionTlrSsdRosNode::PublishImage(std::vector<Context> contexts) {
   // Define information for written label
   std::string  label;
   const int    kFontFace      = cv::FONT_HERSHEY_COMPLEX_SMALL;
-  const double kFontScale     = 1.0;
+  const double kFontScale     = 0.8;
   int          font_baseline  = 0;
   CvScalar     label_color;
 
@@ -438,6 +438,17 @@ void RegionTlrSsdRosNode::PublishImage(std::vector<Context> contexts) {
       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, kFontFace, kFontScale, label_color);

diff --git a/ros/src/data/packages/map_file/nodes/vector_map_loader/vector_map_loader.cpp b/ros/src/data/packages/map_file/nodes/vector_map_loader/vector_map_loader.cpp
@@ -537,12 +537,21 @@ visualization_msgs::MarkerArray createSignalMarkerArray(const VectorMap& vmap, C
     case Signal::YELLOW:
       vector_marker = createVectorMarker("signal", id++, yellow_color, vmap, vector);
       break;
+    case Signal::RED_LEFT:
+      vector_marker = createVectorMarker("signal", id++, Color::LIGHT_RED, vmap, vector);
+          break;
+    case Signal::BLUE_LEFT:
+      vector_marker = createVectorMarker("signal", id++, Color::LIGHT_GREEN, vmap, vector);
+          break;
+    case Signal::YELLOW_LEFT:
+      vector_marker = createVectorMarker("signal", id++, Color::LIGHT_YELLOW, vmap, vector);
+          break;
     case Signal::OTHER:
       vector_marker = createVectorMarker("signal", id++, other_color, vmap, vector);
       break;
     default:
       ROS_WARN_STREAM("[createSignalMarkerArray] unknown signal.type: " << signal.type << " Creating Marker as OTHER.");
-      vector_marker = createVectorMarker("signal", id++, other_color, vmap, vector);
+      vector_marker = createVectorMarker("signal", id++, Color::GRAY, vmap, vector);
       break;
     }
     if (isValidMarker(vector_marker))

diff --git a/ros/src/data/packages/vector_map_msgs/msg/Signal.msg b/ros/src/data/packages/vector_map_msgs/msg/Signal.msg
@@ -1,6 +1,6 @@
 # type
 uint8 RED=1
-uint8 BLUE=2
+uint8 BLUE=2 #Green (outside Japan)
 uint8 YELLOW=3
 uint8 PEDESTRIAN_RED=4
 uint8 PEDESTRIAN_BLUE=5
@@ -12,3 +12,8 @@ int32 vid
 int32 plid
 int32 type
 int32 linkid
+
+# left turn
+uint8 RED_LEFT=21
+uint8 BLUE_LEFT=22 #Green (outside Japan)
+uint8 YELLOW_LEFT=23
diff --git a/ros/src/data/packages/vector_map_server/nodes/vector_map_client/vector_map_client.cpp b/ros/src/data/packages/vector_map_server/nodes/vector_map_client/vector_map_client.cpp
@@ -233,11 +233,20 @@ int main(int argc, char **argv)
         case Signal::YELLOW:
           vector_marker = createVectorMarker("signal", id++, Color::YELLOW, vmap, vector);
           break;
+        case Signal::RED_LEFT:
+          vector_marker = createVectorMarker("signal", id++, Color::LIGHT_RED, vmap, vector);
+          break;
+        case Signal::BLUE_LEFT:
+          vector_marker = createVectorMarker("signal", id++, Color::LIGHT_GREEN, vmap, vector);
+          break;
+        case Signal::YELLOW_LEFT:
+          vector_marker = createVectorMarker("signal", id++, Color::LIGHT_YELLOW, vmap, vector);
+          break;
         case Signal::OTHER:
           vector_marker = createVectorMarker("signal", id++, Color::CYAN, vmap, vector);
           break;
         default:
-          vector_marker = createVectorMarker("signal", id++, Color::CYAN, vmap, vector);
+          vector_marker = createVectorMarker("signal", id++, Color::GRAY, vmap, vector);
           break;
         }
         if (isValidMarker(vector_marker))