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