tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Publish more camera infos

Browse Source
This commit is contained in:
John Zhao 2018-07-22 12:20:27 +08:00
parent 0f9bdd5cf2
commit 384ff028a8
1 changed files with 29 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -166,24 +166,21 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) { for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
auto &&topic = stream_topics[it->first]; auto &&topic = stream_topics[it->first];
if (it->first == Stream::LEFT || it->first == Stream::RIGHT) { // camera if (it->first == Stream::POINTS) { // pointcloud
camera_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1);
} else if (it->first == Stream::POINTS) { // pointcloud
points_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>(topic, 1); points_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>(topic, 1);
} else { // image } else { // camera
image_publishers_[it->first] = it_mynteye.advertise(topic, 1); camera_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1);
} }
NODELET_INFO_STREAM("Advertized on topic " << topic); NODELET_INFO_STREAM("Advertized on topic " << topic);
} }
camera_encodings_ = {{Stream::LEFT, enc::MONO8}, camera_encodings_ = {{Stream::LEFT, enc::MONO8},
{Stream::RIGHT, enc::MONO8}}; {Stream::RIGHT, enc::MONO8},
{Stream::LEFT_RECTIFIED, enc::MONO8},
image_encodings_ = {{Stream::LEFT_RECTIFIED, enc::MONO8}, {Stream::RIGHT_RECTIFIED, enc::MONO8},
{Stream::RIGHT_RECTIFIED, enc::MONO8}, {Stream::DISPARITY, enc::MONO8}, // float
{Stream::DISPARITY, enc::MONO8}, // float {Stream::DISPARITY_NORMALIZED, enc::MONO8},
{Stream::DISPARITY_NORMALIZED, enc::MONO8}, {Stream::DEPTH, enc::MONO16}};
{Stream::DEPTH, enc::MONO16}};
pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 1); pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 1);
NODELET_INFO_STREAM("Advertized on topic " << imu_topic); NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
@ -194,9 +191,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// stream toggles // stream toggles
for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) { for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
if (it->first == Stream::LEFT || it->first == Stream::RIGHT) { // camera if (it->first == Stream::LEFT || it->first == Stream::RIGHT) {
continue; continue; // native streams
} else { // image, pointcloud } else {
if (!api_->Supports(it->first)) if (!api_->Supports(it->first))
continue; continue;
bool enabled = false; bool enabled = false;
@ -292,16 +289,17 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
<< ", exposure_time: " << data.img->exposure_time); << ", exposure_time: " << data.img->exposure_time);
}); });
std::vector<Stream> image_streams{ std::vector<Stream> other_streams{
Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, Stream::DISPARITY, Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, Stream::DISPARITY,
Stream::DISPARITY_NORMALIZED, Stream::DEPTH}; Stream::DISPARITY_NORMALIZED, Stream::DEPTH};
for (auto &&stream : image_streams) { for (auto &&stream : other_streams) {
api_->SetStreamCallback( api_->SetStreamCallback(
stream, [this, stream](const api::StreamData &data) { stream, [this, stream](const api::StreamData &data) {
// data.img is null, not hard timestamp
static std::size_t count = 0; static std::size_t count = 0;
++count; ++count;
publishImage(stream, data, count, ros::Time::now()); publishCamera(stream, data, count, ros::Time::now());
}); });
} }
@ -365,6 +363,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
camera_publishers_[stream].publish(msg, info); camera_publishers_[stream].publish(msg, info);
} }
/*
void publishImage( void publishImage(
const Stream &stream, const api::StreamData &data, std::uint32_t seq, const Stream &stream, const api::StreamData &data, std::uint32_t seq,
ros::Time stamp) { ros::Time stamp) {
@ -382,6 +381,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
cv_bridge::CvImage(header, image_encodings_[stream], img).toImageMsg(); cv_bridge::CvImage(header, image_encodings_[stream], img).toImageMsg();
image_publishers_[stream].publish(msg); image_publishers_[stream].publish(msg);
} }
*/
void publishPoints( void publishPoints(
const api::StreamData &data, std::uint32_t seq, ros::Time stamp) { const api::StreamData &data, std::uint32_t seq, ros::Time stamp) {
@ -584,7 +584,12 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
sensor_msgs::CameraInfo *camera_info = new sensor_msgs::CameraInfo(); sensor_msgs::CameraInfo *camera_info = new sensor_msgs::CameraInfo();
camera_info_ptrs_[stream] = sensor_msgs::CameraInfoPtr(camera_info); camera_info_ptrs_[stream] = sensor_msgs::CameraInfoPtr(camera_info);
auto &&in = api_->GetIntrinsics(stream); Intrinsics in;
if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) {
in = api_->GetIntrinsics(Stream::RIGHT);
} else {
in = api_->GetIntrinsics(Stream::LEFT);
}
camera_info->header.frame_id = frame_ids_[stream]; camera_info->header.frame_id = frame_ids_[stream];
camera_info->width = in.width; camera_info->width = in.width;
@ -603,7 +608,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// P = [ 0 fy' cy' Ty] // P = [ 0 fy' cy' Ty]
// [ 0 0 1 0] // [ 0 0 1 0]
cv::Mat p = left_p_; cv::Mat p = left_p_;
if (stream == Stream::RIGHT) { if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) {
p = right_p_; p = right_p_;
} }
for (int i = 0; i < p.rows; i++) { for (int i = 0; i < p.rows; i++) {
@ -787,16 +792,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
ros::NodeHandle nh_; ros::NodeHandle nh_;
ros::NodeHandle private_nh_; ros::NodeHandle private_nh_;
// camera: LEFT, RIGHT // camera:
// LEFT, RIGHT, LEFT_RECTIFIED, RIGHT_RECTIFIED,
// DISPARITY, DISPARITY_NORMALIZED,
// DEPTH
std::map<Stream, image_transport::CameraPublisher> camera_publishers_; std::map<Stream, image_transport::CameraPublisher> camera_publishers_;
std::map<Stream, sensor_msgs::CameraInfoPtr> camera_info_ptrs_; std::map<Stream, sensor_msgs::CameraInfoPtr> camera_info_ptrs_;
std::map<Stream, std::string> camera_encodings_; std::map<Stream, std::string> camera_encodings_;
// image: LEFT_RECTIFIED, RIGHT_RECTIFIED, DISPARITY, DISPARITY_NORMALIZED,
// DEPTH
std::map<Stream, image_transport::Publisher> image_publishers_;
std::map<Stream, std::string> image_encodings_;
// pointcloud: POINTS // pointcloud: POINTS
ros::Publisher points_publisher_; ros::Publisher points_publisher_;