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_;