98 int64_t time,
int width,
int height) {
100 case FrameId::Compound: {
101 FrameDataExtHead *header =
reinterpret_cast<FrameDataExtHead *
>(extended);
102 int blockNum = (
reinterpret_cast<int *
>(header->data))[0];
103 OutputObstacles *obsDataPack =
reinterpret_cast<OutputObstacles *
>(
104 ((
reinterpret_cast<int *
>(header->data)) + 2));
107 pbSmartereyeObstacles.mutable_header()->set_frame_id(
108 FLAGS_smartereye_obstacles_topic);
109 pbSmartereyeObstacles.set_num_obstacles(blockNum);
112 for (
int j = 0; j < blockNum; j++) {
113 pbOutputObstacle.set_currentspeed(obsDataPack[j].currentSpeed);
114 pbOutputObstacle.set_framerate(obsDataPack[j].frameRate);
115 pbOutputObstacle.set_trackid(obsDataPack[j].trackId);
116 pbOutputObstacle.set_trackframenum(obsDataPack[j].trackFrameNum);
117 pbOutputObstacle.set_statelabel(obsDataPack[j].stateLabel);
118 pbOutputObstacle.set_classlabel(obsDataPack[j].classLabel);
119 pbOutputObstacle.set_continuouslabel(obsDataPack[j].continuousLabel);
120 pbOutputObstacle.set_fuzzyestimationvalid(
121 obsDataPack[j].fuzzyEstimationValid);
122 pbOutputObstacle.set_obstacletype(
123 (OutputObstacle_RecognitionType)obsDataPack[j].obstacleType);
124 pbOutputObstacle.set_avgdisp(obsDataPack[j].avgDistanceZ);
125 pbOutputObstacle.set_avgdistancez(obsDataPack[j].avgDistanceZ);
126 pbOutputObstacle.set_neardistancez(obsDataPack[j].nearDistanceZ);
127 pbOutputObstacle.set_fardistancez(obsDataPack[j].farDistanceZ);
128 pbOutputObstacle.set_real3dleftx(obsDataPack[j].real3DLeftX);
129 pbOutputObstacle.set_real3drightx(obsDataPack[j].real3DRightX);
130 pbOutputObstacle.set_real3dcenterx(obsDataPack[j].real3DCenterX);
131 pbOutputObstacle.set_real3dupy(obsDataPack[j].real3DUpY);
132 pbOutputObstacle.set_real3dlowy(obsDataPack[j].real3DLowY);
133 pbOutputObstacle.set_firstpointx(obsDataPack[j].firstPointX);
134 pbOutputObstacle.set_firstpointy(obsDataPack[j].firstPointY);
135 pbOutputObstacle.set_secondpointx(obsDataPack[j].secondPointX);
136 pbOutputObstacle.set_secondpointy(obsDataPack[j].secondPointY);
137 pbOutputObstacle.set_thirdpointx(obsDataPack[j].thirdPointX);
138 pbOutputObstacle.set_thirdpointy(obsDataPack[j].thirdPointY);
139 pbOutputObstacle.set_fourthpointx(obsDataPack[j].fourthPointX);
140 pbOutputObstacle.set_fourthpointy(obsDataPack[j].fourthPointY);
141 pbOutputObstacle.set_fuzzyrelativedistancez(
142 obsDataPack[j].fuzzyRelativeDistanceZ);
143 pbOutputObstacle.set_fuzzyrelativespeedz(
144 obsDataPack[j].fuzzyRelativeSpeedZ);
145 pbOutputObstacle.set_fuzzycollisiontimez(
146 obsDataPack[j].fuzzyCollisionTimeZ);
147 pbOutputObstacle.set_fuzzycollisionx(obsDataPack[j].fuzzyCollisionX);
148 pbOutputObstacle.set_fuzzy3dwidth(obsDataPack[j].fuzzy3DWidth);
149 pbOutputObstacle.set_fuzzy3dcenterx(obsDataPack[j].fuzzy3DCenterX);
150 pbOutputObstacle.set_fuzzy3dleftx(obsDataPack[j].fuzzy3DLeftX);
151 pbOutputObstacle.set_fuzzy3drightx(obsDataPack[j].fuzzy3DRightX);
152 pbOutputObstacle.set_fuzzy3dheight(obsDataPack[j].fuzzy3DHeight);
153 pbOutputObstacle.set_fuzzy3dupy(obsDataPack[j].fuzzy3DUpY);
154 pbOutputObstacle.set_fuzzy3dlowy(obsDataPack[j].fuzzy3DLowY);
155 pbOutputObstacle.set_fuzzyrelativespeedcenterx(
156 obsDataPack[j].fuzzyRelativeSpeedCenterX);
157 pbOutputObstacle.set_fuzzyrelativespeedleftx(
158 obsDataPack[j].fuzzyRelativeSpeedLeftX);
159 pbOutputObstacle.set_fuzzyrelativespeedrightx(
160 obsDataPack[j].fuzzyRelativeSpeedRightX);
161 (*pbSmartereyeObstacles.mutable_output_obstacles())[j] =
164 SmartereyeObstacles_writer_->Write(pbSmartereyeObstacles);
166 static unsigned char *rgbBuf =
new unsigned char[width * height * 3];
167 RoadwayPainter::imageGrayToRGB((
unsigned char *)image, rgbBuf, width,
169 ObstaclePainter::paintObstacle(header->data, rgbBuf, width, height,
true,
173 pb_image.mutable_header()->set_frame_id(
174 camera_config_->channel_name_image());
175 pb_image.set_width(width);
176 pb_image.set_height(height);
177 pb_image.set_data(rgbBuf, width * height * 3);
178 pb_image.set_encoding(
"rgb8");
179 pb_image.set_step(width * 3);
180 pb_image.mutable_header()->set_timestamp_sec(
182 pb_image.set_measurement_time(time);
184 writer_->Write(pb_image);
186 case FrameId::LaneExt: {
187 FrameDataExtHead *header =
reinterpret_cast<FrameDataExtHead *
>(extended);
188 LdwDataPack *ldwDataPack =
reinterpret_cast<LdwDataPack *
>(header->data);
189 AINFO <<
"ldw degree is: "
190 << ldwDataPack->roadway.left_Lane.left_Boundary.degree;
192 pbSmartereyeLanemark.mutable_lane_road_data()
194 ->set_width_0(ldwDataPack->roadway.width[0]);
195 pbSmartereyeLanemark.mutable_lane_road_data()
197 ->set_width_1(ldwDataPack->roadway.width[1]);
198 pbSmartereyeLanemark.mutable_lane_road_data()
200 ->set_width_2(ldwDataPack->roadway.width[2]);
201 pbSmartereyeLanemark.mutable_lane_road_data()
203 ->set_is_tracking(ldwDataPack->roadway.isTracking);
204 pbSmartereyeLanemark.mutable_lane_road_data()->set_softstatus(
206 pbSmartereyeLanemark.mutable_lane_road_data()->set_steerstatus(
208 pbSmartereyeLanemark.mutable_lane_road_data()
210 ->set_x_image_focal(ldwDataPack->lens.xImageFocal);
211 pbSmartereyeLanemark.mutable_lane_road_data()
213 ->set_y_image_focal(ldwDataPack->lens.yImageFocal);
214 pbSmartereyeLanemark.mutable_lane_road_data()
216 ->set_xratio_focal_pixel(ldwDataPack->lens.xRatioFocalToPixel);
217 pbSmartereyeLanemark.mutable_lane_road_data()
219 ->set_yratio_focal_pixel(ldwDataPack->lens.yRatioFocalToPixel);
220 pbSmartereyeLanemark.mutable_lane_road_data()
222 ->set_mountingheight(ldwDataPack->lens.mountingHeight);
223 pbSmartereyeLanemark.mutable_lane_road_data()->mutable_lens()->set_mcosrx(
224 ldwDataPack->lens.mCosRx);
225 pbSmartereyeLanemark.mutable_lane_road_data()->mutable_lens()->set_msinrx(
226 ldwDataPack->lens.mSinRx);
227 pbSmartereyeLanemark.mutable_lane_road_data()->mutable_lens()->set_mcosry(
228 ldwDataPack->lens.mCosRy);
229 pbSmartereyeLanemark.mutable_lane_road_data()->mutable_lens()->set_msinry(
230 ldwDataPack->lens.mSinRy);
231 SmartereyeLanemark_writer_->Write(pbSmartereyeLanemark);
233 static unsigned char *rgbBuf =
new unsigned char[width * height * 3];
234 RoadwayPainter::imageGrayToRGB((
unsigned char *)image, rgbBuf, width,
236 bool mIsLaneDetected =
237 RoadwayPainter::paintRoadway(header->data, rgbBuf, width, height);
238 AINFO << mIsLaneDetected;
241 pb_image.mutable_header()->set_frame_id(
242 camera_config_->channel_name_image());
243 pb_image.set_width(width);
244 pb_image.set_height(height);
245 pb_image.set_data(rgbBuf, width * height * 3);
246 pb_image.set_encoding(
"rgb8");
247 pb_image.set_step(width * 3);
248 pb_image.mutable_header()->set_timestamp_sec(
250 pb_image.set_measurement_time(time);
252 writer_->Write(pb_image);