Apollo 10.0
自动驾驶开放平台
smartereye_component.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2020 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
18
20
22
23namespace apollo {
24namespace drivers {
25namespace smartereye {
26
28 if (running_.load()) {
29 running_.exchange(false);
30 async_result_.wait();
31 }
32}
33
35 camera_config_ = std::make_shared<Config>();
37 camera_config_.get())) {
38 return false;
39 }
40 AINFO << "SmartereyeDevice config: " << camera_config_->DebugString();
41
42 camera_device_.reset(new SmartereyeDevice());
43 camera_device_->init(camera_config_);
44
45 device_wait_ = camera_config_->device_wait_ms();
46 spin_rate_ = static_cast<uint32_t>((1.0 / camera_config_->spin_rate()) * 1e6);
47
48 b_ispolling_ = false;
49
51
52 writer_ = node_->CreateWriter<Image>(camera_config_->channel_name_image());
53 SmartereyeObstacles_writer_ = node_->CreateWriter<SmartereyeObstacles>(
54 FLAGS_smartereye_obstacles_topic);
55 SmartereyeLanemark_writer_ = node_->CreateWriter<SmartereyeLanemark>(
56 FLAGS_smartereye_lanemark_topic);
57
58 async_result_ = cyber::Async(&SmartereyeComponent::run, this);
59
60 return true;
61}
62
64 running_.exchange(true);
65 while (!cyber::IsShutdown()) {
66 camera_device_->poll();
67 cyber::SleepFor(std::chrono::microseconds(spin_rate_));
68 }
69}
70
72 CallbackFunc fun =
73 std::bind(&SmartereyeComponent::Callback, this, std::placeholders::_1);
74 camera_device_->SetCallback(fun);
75
76 return true;
77}
78
79bool SmartereyeComponent::Callback(RawImageFrame *rawFrame) {
80 if (rawFrame->frameId == FrameId::Compound ||
81 rawFrame->frameId == FrameId::LaneExt) {
82 processFrame(rawFrame->frameId,
83 reinterpret_cast<char *>(rawFrame) + sizeof(RawImageFrame),
84 reinterpret_cast<char *>(rawFrame) + rawFrame->dataSize +
85 sizeof(RawImageFrame),
86 rawFrame->time, rawFrame->width, rawFrame->height);
87 } else {
88 processFrame(rawFrame->frameId,
89 reinterpret_cast<char *>(rawFrame) + sizeof(RawImageFrame),
90 rawFrame->dataSize, rawFrame->width, rawFrame->height,
91 rawFrame->format);
92 }
93
94 return true;
95}
96
97void SmartereyeComponent::processFrame(int frameId, char *image, char *extended,
98 int64_t time, int width, int height) {
99 switch (frameId) {
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));
105
106 SmartereyeObstacles pbSmartereyeObstacles;
107 pbSmartereyeObstacles.mutable_header()->set_frame_id(
108 FLAGS_smartereye_obstacles_topic);
109 pbSmartereyeObstacles.set_num_obstacles(blockNum);
110
111 OutputObstacle pbOutputObstacle;
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] =
162 pbOutputObstacle;
163 }
164 SmartereyeObstacles_writer_->Write(pbSmartereyeObstacles);
165
166 static unsigned char *rgbBuf = new unsigned char[width * height * 3];
167 RoadwayPainter::imageGrayToRGB((unsigned char *)image, rgbBuf, width,
168 height);
169 ObstaclePainter::paintObstacle(header->data, rgbBuf, width, height, true,
170 false);
171
172 Image pb_image;
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(
181 cyber::Time::Now().ToSecond());
182 pb_image.set_measurement_time(time);
183
184 writer_->Write(pb_image);
185 } break;
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;
191 SmartereyeLanemark pbSmartereyeLanemark;
192 pbSmartereyeLanemark.mutable_lane_road_data()
193 ->mutable_roadway()
194 ->set_width_0(ldwDataPack->roadway.width[0]);
195 pbSmartereyeLanemark.mutable_lane_road_data()
196 ->mutable_roadway()
197 ->set_width_1(ldwDataPack->roadway.width[1]);
198 pbSmartereyeLanemark.mutable_lane_road_data()
199 ->mutable_roadway()
200 ->set_width_2(ldwDataPack->roadway.width[2]);
201 pbSmartereyeLanemark.mutable_lane_road_data()
202 ->mutable_roadway()
203 ->set_is_tracking(ldwDataPack->roadway.isTracking);
204 pbSmartereyeLanemark.mutable_lane_road_data()->set_softstatus(
205 (LdwSoftStatus)ldwDataPack->softStatus);
206 pbSmartereyeLanemark.mutable_lane_road_data()->set_steerstatus(
207 (LdwSteerStatus)ldwDataPack->steerStatus);
208 pbSmartereyeLanemark.mutable_lane_road_data()
209 ->mutable_lens()
210 ->set_x_image_focal(ldwDataPack->lens.xImageFocal);
211 pbSmartereyeLanemark.mutable_lane_road_data()
212 ->mutable_lens()
213 ->set_y_image_focal(ldwDataPack->lens.yImageFocal);
214 pbSmartereyeLanemark.mutable_lane_road_data()
215 ->mutable_lens()
216 ->set_xratio_focal_pixel(ldwDataPack->lens.xRatioFocalToPixel);
217 pbSmartereyeLanemark.mutable_lane_road_data()
218 ->mutable_lens()
219 ->set_yratio_focal_pixel(ldwDataPack->lens.yRatioFocalToPixel);
220 pbSmartereyeLanemark.mutable_lane_road_data()
221 ->mutable_lens()
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);
232
233 static unsigned char *rgbBuf = new unsigned char[width * height * 3];
234 RoadwayPainter::imageGrayToRGB((unsigned char *)image, rgbBuf, width,
235 height);
236 bool mIsLaneDetected =
237 RoadwayPainter::paintRoadway(header->data, rgbBuf, width, height);
238 AINFO << mIsLaneDetected;
239
240 Image pb_image;
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(
249 cyber::Time::Now().ToSecond());
250 pb_image.set_measurement_time(time);
251
252 writer_->Write(pb_image);
253 }
254 default:
255 break;
256 }
257}
258
259void SmartereyeComponent::processFrame(int frameId, char *image,
260 uint32_t dataSize, int width, int height,
261 int frameFormat) {
262 switch (frameId) {
263 case FrameId::Lane: {
264 AINFO << "case FrameId::Lane:";
265 LdwDataPack *ldwDataPack = reinterpret_cast<LdwDataPack *>(image);
266 int degree = ldwDataPack->roadway.left_Lane.left_Boundary.degree;
267 AINFO << "ldw degree is: " << degree;
268 } break;
269 case FrameId::Obstacle: {
270 AINFO << "case FrameId::Obstacle:";
271 int blockNum = (reinterpret_cast<int *>(image))[0];
272 AINFO << "blockNum is: " << blockNum;
273 } break;
274 case FrameId::Disparity: {
275 AINFO << "case FrameId::Disparity:";
276 } break;
277 case FrameId::CalibLeftCamera: {
278 AINFO << "case FrameId::CalibLeftCamera:";
279 } break;
280 default:
281 break;
282 }
283}
284
285} // namespace smartereye
286} // namespace drivers
287} // namespace apollo
std::shared_ptr< Node > node_
static Time Now()
get the current time.
Definition time.cc:57
void processFrame(int frameId, char *image, char *extended, int64_t time, int width, int height)
#define AINFO
Definition log.h:42
bool GetProtoFromFile(const std::string &file_name, google::protobuf::Message *message)
Parses the content of the file specified by the file_name as a representation of protobufs,...
Definition file.cc:132
bool IsShutdown()
Definition state.h:46
std::function< bool(RawImageFrame *rawFrame)> CallbackFunc
class register implement
Definition arena_queue.h:37