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

Impl ros wrapper with api layer

Browse Source
This commit is contained in:
John Zhao 2018-04-28 14:54:23 +08:00
parent 2111a183d0
commit 96b5e20580
2 changed files with 25 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt
View File

@ -42,6 +42,9 @@ checkPackage("sensor_msgs" "")
checkPackage("std_msgs" "") checkPackage("std_msgs" "")
checkPackage("tf" "") checkPackage("tf" "")
find_package(OpenCV REQUIRED)
message(STATUS "Found OpenCV: ${OpenCV_VERSION}")
## messages ## messages
add_message_files( add_message_files(
@ -78,6 +81,7 @@ include_directories(
set(LINK_LIBS set(LINK_LIBS
${catkin_LIBRARIES} ${catkin_LIBRARIES}
${OpenCV_LIBS}
mynteye mynteye
) )

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

@ -16,6 +16,7 @@
#include <map> #include <map>
#include <string> #include <string>
#include "mynteye/api.h"
#include "mynteye/context.h" #include "mynteye/context.h"
#include "mynteye/device.h" #include "mynteye/device.h"
@ -32,8 +33,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
~ROSWrapperNodelet() { ~ROSWrapperNodelet() {
// std::cout << __func__ << std::endl; // std::cout << __func__ << std::endl;
if (device_) { if (api_) {
device_->Stop(Source::ALL); api_->Stop(Source::ALL);
} }
if (time_beg_ != -1) { if (time_beg_ != -1) {
double time_end = ros::Time::now().toSec(); double time_end = ros::Time::now().toSec();
@ -55,7 +56,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
void onInit() override { void onInit() override {
initDevice(); initDevice();
NODELET_FATAL_COND(device_ == nullptr, "No MYNT EYE device selected :("); NODELET_FATAL_COND(api_ == nullptr, "No MYNT EYE device selected :(");
nh_ = getMTNodeHandle(); nh_ = getMTNodeHandle();
private_nh_ = getMTPrivateNodeHandle(); private_nh_ = getMTPrivateNodeHandle();
@ -106,10 +107,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
private_nh_.getParam(it->first, value); private_nh_.getParam(it->first, value);
if (value != -1) { if (value != -1) {
NODELET_INFO_STREAM("Set " << it->first << " to " << value); NODELET_INFO_STREAM("Set " << it->first << " to " << value);
device_->SetOptionValue(it->second, value); api_->SetOptionValue(it->second, value);
} }
NODELET_INFO_STREAM( NODELET_INFO_STREAM(
it->second << ": " << device_->GetOptionValue(it->second)); it->second << ": " << api_->GetOptionValue(it->second));
} }
// image publishers // image publishers
@ -182,8 +183,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
void publishTopics() { void publishTopics() {
device_->SetStreamCallback( api_->SetStreamCallback(
Stream::LEFT, [this](const device::StreamData &data) { Stream::LEFT, [this](const api::StreamData &data) {
static double ros_time_beg = ros::Time::now().toSec(); static double ros_time_beg = ros::Time::now().toSec();
static double img_time_beg = data.img->timestamp; static double img_time_beg = data.img->timestamp;
ros::Time stamp( ros::Time stamp(
@ -208,8 +209,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
<< ", exposure_time: " << data.img->exposure_time); << ", exposure_time: " << data.img->exposure_time);
}); });
device_->SetStreamCallback( api_->SetStreamCallback(
Stream::RIGHT, [this](const device::StreamData &data) { Stream::RIGHT, [this](const api::StreamData &data) {
static double ros_time_beg = ros::Time::now().toSec(); static double ros_time_beg = ros::Time::now().toSec();
static double img_time_beg = data.img->timestamp; static double img_time_beg = data.img->timestamp;
ros::Time stamp( ros::Time stamp(
@ -224,7 +225,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
<< ", exposure_time: " << data.img->exposure_time); << ", exposure_time: " << data.img->exposure_time);
}); });
device_->SetMotionCallback([this](const device::MotionData &data) { api_->SetMotionCallback([this](const api::MotionData &data) {
static double ros_time_beg = ros::Time::now().toSec(); static double ros_time_beg = ros::Time::now().toSec();
static double imu_time_beg = data.imu->timestamp; static double imu_time_beg = data.imu->timestamp;
ros::Time stamp( ros::Time stamp(
@ -257,34 +258,31 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}); });
time_beg_ = ros::Time::now().toSec(); time_beg_ = ros::Time::now().toSec();
device_->Start(Source::ALL); api_->Start(Source::ALL);
} }
void publishLeft( void publishLeft(
const device::StreamData &data, std::uint32_t seq, ros::Time stamp) { const api::StreamData &data, std::uint32_t seq, ros::Time stamp) {
std_msgs::Header header; std_msgs::Header header;
header.seq = seq; header.seq = seq;
header.stamp = stamp; header.stamp = stamp;
header.frame_id = left_frame_id_; header.frame_id = left_frame_id_;
cv::Mat img( pub_left_.publish(
data.frame->height(), data.frame->width(), CV_8UC1, data.frame->data()); cv_bridge::CvImage(header, enc::MONO8, data.frame).toImageMsg());
pub_left_.publish(cv_bridge::CvImage(header, enc::MONO8, img).toImageMsg());
} }
void publishRight( void publishRight(
const device::StreamData &data, std::uint32_t seq, ros::Time stamp) { const api::StreamData &data, std::uint32_t seq, ros::Time stamp) {
std_msgs::Header header; std_msgs::Header header;
header.seq = seq; header.seq = seq;
header.stamp = stamp; header.stamp = stamp;
header.frame_id = right_frame_id_; header.frame_id = right_frame_id_;
cv::Mat img(
data.frame->height(), data.frame->width(), CV_8UC1, data.frame->data());
pub_right_.publish( pub_right_.publish(
cv_bridge::CvImage(header, enc::MONO8, img).toImageMsg()); cv_bridge::CvImage(header, enc::MONO8, data.frame).toImageMsg());
} }
void publishImu( void publishImu(
const device::MotionData &data, std::uint32_t seq, ros::Time stamp) { const api::MotionData &data, std::uint32_t seq, ros::Time stamp) {
sensor_msgs::Imu msg; sensor_msgs::Imu msg;
msg.header.seq = seq; msg.header.seq = seq;
@ -373,7 +371,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
} }
device_ = device; api_ = API::Create(device);
} }
ros::NodeHandle nh_; ros::NodeHandle nh_;
@ -397,9 +395,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
double gravity_; double gravity_;
// device // api
std::shared_ptr<Device> device_; std::shared_ptr<API> api_;
double time_beg_ = -1; double time_beg_ = -1;
std::size_t left_count_ = 0; std::size_t left_count_ = 0;