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 6b8f792..c40222e 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -13,6 +13,9 @@ + + + @@ -102,6 +105,9 @@ + + + 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 0fd02ab..d378a1a 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz @@ -7,6 +7,7 @@ Panels: - /Global Options1 - /Status1 - /Images1 + - /Images1/Gray1 - /Images1/Rectified1 - /Disparity1 Splitter Ratio: 0.5 @@ -77,6 +78,34 @@ Visualization Manager: Transport Hint: raw Unreliable: false Value: true + - Class: rviz/Group + Displays: + - Class: rviz/Image + Enabled: true + Image Topic: /mynteye/left/image_gray + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: LeftGray + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /mynteye/right/image_gray + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RightGray + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: false + Name: Gray - Class: rviz/Group Displays: - Class: rviz/Image 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 80b941f..3f6d0a4 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 @@ -115,6 +115,15 @@ class ROSWrapperNodelet : public nodelet::Nodelet { private_nh_.getParam(it->second + "_topic", stream_topics[it->first]); } + std::map gray_names{{Stream::LEFT, "left_gray"}, + {Stream::RIGHT, "right_gray"}}; + + std::map gray_topics{}; + for (auto &&it = gray_names.begin(); it != gray_names.end(); ++it) { + gray_topics[it->first] = it->second; + private_nh_.getParam(it->second + "_topic", gray_topics[it->first]); + } + std::string imu_topic = "imu"; std::string temp_topic = "temp"; private_nh_.getParam("imu_topic", imu_topic); @@ -179,6 +188,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet { NODELET_INFO_STREAM("Advertized on topic " << topic); } + for (auto &&it = gray_topics.begin(); it != gray_topics.end(); ++it) { + auto &&topic = gray_topics[it->first]; + if (it->first == Stream::LEFT || it->first == Stream::RIGHT) { // camera + gray_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1); + } + NODELET_INFO_STREAM("Advertized on topic " << topic); + } + camera_encodings_ = {{Stream::LEFT, enc::BGR8}, {Stream::RIGHT, enc::BGR8}}; image_encodings_ = {{Stream::LEFT_RECTIFIED, enc::BGR8}, @@ -274,6 +291,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { ++left_count_; publishCamera(Stream::LEFT, data, left_count_, stamp); + publishGray(Stream::LEFT, data, left_count_, stamp); NODELET_DEBUG_STREAM( Stream::LEFT << ", count: " << left_count_ << ", frame_id: " << data.img->frame_id @@ -287,6 +305,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet { ++right_count_; publishCamera(Stream::RIGHT, data, right_count_, stamp); + publishGray(Stream::RIGHT, data, right_count_, stamp); NODELET_DEBUG_STREAM( Stream::RIGHT << ", count: " << right_count_ << ", frame_id: " << data.img->frame_id @@ -400,6 +419,23 @@ class ROSWrapperNodelet : public nodelet::Nodelet { image_publishers_[stream].publish(msg); } + void publishGray( + const Stream &stream, const api::StreamData &data, std::uint32_t seq, + ros::Time stamp) { + if (gray_publishers_[stream].getNumSubscribers() == 0) + return; + std_msgs::Header header; + header.seq = seq; + header.stamp = stamp; + header.frame_id = frame_ids_[stream]; + cv::Mat gray; + cv::cvtColor(data.frame, gray, CV_RGB2GRAY); + auto &&msg = cv_bridge::CvImage(header, enc::MONO8, gray).toImageMsg(); + auto &&info = getCameraInfo(stream); + info->header.stamp = msg->header.stamp; + gray_publishers_[stream].publish(msg, info); + } + void publishPoints( const api::StreamData &data, std::uint32_t seq, ros::Time stamp) { if (points_publisher_.getNumSubscribers() == 0) @@ -842,6 +878,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet { std::map image_publishers_; std::map image_encodings_; + // gray: LEFT, RIGHT + std::map gray_publishers_; + // pointcloud: POINTS ros::Publisher points_publisher_;