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

Add ros wrapper

Browse Source
This commit is contained in:
John Zhao 2018-04-14 14:56:05 +08:00
parent fe0c0bd8a3
commit 122fff1722
9 changed files with 703 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
wrappers/README.md
View File

@ -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
```

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

@ -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}
)

8
wrappers/ros/src/mynt_eye_ros_wrapper/launch/display.launch Normal file
View File

@ -0,0 +1,8 @@
<?xml version="1.0"?>
<launch>
<include file="$(find mynt_eye_ros_wrapper)/launch/mynteye.launch" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mynt_eye_ros_wrapper)/rviz/mynteye.rviz" output="screen" />
</launch>

96
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch Normal file
View File

@ -0,0 +1,96 @@
<?xml version="1.0"?>
<launch>
<arg name="mynteye" default="mynteye" />
<!-- node params -->
<arg name="left_topic" default="left" />
<arg name="right_topic" default="right" />
<arg name="imu_topic" default="imu" />
<arg name="base_frame_id" default="$(arg mynteye)_link" />
<arg name="left_frame_id" default="$(arg mynteye)_left_frame" />
<arg name="right_frame_id" default="$(arg mynteye)_right_frame" />
<arg name="imu_frame_id" default="$(arg mynteye)_imu_frame" />
<!-- device options, -1 will not set the value -->
<!-- gain range: [0,48]-->
<arg name="gain" default="-1" />
<!-- <arg name="gain" default="24" /> -->
<!-- brightness range: [0,240]-->
<arg name="brightness" default="-1" />
<!-- <arg name="brightness" default="120" /> -->
<!-- contrast range: [0,255]-->
<arg name="contrast" default="-1" />
<!-- <arg name="contrast" default="127" /> -->
<!-- frame_rate range: [10,60]-->
<arg name="frame_rate" default="-1" />
<!-- <arg name="frame_rate" default="25" /> -->
<!-- imu_frequency range: [200,500]-->
<arg name="imu_frequency" default="-1" />
<!-- <arg name="imu_frequency" default="200" /> -->
<!-- exposure_mode, 0: auto-exposure, 1: manual-exposure -->
<arg name="exposure_mode" default="-1" />
<!-- <arg name="exposure_mode" default="0" /> -->
<!-- max_gain range: [0,48]-->
<arg name="max_gain" default="-1" />
<!-- <arg name="max_gain" default="48" /> -->
<!-- max_exposure_time range: [0,240]-->
<arg name="max_exposure_time" default="-1" />
<!-- <arg name="max_exposure_time" default="240" /> -->
<!-- desired_brightness range: [0,255]-->
<arg name="desired_brightness" default="-1" />
<!-- <arg name="desired_brightness" default="192" /> -->
<!-- ir_control range: [0,160]-->
<arg name="ir_control" default="-1" />
<!-- <arg name="ir_control" default="0" /> -->
<!-- hdr_mode, 0: 10-bit, 1: 12-bit -->
<arg name="hdr_mode" default="-1" />
<!-- <arg name="hdr_mode" default="0" /> -->
<!-- Push down all topics/nodelets into "mynteye" namespace -->
<group ns="$(arg mynteye)">
<!-- mynteye_wrapper_node -->
<node name="mynteye_wrapper_node" pkg="mynt_eye_ros_wrapper" type="mynteye_wrapper_node" output="screen">
<!-- node params -->
<param name="left_topic" value="$(arg left_topic)" />
<param name="right_topic" value="$(arg right_topic)" />
<param name="imu_topic" value="$(arg imu_topic)" />
<param name="base_frame_id" value="$(arg base_frame_id)" />
<param name="left_frame_id" value="$(arg left_frame_id)" />
<param name="right_frame_id" value="$(arg right_frame_id)" />
<param name="imu_frame_id" value="$(arg imu_frame_id)" />
<!-- device options -->
<param name="gain" value="$(arg gain)" />
<param name="brightness" value="$(arg brightness)" />
<param name="contrast" value="$(arg contrast)" />
<param name="frame_rate" value="$(arg frame_rate)" />
<param name="imu_frequency" value="$(arg imu_frequency)" />
<param name="exposure_mode" value="$(arg exposure_mode)" />
<param name="max_gain" value="$(arg max_gain)" />
<param name="max_exposure_time" value="$(arg max_exposure_time)" />
<param name="desired_brightness" value="$(arg desired_brightness)" />
<param name="ir_control" value="$(arg ir_control)" />
<param name="hdr_mode" value="$(arg hdr_mode)" />
</node>
</group> <!-- mynteye -->
</launch>

7
wrappers/ros/src/mynt_eye_ros_wrapper/nodelet_plugins.xml Normal file
View File

@ -0,0 +1,7 @@
<library path="lib/libmynteye_wrapper">
<class name="mynteye/ROSWrapperNodelet"
type="mynteye::ROSWrapperNodelet"
base_class_type="nodelet::Nodelet">
<description>This is the nodelet of ROS interface for MYNT EYE camera.</description>
</class>
</library>

7
wrappers/ros/src/mynt_eye_ros_wrapper/package.xml
View File

@ -14,28 +14,35 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<build_export_depend>cv_bridge</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>image_transport</build_export_depend>
<build_export_depend>nodelet</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>tf</build_export_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>image_transport</exec_depend>
<exec_depend>nodelet</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
</export>
</package>

150
wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz Normal file
View File

@ -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: <Fixed 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: <Fixed 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

22
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_node.cc Normal file
View File

@ -0,0 +1,22 @@
#include <nodelet/loader.h>
#include <ros/ros.h>
#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;
}

354
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc Normal file
View File

@ -0,0 +1,354 @@
#include <nodelet/nodelet.h> // NOLINT
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/image_encodings.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <glog/logging.h>
#include <map>
#include <string>
#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<std::string, Option> 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<sensor_msgs::Imu>(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> 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> 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 <pluginlib/class_list_macros.h> // NOLINT
PLUGINLIB_EXPORT_CLASS(mynteye::ROSWrapperNodelet, nodelet::Nodelet);