From 122fff172273a12d41a05bd1889d4dbbdf91387c Mon Sep 17 00:00:00 2001 From: John Zhao Date: Sat, 14 Apr 2018 14:56:05 +0800 Subject: [PATCH] Add ros wrapper --- wrappers/README.md | 9 + .../src/mynt_eye_ros_wrapper/CMakeLists.txt | 54 ++- .../launch/display.launch | 8 + .../launch/mynteye.launch | 96 +++++ .../mynt_eye_ros_wrapper/nodelet_plugins.xml | 7 + .../ros/src/mynt_eye_ros_wrapper/package.xml | 7 + .../mynt_eye_ros_wrapper/rviz/mynteye.rviz | 150 ++++++++ .../mynt_eye_ros_wrapper/src/wrapper_node.cc | 22 ++ .../src/wrapper_nodelet.cc | 354 ++++++++++++++++++ 9 files changed, 703 insertions(+), 4 deletions(-) create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/launch/display.launch create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/nodelet_plugins.xml create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_node.cc create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc diff --git a/wrappers/README.md b/wrappers/README.md index 7f657d0..350983d 100644 --- a/wrappers/README.md +++ b/wrappers/README.md @@ -12,4 +12,13 @@ make ros ### Run ```bash +source wrappers/ros/devel/setup.bash +roslaunch mynt_eye_ros_wrapper mynteye.launch +``` + +With RViz to preview, + +```bash +source wrappers/ros/devel/setup.bash +roslaunch mynt_eye_ros_wrapper display.launch ``` diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt index 441e183..e925dc6 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt @@ -1,10 +1,15 @@ cmake_minimum_required(VERSION 2.8.3) project(mynt_eye_ros_wrapper) + +get_filename_component(SDK_DIR "${PROJECT_SOURCE_DIR}/../../../.." ABSOLUTE) + if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() +# packages + function(checkPackage package customMessage) set(varName "${package}_FOUND") if (NOT "${${varName}}") @@ -19,34 +24,71 @@ endfunction(checkPackage) find_package(catkin REQUIRED COMPONENTS cv_bridge + geometry_msgs image_transport nodelet roscpp sensor_msgs std_msgs + tf ) checkPackage("cv_bridge" "") +checkPackage("geometry_msgs" "") checkPackage("image_transport" "") checkPackage("nodelet" "") checkPackage("roscpp" "") checkPackage("sensor_msgs" "") checkPackage("std_msgs" "") +checkPackage("tf" "") catkin_package( - CATKIN_DEPENDS cv_bridge image_transport nodelet roscpp sensor_msgs std_msgs + CATKIN_DEPENDS cv_bridge geometry_msgs image_transport nodelet roscpp sensor_msgs std_msgs tf ) -# Build +get_filename_component(SDK_DIR "${PROJECT_SOURCE_DIR}/../../../.." ABSOLUTE) + +LIST(APPEND CMAKE_PREFIX_PATH ${SDK_DIR}/third_party/glog/_build) +find_package(glog REQUIRED) +message(STATUS "Found glog: ${glog_VERSION}") + +LIST(APPEND CMAKE_PREFIX_PATH ${SDK_DIR}/_install/lib/cmake) +find_package(mynteye REQUIRED) +message(STATUS "Found mynteye: ${mynteye_VERSION}") + +# targets add_compile_options(-std=c++11) include_directories( -# include ${catkin_INCLUDE_DIRS} + ${SDK_DIR}/src # for device layer interface ) -# Install +set(LINK_LIBS + ${catkin_LIBRARIES} + mynteye +) + +add_library(mynteye_wrapper src/wrapper_nodelet.cc) +target_link_libraries(mynteye_wrapper ${LINK_LIBS}) + +add_executable(mynteye_wrapper_node src/wrapper_node.cc) +target_link_libraries(mynteye_wrapper_node mynteye_wrapper ${LINK_LIBS}) + +if(MSVC OR MSYS OR MINGW) + target_compile_definitions(mynteye_wrapper_node + PUBLIC GLOG_NO_ABBREVIATED_SEVERITIES + ) +endif() + +# install + +install(TARGETS mynteye_wrapper mynteye_wrapper_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} @@ -55,3 +97,7 @@ install(DIRECTORY launch install(DIRECTORY rviz DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) + +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/display.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/display.launch new file mode 100644 index 0000000..0cc7c0a --- /dev/null +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/display.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch new file mode 100644 index 0000000..6eeef94 --- /dev/null +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -0,0 +1,96 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/nodelet_plugins.xml b/wrappers/ros/src/mynt_eye_ros_wrapper/nodelet_plugins.xml new file mode 100644 index 0000000..ab31cd8 --- /dev/null +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/nodelet_plugins.xml @@ -0,0 +1,7 @@ + + + This is the nodelet of ROS interface for MYNT EYE camera. + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml b/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml index 63cd33d..68c3326 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml @@ -14,28 +14,35 @@ catkin cv_bridge + geometry_msgs image_transport nodelet roscpp sensor_msgs std_msgs + tf cv_bridge + geometry_msgs image_transport nodelet roscpp sensor_msgs std_msgs + tf cv_bridge + geometry_msgs image_transport nodelet roscpp sensor_msgs std_msgs + tf + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz new file mode 100644 index 0000000..1f82404 --- /dev/null +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz @@ -0,0 +1,150 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 853 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Left +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_plugin_tutorials/Imu + Color: 204; 51; 204 + Enabled: false + History Length: 1 + Name: Imu + 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 + Default Light: true + Fixed Frame: mynteye_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 14.7952175 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.785398006 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + Left: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bb0000011e0000024f00000191fb0000000a00520069006700680074030000040c0000011e0000025000000190000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Right: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1855 + X: 65 + Y: 24 diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_node.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_node.cc new file mode 100644 index 0000000..3a729df --- /dev/null +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_node.cc @@ -0,0 +1,22 @@ +#include +#include + +#include "mynteye/glog_init.h" + +int main(int argc, char *argv[]) { + glog_init _(argc, argv); + + ros::init(argc, argv, "mynteye_wrapper_node"); + ros::console::set_logger_level( + ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); + + nodelet::Loader nodelet; + nodelet::M_string remap(ros::names::getRemappings()); + nodelet::V_string nargv; + nodelet.load( + ros::this_node::getName(), "mynteye/ROSWrapperNodelet", remap, nargv); + + ros::spin(); + + return 0; +} 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 new file mode 100644 index 0000000..2076bee --- /dev/null +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -0,0 +1,354 @@ +#include // NOLINT +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "device/context.h" +#include "device/device.h" + +MYNTEYE_BEGIN_NAMESPACE + +namespace enc = sensor_msgs::image_encodings; + +class ROSWrapperNodelet : public nodelet::Nodelet { + public: + ROSWrapperNodelet() {} + + ~ROSWrapperNodelet() { + // std::cout << __func__ << std::endl; + if (device_) { + device_->Stop(Source::ALL); + } + if (time_beg_ != -1) { + double time_end = ros::Time::now().toSec(); + double time_elapsed = time_end - time_beg_; + + LOG(INFO) << "Time elapsed: " << time_elapsed << "s"; + LOG(INFO) << "Left count: " << left_count_ + << ", fps: " << (left_count_ / time_elapsed); + LOG(INFO) << "Right count: " << right_count_ + << ", fps: " << (right_count_ / time_elapsed); + LOG(INFO) << "Imu count: " << imu_count_ + << ", hz: " << (imu_count_ / time_elapsed); + + // ROS messages could not be reliably printed here, using glog instead :( + // ros::Duration(1).sleep(); // 1s + // https://answers.ros.org/question/35163/how-to-perform-an-action-at-nodelet-unload-shutdown/ + } + } + + void onInit() override { + initDevice(); + NODELET_FATAL_COND(device_ == nullptr, "No MYNT EYE device selected :("); + + nh_ = getMTNodeHandle(); + private_nh_ = getMTPrivateNodeHandle(); + + // node params + + std::string left_topic = "left"; + std::string right_topic = "right"; + std::string imu_topic = "imu"; + private_nh_.getParam("left_topic", left_topic); + private_nh_.getParam("right_topic", right_topic); + private_nh_.getParam("imu_topic", imu_topic); + + base_frame_id_ = "camera_link"; + left_frame_id_ = "camera_left_frame"; + right_frame_id_ = "camera_right_frame"; + imu_frame_id_ = "camera_imu_frame"; + private_nh_.getParam("base_frame_id", base_frame_id_); + private_nh_.getParam("left_frame_id", left_frame_id_); + private_nh_.getParam("right_frame_id", right_frame_id_); + private_nh_.getParam("imu_frame_id", imu_frame_id_); + + // 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}, + }; + int value; + for (auto &&it = option_map.begin(); it != option_map.end(); it++) { + value = -1; + private_nh_.getParam(it->first, value); + if (value != -1) { + NODELET_INFO_STREAM("Set " << it->first << " to " << value); + device_->SetOptionValue(it->second, value); + } + NODELET_INFO_STREAM( + it->second << ": " << device_->GetOptionValue(it->second)); + } + + // image publishers + + image_transport::ImageTransport it_mynteye(nh_); + + pub_left_ = it_mynteye.advertise(left_topic, 1); + NODELET_INFO_STREAM("Advertized on topic " << left_topic); + pub_right_ = it_mynteye.advertise(right_topic, 1); + NODELET_INFO_STREAM("Advertized on topic " << right_topic); + + // imu publisher + + pub_imu_ = nh_.advertise(imu_topic, 1); + NODELET_INFO_STREAM("Advertized on topic " << imu_topic); + + publishStaticTransforms(); + publishTopics(); + } + + void publishStaticTransforms() { + ros::Time tf_stamp = ros::Time::now(); + + // The left frame is used as the base frame. + geometry_msgs::TransformStamped b2l_msg; + b2l_msg.header.stamp = tf_stamp; + b2l_msg.header.frame_id = base_frame_id_; + b2l_msg.child_frame_id = left_frame_id_; + b2l_msg.transform.translation.x = 0; + b2l_msg.transform.translation.y = 0; + b2l_msg.transform.translation.z = 0; + b2l_msg.transform.rotation.x = 0; + b2l_msg.transform.rotation.y = 0; + b2l_msg.transform.rotation.z = 0; + b2l_msg.transform.rotation.w = 1; + static_tf_broadcaster_.sendTransform(b2l_msg); + + // Transform base frame to right frame + geometry_msgs::TransformStamped b2r_msg; + b2r_msg.header.stamp = tf_stamp; + b2r_msg.header.frame_id = base_frame_id_; + b2r_msg.child_frame_id = right_frame_id_; + 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; + static_tf_broadcaster_.sendTransform(b2r_msg); + + // Transform base frame to imu frame + geometry_msgs::TransformStamped b2i_msg; + b2i_msg.header.stamp = tf_stamp; + b2i_msg.header.frame_id = base_frame_id_; + b2i_msg.child_frame_id = imu_frame_id_; + b2i_msg.transform.translation.x = 0; + b2i_msg.transform.translation.y = 0; + b2i_msg.transform.translation.z = 0; + b2i_msg.transform.rotation.x = 0; + b2i_msg.transform.rotation.y = 0; + b2i_msg.transform.rotation.z = 0; + b2i_msg.transform.rotation.w = 1; + static_tf_broadcaster_.sendTransform(b2i_msg); + } + + void publishTopics() { + device_->SetStreamCallback( + Stream::LEFT, [this](const device::StreamData &data) { + ++left_count_; + publishLeft(data, left_count_, ros::Time::now()); + NODELET_DEBUG_STREAM( + Stream::LEFT << ", count: " << left_count_ + << ", frame_id: " << data.img->frame_id + << ", timestamp: " << data.img->timestamp + << ", exposure_time: " << data.img->exposure_time); + }); + + device_->SetStreamCallback( + Stream::RIGHT, [this](const device::StreamData &data) { + ++right_count_; + publishRight(data, right_count_, ros::Time::now()); + NODELET_DEBUG_STREAM( + Stream::RIGHT << ", count: " << right_count_ + << ", frame_id: " << data.img->frame_id + << ", timestamp: " << data.img->timestamp + << ", exposure_time: " << data.img->exposure_time); + }); + + device_->SetMotionCallback([this](const device::MotionData &data) { + static std::uint32_t imu_time_beg = -1; + static double imu_ros_time_beg; + if (imu_time_beg == -1) { + imu_time_beg = data.imu->timestamp; + imu_ros_time_beg = ros::Time::now().toSec(); + } + // 0.01 ms > 1 s + ros::Time stamp( + imu_ros_time_beg + (data.imu->timestamp - imu_time_beg) * 0.00001f); + + ++imu_count_; + publishIMU(data, imu_count_, stamp); + NODELET_DEBUG_STREAM( + "Imu count: " << imu_count_ << ", frame_id: " << data.imu->frame_id + << ", timestamp: " << data.imu->timestamp + << ", accel_x: " << data.imu->accel[0] + << ", accel_y: " << data.imu->accel[1] + << ", accel_z: " << data.imu->accel[2] + << ", gyro_x: " << data.imu->gyro[0] + << ", gyro_y: " << data.imu->gyro[1] + << ", gyro_z: " << data.imu->gyro[2] + << ", temperature: " << data.imu->temperature); + }); + + time_beg_ = ros::Time::now().toSec(); + device_->Start(Source::ALL); + } + + void publishLeft( + const device::StreamData &data, std::uint32_t seq, ros::Time stamp) { + std_msgs::Header header; + header.seq = seq; + header.stamp = stamp; + header.frame_id = left_frame_id_; + cv::Mat img( + data.frame->height(), data.frame->width(), CV_8UC1, data.frame->data()); + pub_left_.publish(cv_bridge::CvImage(header, enc::MONO8, img).toImageMsg()); + } + + void publishRight( + const device::StreamData &data, std::uint32_t seq, ros::Time stamp) { + std_msgs::Header header; + header.seq = seq; + header.stamp = stamp; + header.frame_id = right_frame_id_; + cv::Mat img( + data.frame->height(), data.frame->width(), CV_8UC1, data.frame->data()); + pub_right_.publish( + cv_bridge::CvImage(header, enc::MONO8, img).toImageMsg()); + } + + void publishIMU( + const device::MotionData &data, std::uint32_t seq, ros::Time stamp) { + sensor_msgs::Imu msg; + + msg.header.seq = seq; + msg.header.stamp = stamp; + msg.header.frame_id = imu_frame_id_; + + // acceleration should be in m/s^2 (not in g's) + msg.linear_acceleration.x = data.imu->accel[0] * 9.8; + msg.linear_acceleration.y = data.imu->accel[1] * 9.8; + msg.linear_acceleration.z = data.imu->accel[2] * 9.8; + + msg.linear_acceleration_covariance[0] = 0; + msg.linear_acceleration_covariance[1] = 0; + msg.linear_acceleration_covariance[2] = 0; + + msg.linear_acceleration_covariance[3] = 0; + msg.linear_acceleration_covariance[4] = 0; + msg.linear_acceleration_covariance[5] = 0; + + msg.linear_acceleration_covariance[6] = 0; + msg.linear_acceleration_covariance[7] = 0; + msg.linear_acceleration_covariance[8] = 0; + + // velocity should be in rad/sec + msg.angular_velocity.x = data.imu->gyro[0] / 57.2956; + msg.angular_velocity.y = data.imu->gyro[1] / 57.2956; + msg.angular_velocity.z = data.imu->gyro[2] / 57.2956; + + msg.angular_velocity_covariance[0] = 0; + msg.angular_velocity_covariance[1] = 0; + msg.angular_velocity_covariance[2] = 0; + + msg.angular_velocity_covariance[3] = 0; + msg.angular_velocity_covariance[4] = 0; + msg.angular_velocity_covariance[5] = 0; + + msg.angular_velocity_covariance[6] = 0; + msg.angular_velocity_covariance[7] = 0; + msg.angular_velocity_covariance[8] = 0; + + pub_imu_.publish(msg); + } + + private: + void initDevice() { + NODELET_INFO_STREAM("Detecting MYNT EYE devices"); + + Context context; + auto &&devices = context.devices(); + + size_t n = devices.size(); + NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :("); + + NODELET_INFO_STREAM("MYNT EYE devices: "); + for (size_t i = 0; i < n; i++) { + auto &&device = devices[i]; + auto &&name = device->GetInfo(Info::DEVICE_NAME); + NODELET_INFO_STREAM(" index: " << i << ", name: " << name); + } + + std::shared_ptr device = nullptr; + if (n <= 1) { + device = devices[0]; + NODELET_INFO_STREAM("Only one MYNT EYE device, select index: 0"); + } else { + while (true) { + size_t i; + NODELET_INFO_STREAM( + "There are " << n << " MYNT EYE devices, select index: "); + std::cin >> i; + if (i >= n) { + NODELET_WARN_STREAM("Index out of range :("); + continue; + } + device = devices[i]; + break; + } + } + + device_ = device; + } + + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + + image_transport::Publisher pub_left_; + image_transport::Publisher pub_right_; + + ros::Publisher pub_imu_; + + tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_; + + // node params + + std::string base_frame_id_; + std::string left_frame_id_; + std::string right_frame_id_; + std::string imu_frame_id_; + + // device + + std::shared_ptr device_; + + double time_beg_ = -1; + std::size_t left_count_ = 0; + std::size_t right_count_ = 0; + std::size_t imu_count_ = 0; +}; + +MYNTEYE_END_NAMESPACE + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(mynteye::ROSWrapperNodelet, nodelet::Nodelet);