From f636ba3a86e3a920aa9e1a6770dfbf4667d7ac02 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Mon, 30 Apr 2018 10:51:09 +0800 Subject: [PATCH] Support enable stream and publish them in ros --- .../launch/mynteye.launch | 48 ++++- .../mynt_eye_ros_wrapper/rviz/mynteye.rviz | 165 ++++++++++++--- .../src/wrapper_nodelet.cc | 198 +++++++++++++----- 3 files changed, 331 insertions(+), 80 deletions(-) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch index f601210..883d909 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -6,17 +6,40 @@ + + + + + + + + + + + + + + + + + + + + + + + @@ -56,7 +79,7 @@ - + @@ -73,17 +96,40 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz index 1f82404..97eb3ff 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz @@ -6,6 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /Images1 + - /Images1/Rectified1 + - /Disparity1 Splitter Ratio: 0.5 Tree Height: 853 - Class: rviz/Selection @@ -48,6 +51,132 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true + - Class: rviz/Group + Displays: + - Class: rviz/Image + Enabled: true + Image Topic: /mynteye/left + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Left + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /mynteye/right + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Right + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Group + Displays: + - Class: rviz/Image + Enabled: true + Image Topic: /mynteye/left_rect + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: LeftRect + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /mynteye/right_rect + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RightRect + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: false + Name: Rectified + Enabled: true + Name: Images + - Class: rviz/Group + Displays: + - Class: rviz/Image + Enabled: true + Image Topic: /mynteye/disparity + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Disparity + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /mynteye/disparity_norm + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: DisparityNorm + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: false + Name: Disparity + - Class: rviz/Image + Enabled: true + Image Topic: /mynteye/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: 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: "" + 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 + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Flat Squares + Topic: /mynteye/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true - Alpha: 1 Class: rviz_plugin_tutorials/Imu Color: 204; 51; 204 @@ -57,30 +186,6 @@ Visualization Manager: Topic: /mynteye/imu Unreliable: false Value: false - - Class: rviz/Image - Enabled: true - Image Topic: /mynteye/left - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Left - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /mynteye/right - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Right - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -127,6 +232,12 @@ Visualization Manager: Yaw: 0.785398006 Saved: ~ Window Geometry: + Depth: + collapsed: false + Disparity: + collapsed: false + DisparityNorm: + collapsed: false Displays: collapsed: false Height: 1056 @@ -134,9 +245,13 @@ Window Geometry: Hide Right Dock: false Left: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bb0000011e0000024f00000191fb0000000a00520069006700680074030000040c0000011e0000025000000190000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + LeftRect: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bd000000860000024e00000190fb0000000a00520069006700680074030000040c000000860000025000000190fb00000010004c006500660074005200650063007402000001bd000002170000024e00000190fb00000012005200690067006800740052006500630074020000040c000002170000024f00000190fb0000000a0044006500700074006803000001bc000002170000024f00000190fb0000001200440069007300700061007200690074007902000001bd000002170000024e0000018ffb0000001a004400690073007000610072006900740079004e006f0072006d020000040c000002170000025100000190000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Right: collapsed: false + RightRect: + collapsed: false Selection: collapsed: false Time: diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 1e06be9..8bf33e9 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -4,7 +4,9 @@ #include #include #include +#include #include +#include #include #include @@ -63,10 +65,21 @@ class ROSWrapperNodelet : public nodelet::Nodelet { // node params - std::string left_topic = "left"; - std::string right_topic = "right"; - private_nh_.getParam("left_topic", left_topic); - private_nh_.getParam("right_topic", right_topic); + std::map stream_names{ + {Stream::LEFT, "left"}, + {Stream::RIGHT, "right"}, + {Stream::LEFT_RECTIFIED, "left_rect"}, + {Stream::RIGHT_RECTIFIED, "right_rect"}, + {Stream::DISPARITY, "disparity"}, + {Stream::DISPARITY_NORMALIZED, "disparity_norm"}, + {Stream::DEPTH, "depth"}, + {Stream::POINTS, "points"}}; + + std::map stream_topics{}; + for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) { + stream_topics[it->first] = it->second; + private_nh_.getParam(it->second + "_topic", stream_topics[it->first]); + } std::string imu_topic = "imu"; std::string temp_topic = "temp"; @@ -76,10 +89,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet { base_frame_id_ = "camera_link"; private_nh_.getParam("base_frame_id", base_frame_id_); - camera_frame_ids_[Stream::LEFT] = "camera_left_frame"; - camera_frame_ids_[Stream::RIGHT] = "camera_right_frame"; - private_nh_.getParam("left_frame_id", camera_frame_ids_[Stream::LEFT]); - private_nh_.getParam("right_frame_id", camera_frame_ids_[Stream::RIGHT]); + for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) { + frame_ids_[it->first] = "camera_" + it->second + "_frame"; + private_nh_.getParam(it->second + "_frame_id", frame_ids_[it->first]); + } imu_frame_id_ = "camera_imu_frame"; temp_frame_id_ = "camera_temp_frame"; @@ -91,55 +104,79 @@ class ROSWrapperNodelet : public nodelet::Nodelet { // device options - std::map option_map = { - {"gain", Option::GAIN}, - {"brightness", Option::BRIGHTNESS}, - {"contrast", Option::CONTRAST}, - {"frame_rate", Option::FRAME_RATE}, - {"imu_frequency", Option::IMU_FREQUENCY}, - {"exposure_mode", Option::EXPOSURE_MODE}, - {"max_gain", Option::MAX_GAIN}, - {"max_exposure_time", Option::MAX_EXPOSURE_TIME}, - {"desired_brightness", Option::DESIRED_BRIGHTNESS}, - {"ir_control", Option::IR_CONTROL}, - {"hdr_mode", Option::HDR_MODE}, + std::map option_names = { + {Option::GAIN, "gain"}, + {Option::BRIGHTNESS, "brightness"}, + {Option::CONTRAST, "contrast"}, + {Option::FRAME_RATE, "frame_rate"}, + {Option::IMU_FREQUENCY, "imu_frequency"}, + {Option::EXPOSURE_MODE, "exposure_mode"}, + {Option::MAX_GAIN, "max_gain"}, + {Option::MAX_EXPOSURE_TIME, "max_exposure_time"}, + {Option::DESIRED_BRIGHTNESS, "desired_brightness"}, + {Option::IR_CONTROL, "ir_control"}, + {Option::HDR_MODE, "hdr_mode"}, }; - int value; - for (auto &&it = option_map.begin(); it != option_map.end(); it++) { - value = -1; - private_nh_.getParam(it->first, value); + for (auto &&it = option_names.begin(); it != option_names.end(); ++it) { + if (!api_->Supports(it->first)) + continue; + int value = -1; + private_nh_.getParam(it->second, value); if (value != -1) { - NODELET_INFO_STREAM("Set " << it->first << " to " << value); - api_->SetOptionValue(it->second, value); + NODELET_INFO_STREAM("Set " << it->second << " to " << value); + api_->SetOptionValue(it->first, value); } - NODELET_INFO_STREAM( - it->second << ": " << api_->GetOptionValue(it->second)); + NODELET_INFO_STREAM(it->first << ": " << api_->GetOptionValue(it->first)); } - // image publishers + // publishers image_transport::ImageTransport it_mynteye(nh_); - camera_encodings_[Stream::LEFT] = enc::MONO8; - camera_publishers_[Stream::LEFT] = - it_mynteye.advertiseCamera(left_topic, 1); - NODELET_INFO_STREAM("Advertized on topic " << left_topic); + for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) { + auto &&topic = stream_topics[it->first]; + if (it->first == Stream::LEFT || it->first == Stream::RIGHT) { // camera + camera_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1); + } else if (it->first == Stream::POINTS) { // pointcloud + points_publisher_ = nh_.advertise(topic, 1); + } else { // image + image_publishers_[it->first] = it_mynteye.advertise(topic, 1); + } + NODELET_INFO_STREAM("Advertized on topic " << topic); + } - camera_encodings_[Stream::RIGHT] = enc::MONO8; - camera_publishers_[Stream::RIGHT] = - it_mynteye.advertiseCamera(right_topic, 1); - NODELET_INFO_STREAM("Advertized on topic " << right_topic); + camera_encodings_ = {{Stream::LEFT, enc::MONO8}, + {Stream::RIGHT, enc::MONO8}}; - // imu publisher + image_encodings_ = {{Stream::LEFT_RECTIFIED, enc::MONO8}, + {Stream::RIGHT_RECTIFIED, enc::MONO8}, + {Stream::DISPARITY, enc::MONO8}, // float + {Stream::DISPARITY_NORMALIZED, enc::MONO8}, + {Stream::DEPTH, enc::MONO16}}; pub_imu_ = nh_.advertise(imu_topic, 1); NODELET_INFO_STREAM("Advertized on topic " << imu_topic); - // temp publisher - pub_temp_ = nh_.advertise(temp_topic, 1); NODELET_INFO_STREAM("Advertized on topic " << temp_topic); + // stream toggles + + for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) { + if (it->first == Stream::LEFT || it->first == Stream::RIGHT) { // camera + continue; + } else { // image, pointcloud + if (!api_->Supports(it->first)) + continue; + bool enabled = false; + private_nh_.getParam("enable_" + it->second, enabled); + if (enabled) { + api_->EnableStreamData(it->first); + NODELET_INFO_STREAM("Enable stream data of " << it->first); + } + } + } + publishStaticTransforms(); publishTopics(); } @@ -151,7 +188,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { geometry_msgs::TransformStamped b2l_msg; b2l_msg.header.stamp = tf_stamp; b2l_msg.header.frame_id = base_frame_id_; - b2l_msg.child_frame_id = camera_frame_ids_[Stream::LEFT]; + b2l_msg.child_frame_id = frame_ids_[Stream::LEFT]; b2l_msg.transform.translation.x = 0; b2l_msg.transform.translation.y = 0; b2l_msg.transform.translation.z = 0; @@ -162,17 +199,24 @@ class ROSWrapperNodelet : public nodelet::Nodelet { static_tf_broadcaster_.sendTransform(b2l_msg); // Transform base frame to right frame + auto &&b2r_ex = api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT); + tf::Quaternion b2r_q; + tf::Matrix3x3 b2r_r( + b2r_ex.rotation[0][0], b2r_ex.rotation[0][1], b2r_ex.rotation[0][2], + b2r_ex.rotation[1][0], b2r_ex.rotation[1][1], b2r_ex.rotation[1][2], + b2r_ex.rotation[2][0], b2r_ex.rotation[2][1], b2r_ex.rotation[2][2]); + b2r_r.getRotation(b2r_q); geometry_msgs::TransformStamped b2r_msg; b2r_msg.header.stamp = tf_stamp; b2r_msg.header.frame_id = base_frame_id_; - b2r_msg.child_frame_id = camera_frame_ids_[Stream::RIGHT]; - b2r_msg.transform.translation.x = 0; - b2r_msg.transform.translation.y = 0; - b2r_msg.transform.translation.z = 0; - b2r_msg.transform.rotation.x = 0; - b2r_msg.transform.rotation.y = 0; - b2r_msg.transform.rotation.z = 0; - b2r_msg.transform.rotation.w = 1; + b2r_msg.child_frame_id = frame_ids_[Stream::RIGHT]; + b2r_msg.transform.translation.x = b2r_ex.translation[0]; + b2r_msg.transform.translation.y = b2r_ex.translation[1]; + b2r_msg.transform.translation.z = b2r_ex.translation[2]; + b2r_msg.transform.rotation.x = b2r_q.getX(); + b2r_msg.transform.rotation.y = b2r_q.getY(); + b2r_msg.transform.rotation.z = b2r_q.getZ(); + b2r_msg.transform.rotation.w = b2r_q.getW(); static_tf_broadcaster_.sendTransform(b2r_msg); // Transform base frame to imu frame @@ -233,6 +277,19 @@ class ROSWrapperNodelet : public nodelet::Nodelet { << ", exposure_time: " << data.img->exposure_time); }); + std::vector image_streams{ + Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, Stream::DISPARITY, + Stream::DISPARITY_NORMALIZED, Stream::DEPTH}; + + for (auto &&stream : image_streams) { + api_->SetStreamCallback( + stream, [this, stream](const api::StreamData &data) { + static std::size_t count = 0; + ++count; + publishImage(stream, data, count, ros::Time::now()); + }); + } + api_->SetMotionCallback([this](const api::MotionData &data) { static double ros_time_beg = ros::Time::now().toSec(); static double imu_time_beg = data.imu->timestamp; @@ -277,15 +334,39 @@ class ROSWrapperNodelet : public nodelet::Nodelet { std_msgs::Header header; header.seq = seq; header.stamp = stamp; - header.frame_id = camera_frame_ids_[stream]; + header.frame_id = frame_ids_[stream]; + cv::Mat img = data.frame; + if (stream == Stream::DISPARITY) { // 32FC1 > 8UC1 = MONO8 + img.convertTo(img, CV_8UC1); + } auto &&msg = - cv_bridge::CvImage(header, camera_encodings_[stream], data.frame) - .toImageMsg(); + cv_bridge::CvImage(header, camera_encodings_[stream], img).toImageMsg(); auto &&info = getCameraInfo(stream); info->header.stamp = msg->header.stamp; camera_publishers_[stream].publish(msg, info); } + void publishImage( + const Stream &stream, const api::StreamData &data, std::uint32_t seq, + ros::Time stamp) { + if (image_publishers_[stream].getNumSubscribers() == 0) + return; + std_msgs::Header header; + header.seq = seq; + header.stamp = stamp; + header.frame_id = frame_ids_[stream]; + cv::Mat img = data.frame; + if (stream == Stream::DISPARITY) { // 32FC1 > 8UC1 = MONO8 + img.convertTo(img, CV_8UC1); + } + auto &&msg = + cv_bridge::CvImage(header, image_encodings_[stream], img).toImageMsg(); + image_publishers_[stream].publish(msg); + } + + // void publishPoints( + // ) + void publishImu( const api::MotionData &data, std::uint32_t seq, ros::Time stamp) { if (pub_imu_.getNumSubscribers() == 0) @@ -394,7 +475,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { auto &&in = api_->GetIntrinsics(stream); - camera_info->header.frame_id = camera_frame_ids_[stream]; + camera_info->header.frame_id = frame_ids_[stream]; camera_info->width = in.width; camera_info->height = in.height; @@ -456,10 +537,19 @@ class ROSWrapperNodelet : public nodelet::Nodelet { ros::NodeHandle nh_; ros::NodeHandle private_nh_; + // camera: LEFT, RIGHT std::map camera_publishers_; std::map camera_info_ptrs_; std::map camera_encodings_; + // image: LEFT_RECTIFIED, RIGHT_RECTIFIED, DISPARITY, DISPARITY_NORMALIZED, + // DEPTH + std::map image_publishers_; + std::map image_encodings_; + + // pointcloud: POINTS + ros::Publisher points_publisher_; + ros::Publisher pub_imu_; ros::Publisher pub_temp_; @@ -470,7 +560,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { std::string base_frame_id_; std::string imu_frame_id_; std::string temp_frame_id_; - std::map camera_frame_ids_; + std::map frame_ids_; double gravity_;