diff --git a/modules/drivers/smartereye/BUILD b/modules/drivers/smartereye/BUILD new file mode 100755 index 00000000000..2a59a50117b --- /dev/null +++ b/modules/drivers/smartereye/BUILD @@ -0,0 +1,73 @@ +load("//tools:cpplint.bzl", "cpplint") + +package(default_visibility = ["//visibility:public"]) + +cc_binary( + name = "libsmartereye_component.so", + linkshared = True, + linkstatic = False, + deps = [ + ":smartereye_component_lib", + ":compress_component_lib", + ], +) + +cc_library( + name = "smartereye_component_lib", + srcs = ["smartereye_component.cc"], + hdrs = ["smartereye_component.h"], + copts = ['-DMODULE_NAME=\\"camera\\"'], + deps = [ + ":smartereye_device", + ":smartereye_handler", + "//cyber", + "//modules/common/adapters:adapter_gflags", + "//modules/drivers/proto:sensor_proto", + "//third_party/camera_library/smartereye:smartereye", + ], +) + +cc_library( + name = "smartereye_device", + srcs = ["smartereye_device.cc"], + hdrs = ["smartereye_device.h"], + copts = ['-DMODULE_NAME=\\"camera\\"'], + deps = [ + ":smartereye_handler", + "//cyber", + "//modules/drivers/proto:sensor_proto", + "//modules/drivers/smartereye/proto:camera_proto", + "//third_party/camera_library/smartereye:smartereye", + ], +) + +cc_library( + name = "smartereye_handler", + srcs = ["smartereye_handler.cc"], + hdrs = ["smartereye_handler.h"], + copts = ['-DMODULE_NAME=\\"camera\\"'], + deps = [ + "//cyber", + "//modules/drivers/proto:sensor_proto", + "//modules/drivers/smartereye/proto:camera_proto", + "//third_party/camera_library/smartereye:smartereye", + ], +) + +cc_library( + name = "compress_component_lib", + srcs = ["compress_component.cc"], + hdrs = ["compress_component.h"], + copts = ['-DMODULE_NAME=\\"camera\\"'], + linkopts = [ + "-lopencv_core", + "-lopencv_highgui", + ], + deps = [ + "//cyber", + "//modules/drivers/smartereye/proto:camera_proto", + "//modules/drivers/proto:sensor_proto", + ], +) + +cpplint() diff --git a/modules/drivers/smartereye/compress_component.cc b/modules/drivers/smartereye/compress_component.cc new file mode 100644 index 00000000000..ee8d1026956 --- /dev/null +++ b/modules/drivers/smartereye/compress_component.cc @@ -0,0 +1,84 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/smartereye/compress_component.h" + +#include +#include + +#include "opencv2/core/core.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/imgproc/imgproc.hpp" + +namespace apollo { +namespace drivers { +namespace smartereye { + +bool CompressComponent::Init() { + if (!GetProtoConfig(&config_)) { + AERROR << "Parse config file failed: " << ConfigFilePath(); + return false; + } + AINFO << "Camera config: \n" << config_.DebugString(); + try { + image_pool_.reset(new CCObjectPool( + config_.compress_conf().image_pool_size())); + image_pool_->ConstructAll(); + } catch (const std::bad_alloc& e) { + AERROR << e.what(); + return false; + } + + writer_ = node_->CreateWriter( + config_.compress_conf().output_channel()); + return true; +} + +bool CompressComponent::Proc(const std::shared_ptr& image) { + ADEBUG << "procing compressed"; + auto compressed_image = image_pool_->GetObject(); + compressed_image->mutable_header()->CopyFrom(image->header()); + compressed_image->set_frame_id(image->frame_id()); + compressed_image->set_measurement_time(image->measurement_time()); + compressed_image->set_format(image->encoding() + "; jpeg compressed bgr8"); + + std::vector params; + params.resize(3, 0); + params[0] = CV_IMWRITE_JPEG_QUALITY; + params[1] = 95; + + try { + cv::Mat mat_image(image->height(), image->width(), CV_8UC3, + const_cast(image->data().data()), image->step()); + cv::Mat tmp_mat; + cv::cvtColor(mat_image, tmp_mat, cv::COLOR_RGB2BGR); + std::vector compress_buffer; + if (!cv::imencode(".jpg", tmp_mat, compress_buffer, params)) { + AERROR << "cv::imencode (jpeg) failed on input image"; + return false; + } + compressed_image->set_data(compress_buffer.data(), compress_buffer.size()); + writer_->Write(compressed_image); + } catch (std::exception& e) { + AERROR << "cv::imencode (jpeg) exception :" << e.what(); + return false; + } + return true; +} + +} // namespace smartereye +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/smartereye/compress_component.h b/modules/drivers/smartereye/compress_component.h new file mode 100644 index 00000000000..acc67b58b59 --- /dev/null +++ b/modules/drivers/smartereye/compress_component.h @@ -0,0 +1,50 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include + +#include "cyber/base/concurrent_object_pool.h" +#include "cyber/cyber.h" +#include "modules/drivers/proto/sensor_image.pb.h" +#include "modules/drivers/smartereye/proto/config.pb.h" + +namespace apollo { +namespace drivers { +namespace smartereye { + +using apollo::cyber::Component; +using apollo::cyber::Writer; +using apollo::cyber::base::CCObjectPool; +using apollo::drivers::Image; +using apollo::drivers::smartereye::config::Config; + +class CompressComponent : public Component { + public: + bool Init() override; + bool Proc(const std::shared_ptr& image) override; + + private: + std::shared_ptr> image_pool_ = nullptr; + std::shared_ptr> writer_ = nullptr; + Config config_; +}; + +CYBER_REGISTER_COMPONENT(CompressComponent) +} // namespace smartereye +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/smartereye/conf/smartereye.pb.txt b/modules/drivers/smartereye/conf/smartereye.pb.txt new file mode 100755 index 00000000000..4cf86dd8ac2 --- /dev/null +++ b/modules/drivers/smartereye/conf/smartereye.pb.txt @@ -0,0 +1,33 @@ +camera_dev: "/dev/camera/smartereye" +frame_id: "smartereye" +pixel_format: "yuyv" +io_method: IO_METHOD_MMAP +width: 1280 +height: 720 +frame_rate: 20 +monochrome: false +brightness: -1 +contrast: -1 +saturation: -1 +sharpness: -1 +gain: -1 +auto_focus: false +focus: -1 +auto_exposure: true +exposure: 100 +auto_white_balance: true +white_balance: 4000 +bytes_per_pixel: 2 +trigger_internal: 0 +trigger_fps: 20 +channel_name: "/apollo/sensor/smartereye" +channel_name_image: "/apollo/sensor/smartereye/image" +channel_name_image_compressed: "/apollo/sensor/smartereye/image/compressed" +device_wait_ms: 2000 +spin_rate: 200 +output_type: RGB + +compress_conf { + output_channel: "/apollo/sensor/smartereye/image/compressed" + image_pool_size: 100 +} diff --git a/modules/drivers/smartereye/dag/smartereye.dag b/modules/drivers/smartereye/dag/smartereye.dag new file mode 100755 index 00000000000..6d53cfabaa9 --- /dev/null +++ b/modules/drivers/smartereye/dag/smartereye.dag @@ -0,0 +1,22 @@ +# Define all coms in DAG streaming. +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/smartereye/libsmartereye_component.so" + components { + class_name : "SmartereyeComponent" + config { + name : "smartereye" + config_file_path : "/apollo/modules/drivers/smartereye/conf/smartereye.pb.txt" + } + } + components { + class_name : "CompressComponent" + config { + name : "smartereye_compress" + config_file_path : "/apollo/modules/drivers/smartereye/conf/smartereye.pb.txt" + readers { + channel: "/apollo/sensor/smartereye/image" + pending_queue_size: 10 + } + } + } +} \ No newline at end of file diff --git a/modules/drivers/smartereye/launch/smartereye.launch b/modules/drivers/smartereye/launch/smartereye.launch new file mode 100755 index 00000000000..e18e1b78ba1 --- /dev/null +++ b/modules/drivers/smartereye/launch/smartereye.launch @@ -0,0 +1,7 @@ + + + smartereye + /apollo/modules/drivers/smartereye/dag/smartereye.dag + smartereye + + diff --git a/modules/drivers/smartereye/proto/BUILD b/modules/drivers/smartereye/proto/BUILD new file mode 100755 index 00000000000..c60a4b3a466 --- /dev/null +++ b/modules/drivers/smartereye/proto/BUILD @@ -0,0 +1,27 @@ +load("//tools:cpplint.bzl", "cpplint") + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "camera_proto", + deps = [ + ":sensor_camera_proto", + ], +) + +cc_proto_library( + name = "sensor_camera_proto", + deps = [ + ":camera_proto_lib", + ], +) + +proto_library( + name = "camera_proto_lib", + srcs = ["config.proto"], + deps = [ + "//modules/common/proto:header_proto_lib", + ], +) + +cpplint() diff --git a/modules/drivers/smartereye/proto/config.proto b/modules/drivers/smartereye/proto/config.proto new file mode 100755 index 00000000000..c09668b5660 --- /dev/null +++ b/modules/drivers/smartereye/proto/config.proto @@ -0,0 +1,58 @@ +syntax = "proto2"; + +package apollo.drivers.smartereye.config; + +enum IOMethod { + IO_METHOD_UNKNOWN = 0; + IO_METHOD_READ = 1; + IO_METHOD_MMAP = 2; + IO_METHOD_USERPTR = 3; +} + +enum OutputType { + YUYV = 0; + RGB = 1; +} + +message Config { + optional string camera_dev = 1; + optional string frame_id = 2; + // v4l pixel format + optional string pixel_format = 3 [default = "yuyv"]; + // mmap, userptr, read + optional IOMethod io_method = 4; + optional uint32 width = 5; + optional uint32 height = 6; + optional uint32 frame_rate = 7; + optional bool monochrome = 8 [default = false]; + + optional int32 brightness = 9 [default = -1]; + optional int32 contrast = 10 [default = -1]; + optional int32 saturation = 11 [default = -1]; + optional int32 sharpness = 12 [default = -1]; + optional int32 gain = 13 [default = -1]; + + optional bool auto_focus = 14 [default = false]; + optional int32 focus = 15 [default = -1]; + optional bool auto_exposure = 16 [default = true]; + optional int32 exposure = 17 [default = 100]; + optional bool auto_white_balance = 18 [default = true]; + optional int32 white_balance = 19 [default = 4000]; + optional uint32 bytes_per_pixel = 20 [default = 3]; + optional uint32 trigger_internal = 21 [default = 0]; + optional uint32 trigger_fps = 22 [default = 30]; + optional string channel_name = 23; + optional string channel_name_image = 24; + optional string channel_name_image_compressed = 25; + // wait time when camera select timeout + optional uint32 device_wait_ms = 26 [default = 2000]; + // camera select spin time + optional uint32 spin_rate = 27 [default = 200]; + optional OutputType output_type = 28; + + message CompressConfig { + optional string output_channel = 1; + optional uint32 image_pool_size = 2 [default = 20]; + } + optional CompressConfig compress_conf = 29; +} diff --git a/modules/drivers/smartereye/smartereye_component.cc b/modules/drivers/smartereye/smartereye_component.cc new file mode 100755 index 00000000000..cf203884cee --- /dev/null +++ b/modules/drivers/smartereye/smartereye_component.cc @@ -0,0 +1,287 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/smartereye/smartereye_component.h" + +#include "modules/drivers/smartereye/smartereye_handler.h" + +#include "modules/common/adapters/adapter_gflags.h" + +namespace apollo { +namespace drivers { +namespace smartereye { + +SmartereyeComponent::~SmartereyeComponent() { + if (running_.load()) { + running_.exchange(false); + async_result_.wait(); + } +} + +bool SmartereyeComponent::Init() { + camera_config_ = std::make_shared(); + if (!apollo::cyber::common::GetProtoFromFile(config_file_path_, + camera_config_.get())) { + return false; + } + AINFO << "SmartereyeDevice config: " << camera_config_->DebugString(); + + camera_device_.reset(new SmartereyeDevice()); + camera_device_->init(camera_config_); + + device_wait_ = camera_config_->device_wait_ms(); + spin_rate_ = static_cast((1.0 / camera_config_->spin_rate()) * 1e6); + + b_ispolling_ = false; + + SetCallback(); + + writer_ = node_->CreateWriter(camera_config_->channel_name_image()); + SmartereyeObstacles_writer_ = node_->CreateWriter( + FLAGS_smartereye_obstacles_topic); + SmartereyeLanemark_writer_ = node_->CreateWriter( + FLAGS_smartereye_lanemark_topic); + + async_result_ = cyber::Async(&SmartereyeComponent::run, this); + + return true; +} + +void SmartereyeComponent::run() { + running_.exchange(true); + while (!cyber::IsShutdown()) { + camera_device_->poll(); + cyber::SleepFor(std::chrono::microseconds(spin_rate_)); + } +} + +bool SmartereyeComponent::SetCallback() { + CallbackFunc fun = + std::bind(&SmartereyeComponent::Callback, this, std::placeholders::_1); + camera_device_->SetCallback(fun); + + return true; +} + +bool SmartereyeComponent::Callback(RawImageFrame *rawFrame) { + if (rawFrame->frameId == FrameId::Compound || + rawFrame->frameId == FrameId::LaneExt) { + processFrame(rawFrame->frameId, + reinterpret_cast(rawFrame) + sizeof(RawImageFrame), + reinterpret_cast(rawFrame) + rawFrame->dataSize + + sizeof(RawImageFrame), + rawFrame->time, rawFrame->width, rawFrame->height); + } else { + processFrame(rawFrame->frameId, + reinterpret_cast(rawFrame) + sizeof(RawImageFrame), + rawFrame->dataSize, rawFrame->width, rawFrame->height, + rawFrame->format); + } + + return true; +} + +void SmartereyeComponent::processFrame(int frameId, char *image, char *extended, + int64_t time, int width, int height) { + switch (frameId) { + case FrameId::Compound: { + FrameDataExtHead *header = reinterpret_cast(extended); + int blockNum = (reinterpret_cast(header->data))[0]; + OutputObstacles *obsDataPack = reinterpret_cast( + ((reinterpret_cast(header->data)) + 2)); + + SmartereyeObstacles pbSmartereyeObstacles; + pbSmartereyeObstacles.mutable_header()->set_frame_id( + FLAGS_smartereye_obstacles_topic); + pbSmartereyeObstacles.set_num_obstacles(blockNum); + + OutputObstacle pbOutputObstacle; + for (int j = 0; j < blockNum; j++) { + pbOutputObstacle.set_currentspeed(obsDataPack[j].currentSpeed); + pbOutputObstacle.set_framerate(obsDataPack[j].frameRate); + pbOutputObstacle.set_trackid(obsDataPack[j].trackId); + pbOutputObstacle.set_trackframenum(obsDataPack[j].trackFrameNum); + pbOutputObstacle.set_statelabel(obsDataPack[j].stateLabel); + pbOutputObstacle.set_classlabel(obsDataPack[j].classLabel); + pbOutputObstacle.set_continuouslabel(obsDataPack[j].continuousLabel); + pbOutputObstacle.set_fuzzyestimationvalid( + obsDataPack[j].fuzzyEstimationValid); + pbOutputObstacle.set_obstacletype( + (OutputObstacle_RecognitionType)obsDataPack[j].obstacleType); + pbOutputObstacle.set_avgdisp(obsDataPack[j].avgDistanceZ); + pbOutputObstacle.set_avgdistancez(obsDataPack[j].avgDistanceZ); + pbOutputObstacle.set_neardistancez(obsDataPack[j].nearDistanceZ); + pbOutputObstacle.set_fardistancez(obsDataPack[j].farDistanceZ); + pbOutputObstacle.set_real3dleftx(obsDataPack[j].real3DLeftX); + pbOutputObstacle.set_real3drightx(obsDataPack[j].real3DRightX); + pbOutputObstacle.set_real3dcenterx(obsDataPack[j].real3DCenterX); + pbOutputObstacle.set_real3dupy(obsDataPack[j].real3DUpY); + pbOutputObstacle.set_real3dlowy(obsDataPack[j].real3DLowY); + pbOutputObstacle.set_firstpointx(obsDataPack[j].firstPointX); + pbOutputObstacle.set_firstpointy(obsDataPack[j].firstPointY); + pbOutputObstacle.set_secondpointx(obsDataPack[j].secondPointX); + pbOutputObstacle.set_secondpointy(obsDataPack[j].secondPointY); + pbOutputObstacle.set_thirdpointx(obsDataPack[j].thirdPointX); + pbOutputObstacle.set_thirdpointy(obsDataPack[j].thirdPointY); + pbOutputObstacle.set_fourthpointx(obsDataPack[j].fourthPointX); + pbOutputObstacle.set_fourthpointy(obsDataPack[j].fourthPointY); + pbOutputObstacle.set_fuzzyrelativedistancez( + obsDataPack[j].fuzzyRelativeDistanceZ); + pbOutputObstacle.set_fuzzyrelativespeedz( + obsDataPack[j].fuzzyRelativeSpeedZ); + pbOutputObstacle.set_fuzzycollisiontimez( + obsDataPack[j].fuzzyCollisionTimeZ); + pbOutputObstacle.set_fuzzycollisionx(obsDataPack[j].fuzzyCollisionX); + pbOutputObstacle.set_fuzzy3dwidth(obsDataPack[j].fuzzy3DWidth); + pbOutputObstacle.set_fuzzy3dcenterx(obsDataPack[j].fuzzy3DCenterX); + pbOutputObstacle.set_fuzzy3dleftx(obsDataPack[j].fuzzy3DLeftX); + pbOutputObstacle.set_fuzzy3drightx(obsDataPack[j].fuzzy3DRightX); + pbOutputObstacle.set_fuzzy3dheight(obsDataPack[j].fuzzy3DHeight); + pbOutputObstacle.set_fuzzy3dupy(obsDataPack[j].fuzzy3DUpY); + pbOutputObstacle.set_fuzzy3dlowy(obsDataPack[j].fuzzy3DLowY); + pbOutputObstacle.set_fuzzyrelativespeedcenterx( + obsDataPack[j].fuzzyRelativeSpeedCenterX); + pbOutputObstacle.set_fuzzyrelativespeedleftx( + obsDataPack[j].fuzzyRelativeSpeedLeftX); + pbOutputObstacle.set_fuzzyrelativespeedrightx( + obsDataPack[j].fuzzyRelativeSpeedRightX); + (*pbSmartereyeObstacles.mutable_output_obstacles())[j] = + pbOutputObstacle; + } + SmartereyeObstacles_writer_->Write(pbSmartereyeObstacles); + + static unsigned char *rgbBuf = new unsigned char[width * height * 3]; + RoadwayPainter::imageGrayToRGB((unsigned char *)image, rgbBuf, width, + height); + ObstaclePainter::paintObstacle(header->data, rgbBuf, width, height, true, + false); + + Image pb_image; + pb_image.mutable_header()->set_frame_id( + camera_config_->channel_name_image()); + pb_image.set_width(width); + pb_image.set_height(height); + pb_image.set_data(rgbBuf, width * height * 3); + pb_image.set_encoding("rgb8"); + pb_image.set_step(width * 3); + pb_image.mutable_header()->set_timestamp_sec( + cyber::Time::Now().ToSecond()); + pb_image.set_measurement_time(time); + + writer_->Write(pb_image); + } break; + case FrameId::LaneExt: { + FrameDataExtHead *header = reinterpret_cast(extended); + LdwDataPack *ldwDataPack = reinterpret_cast(header->data); + AINFO << "ldw degree is: " + << ldwDataPack->roadway.left_Lane.left_Boundary.degree; + SmartereyeLanemark pbSmartereyeLanemark; + pbSmartereyeLanemark.mutable_lane_road_data() + ->mutable_roadway() + ->set_width_0(ldwDataPack->roadway.width[0]); + pbSmartereyeLanemark.mutable_lane_road_data() + ->mutable_roadway() + ->set_width_1(ldwDataPack->roadway.width[1]); + pbSmartereyeLanemark.mutable_lane_road_data() + ->mutable_roadway() + ->set_width_2(ldwDataPack->roadway.width[2]); + pbSmartereyeLanemark.mutable_lane_road_data() + ->mutable_roadway() + ->set_is_tracking(ldwDataPack->roadway.isTracking); + pbSmartereyeLanemark.mutable_lane_road_data()->set_softstatus( + (LdwSoftStatus)ldwDataPack->softStatus); + pbSmartereyeLanemark.mutable_lane_road_data()->set_steerstatus( + (LdwSteerStatus)ldwDataPack->steerStatus); + pbSmartereyeLanemark.mutable_lane_road_data() + ->mutable_lens() + ->set_x_image_focal(ldwDataPack->lens.xImageFocal); + pbSmartereyeLanemark.mutable_lane_road_data() + ->mutable_lens() + ->set_y_image_focal(ldwDataPack->lens.yImageFocal); + pbSmartereyeLanemark.mutable_lane_road_data() + ->mutable_lens() + ->set_xratio_focal_pixel(ldwDataPack->lens.xRatioFocalToPixel); + pbSmartereyeLanemark.mutable_lane_road_data() + ->mutable_lens() + ->set_yratio_focal_pixel(ldwDataPack->lens.yRatioFocalToPixel); + pbSmartereyeLanemark.mutable_lane_road_data() + ->mutable_lens() + ->set_mountingheight(ldwDataPack->lens.mountingHeight); + pbSmartereyeLanemark.mutable_lane_road_data()->mutable_lens()->set_mcosrx( + ldwDataPack->lens.mCosRx); + pbSmartereyeLanemark.mutable_lane_road_data()->mutable_lens()->set_msinrx( + ldwDataPack->lens.mSinRx); + pbSmartereyeLanemark.mutable_lane_road_data()->mutable_lens()->set_mcosry( + ldwDataPack->lens.mCosRy); + pbSmartereyeLanemark.mutable_lane_road_data()->mutable_lens()->set_msinry( + ldwDataPack->lens.mSinRy); + SmartereyeLanemark_writer_->Write(pbSmartereyeLanemark); + + static unsigned char *rgbBuf = new unsigned char[width * height * 3]; + RoadwayPainter::imageGrayToRGB((unsigned char *)image, rgbBuf, width, + height); + bool mIsLaneDetected = + RoadwayPainter::paintRoadway(header->data, rgbBuf, width, height); + AINFO << mIsLaneDetected; + + Image pb_image; + pb_image.mutable_header()->set_frame_id( + camera_config_->channel_name_image()); + pb_image.set_width(width); + pb_image.set_height(height); + pb_image.set_data(rgbBuf, width * height * 3); + pb_image.set_encoding("rgb8"); + pb_image.set_step(width * 3); + pb_image.mutable_header()->set_timestamp_sec( + cyber::Time::Now().ToSecond()); + pb_image.set_measurement_time(time); + + writer_->Write(pb_image); + } + default: + break; + } +} + +void SmartereyeComponent::processFrame(int frameId, char *image, + uint32_t dataSize, int width, int height, + int frameFormat) { + switch (frameId) { + case FrameId::Lane: { + AINFO << "case FrameId::Lane:"; + LdwDataPack *ldwDataPack = reinterpret_cast(image); + int degree = ldwDataPack->roadway.left_Lane.left_Boundary.degree; + AINFO << "ldw degree is: " << degree; + } break; + case FrameId::Obstacle: { + AINFO << "case FrameId::Obstacle:"; + int blockNum = (reinterpret_cast(image))[0]; + AINFO << "blockNum is: " << blockNum; + } break; + case FrameId::Disparity: { + AINFO << "case FrameId::Disparity:"; + } break; + case FrameId::CalibLeftCamera: { + AINFO << "case FrameId::CalibLeftCamera:"; + } break; + default: + break; + } +} + +} // namespace smartereye +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/smartereye/smartereye_component.h b/modules/drivers/smartereye/smartereye_component.h new file mode 100755 index 00000000000..11297420243 --- /dev/null +++ b/modules/drivers/smartereye/smartereye_component.h @@ -0,0 +1,77 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include "cyber/cyber.h" +#include "modules/drivers/proto/sensor_image.pb.h" +#include "modules/drivers/proto/smartereye.pb.h" +#include "modules/drivers/smartereye/proto/config.pb.h" +#include "modules/drivers/smartereye/smartereye_device.h" +#include "third_party/camera_library/smartereye/include/frameext.h" +#include "third_party/camera_library/smartereye/include/obstacleData.h" +#include "third_party/camera_library/smartereye/include/obstaclepainter.h" +#include "third_party/camera_library/smartereye/include/roadwaypainter.h" +#include "third_party/camera_library/smartereye/include/yuv2rgb.h" + +namespace apollo { +namespace drivers { +namespace smartereye { + +using apollo::cyber::Component; +using apollo::cyber::Reader; +using apollo::cyber::Writer; +using apollo::drivers::Image; +using apollo::drivers::smartereye::config::Config; + +class SmartereyeComponent : public Component<> { + public: + bool Init() override; + ~SmartereyeComponent(); + + protected: + void run(); + bool SetCallback(); + bool Callback(RawImageFrame *rawFrame); + void processFrame(int frameId, char *image, char *extended, int64_t time, + int width, int height); + void processFrame(int frameId, char *image, uint32_t dataSize, int width, + int height, int frameFormat); + + private: + std::shared_ptr> writer_ = nullptr; + std::shared_ptr> SmartereyeObstacles_writer_ = + nullptr; + std::shared_ptr> SmartereyeLanemark_writer_ = + nullptr; + std::unique_ptr camera_device_ = nullptr; + std::shared_ptr camera_config_ = nullptr; + uint32_t spin_rate_ = 200; + uint32_t device_wait_ = 2000; + std::future async_result_; + std::atomic running_ = {false}; + bool b_ispolling_ = false; +}; + +CYBER_REGISTER_COMPONENT(SmartereyeComponent) +} // namespace smartereye +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/smartereye/smartereye_device.cc b/modules/drivers/smartereye/smartereye_device.cc new file mode 100755 index 00000000000..2ea84aacedd --- /dev/null +++ b/modules/drivers/smartereye/smartereye_device.cc @@ -0,0 +1,81 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/smartereye/smartereye_device.h" + +#include + +#include +#include + +#define CLEAR(x) memset(&(x), 0, sizeof(x)) + +namespace apollo { +namespace drivers { +namespace smartereye { + +SmartereyeDevice::SmartereyeDevice() {} + +SmartereyeDevice::~SmartereyeDevice() { uninit(); } + +bool SmartereyeDevice::init(const std::shared_ptr& camera_config) { + pcamera_ = StereoCamera::connect("192.168.1.251"); + pcameraHandler_ = new SmartereyeHandler("camera A"); + pcamera_->enableTasks(TaskId::ObstacleTask | TaskId::DisplayTask); + inited_ = true; + + return true; +} + +bool SmartereyeDevice::SetCallback(CallbackFunc ptr) { + pcameraHandler_->SetCallback(ptr); + + return true; +} + +int SmartereyeDevice::poll() { + pcamera_->requestFrame(pcameraHandler_, FrameId::Compound); + is_capturing_ = true; + + return 1; +} + +int SmartereyeDevice::uninit() { + if (!inited_) { + return 1; + } + + pcamera_->disconnectFromServer(); + is_capturing_ = false; + inited_ = false; + + return 1; +} + +bool SmartereyeDevice::is_capturing() { return is_capturing_; } + +bool SmartereyeDevice::wait_for_device() { + if (is_capturing_) { + ADEBUG << "is capturing"; + return true; + } + + return true; +} + +} // namespace smartereye +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/smartereye/smartereye_device.h b/modules/drivers/smartereye/smartereye_device.h new file mode 100755 index 00000000000..26a2b2af649 --- /dev/null +++ b/modules/drivers/smartereye/smartereye_device.h @@ -0,0 +1,68 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "cyber/cyber.h" +#include "modules/drivers/smartereye/proto/config.pb.h" +#include "modules/drivers/smartereye/smartereye_handler.h" +#include "third_party/camera_library/smartereye/include/LdwDataInterface.h" +#include "third_party/camera_library/smartereye/include/calibrationparams.h" +#include "third_party/camera_library/smartereye/include/camerahandler.h" +#include "third_party/camera_library/smartereye/include/disparityconvertor.h" +#include "third_party/camera_library/smartereye/include/frameid.h" +#include "third_party/camera_library/smartereye/include/obstacleData.h" +#include "third_party/camera_library/smartereye/include/rotationmatrix.h" +#include "third_party/camera_library/smartereye/include/satpext.h" +#include "third_party/camera_library/smartereye/include/stereocamera.h" +#include "third_party/camera_library/smartereye/include/taskiddef.h" + +namespace apollo { +namespace drivers { +namespace smartereye { + +using apollo::drivers::smartereye::config::Config; + +class SmartereyeDevice { + public: + SmartereyeDevice(); + virtual ~SmartereyeDevice(); + virtual bool init(const std::shared_ptr &camera_config); + bool is_capturing(); + bool wait_for_device(); + int uninit(); + bool SetCallback(CallbackFunc ptr); + int poll(); + + private: + bool is_capturing_ = false; + bool inited_ = false; + std::shared_ptr config_; + StereoCamera *pcamera_ = nullptr; + SmartereyeHandler *pcameraHandler_ = nullptr; +}; + +} // namespace smartereye +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/smartereye/smartereye_handler.cc b/modules/drivers/smartereye/smartereye_handler.cc new file mode 100755 index 00000000000..f434e0358ab --- /dev/null +++ b/modules/drivers/smartereye/smartereye_handler.cc @@ -0,0 +1,53 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ +#include "modules/drivers/smartereye/smartereye_handler.h" + +#include +#include +#include + +#include "third_party/camera_library/smartereye/include/LdwDataInterface.h" +#include "third_party/camera_library/smartereye/include/disparityconvertor.h" +#include "third_party/camera_library/smartereye/include/frameid.h" +#include "third_party/camera_library/smartereye/include/obstacleData.h" +#include "third_party/camera_library/smartereye/include/satpext.h" + +namespace apollo { +namespace drivers { +namespace smartereye { + +SmartereyeHandler::SmartereyeHandler(std::string name) + : mName(name) { + pCallbackFunc = nullptr; +} + +SmartereyeHandler::~SmartereyeHandler() { + pCallbackFunc = nullptr; +} + +bool SmartereyeHandler::SetCallback(CallbackFunc ptr) { + pCallbackFunc = ptr; + + return true; +} + +void SmartereyeHandler::handleRawFrame(const RawImageFrame *rawFrame) { + pCallbackFunc(const_cast(rawFrame)); +} + +} // namespace smartereye +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/smartereye/smartereye_handler.h b/modules/drivers/smartereye/smartereye_handler.h new file mode 100755 index 00000000000..f987efaa2f0 --- /dev/null +++ b/modules/drivers/smartereye/smartereye_handler.h @@ -0,0 +1,55 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include "cyber/cyber.h" +#include "third_party/camera_library/smartereye/include/calibrationparams.h" +#include "third_party/camera_library/smartereye/include/camerahandler.h" +#include "third_party/camera_library/smartereye/include/rotationmatrix.h" + +namespace apollo { +namespace drivers { +namespace smartereye { + +typedef std::function CallbackFunc; + +class SmartereyeHandler : public CameraHandler { + public: + explicit SmartereyeHandler(std::string name); + ~SmartereyeHandler(); + void handleRawFrame(const RawImageFrame *rawFrame); + bool SetCallback(CallbackFunc ptr); + void handleUpdateFinished(Result result) {} + + protected: + void handleDisparityPointByPoint(unsigned char *image, int width, int height, + int bitNum) {} + void handleDisparityByLookupTable(unsigned char *image, int width, int height, + int bitNum) {} + + private: + std::string mName = nullptr; + CallbackFunc pCallbackFunc = nullptr; +}; + +} // namespace smartereye +} // namespace drivers +} // namespace apollo