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

fix(ros): fix init device

Browse Source
This commit is contained in:
kalman 2018-12-20 20:45:11 +08:00
parent 09e14293ed
commit 1652d976bf
4 changed files with 52 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/mynteye/api/api.h
View File

@ -114,6 +114,12 @@ class MYNTEYE_API API {
*/ */
static std::shared_ptr<API> Create( static std::shared_ptr<API> Create(
int argc, char *argv[], const std::shared_ptr<Device> &device); int argc, char *argv[], const std::shared_ptr<Device> &device);
/**
* Create the API instance.
* @param device the selected device.
* @return the API instance.
*/
static std::shared_ptr<API> Create(const std::shared_ptr<Device> &device);
/** /**
* Get the model. * Get the model.

4
src/mynteye/api/api.cc
View File

@ -227,6 +227,10 @@ std::shared_ptr<API> API::Create(int argc, char *argv[]) {
std::shared_ptr<API> API::Create( std::shared_ptr<API> API::Create(
int argc, char *argv[], const std::shared_ptr<Device> &device) { int argc, char *argv[], const std::shared_ptr<Device> &device) {
static glog_init _(argc, argv); static glog_init _(argc, argv);
return Create(device);
}
std::shared_ptr<API> API::Create(const std::shared_ptr<Device> &device) {
return std::make_shared<API>(device); return std::make_shared<API>(device);
} }

41
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
View File

@ -35,26 +35,27 @@
<!-- stream toggles --> <!-- stream toggles -->
<!-- Resolution --> <!-- Request index -->
<arg name="res_752x480" default="0" />
<arg name="res_1280x400" default="1" />
<arg name="res_2560x800" default="2" />
<!-- FrameRate --> <!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 10 -->
<arg name="rate_10_fps" default="10" /> <arg name="index_s210a_0" default="0" />
<arg name="rate_20_fps" default="20" /> <!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 20 -->
<arg name="rate_25_fps" default="25" /> <arg name="index_s210a_1" default="1" />
<arg name="rate_30_fps" default="30" /> <!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 30 -->
<arg name="rate_60_fps" default="60" /> <arg name="index_s210a_2" default="2" />
<!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 60 -->
<arg name="index_s210a_3" default="3" />
<!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 10 -->
<arg name="index_s210a_4" default="4" />
<!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 20 -->
<arg name="index_s210a_5" default="5" />
<!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 30 -->
<arg name="index_s210a_6" default="6" />
<!-- Format --> <!-- MYNTEYE-S1030, Reslution: 752x480, Format: YUYV, Fps: 25 -->
<arg name="grey" default="1497715271" /> <arg name="index_s1030_0" default="0" />
<arg name="yuyv" default="1497715271" />
<arg name="bgr888" default="861030210" />
<arg name="resolution" default="$(arg res_1280x400)" /> <arg name="request_index" default="$(arg index_s210a_2)" />
<arg name="framerate" default="$(arg rate_30_fps)" />
<arg name="format" default="$(arg bgr888)" />
<arg name="enable_left_rect" default="false" /> <arg name="enable_left_rect" default="false" />
<arg name="enable_right_rect" default="false" /> <arg name="enable_right_rect" default="false" />
@ -151,10 +152,8 @@
<param name="enable_points" value="$(arg enable_points)" /> <param name="enable_points" value="$(arg enable_points)" />
<param name="enable_depth" value="$(arg enable_depth)" /> <param name="enable_depth" value="$(arg enable_depth)" />
<!-- init params --> <!-- stream request index -->
<param name="resolution" value="$(arg resolution)" /> <param name="request_index" value="$(arg request_index)" />
<param name="framerate" value="$(arg framerate)" />
<param name="format" value="$(arg format)" />
<!-- device options --> <!-- device options -->

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

@ -105,15 +105,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
void onInit() override { void onInit() override {
nh_ = getMTNodeHandle(); nh_ = getMTNodeHandle();
private_nh_ = getMTPrivateNodeHandle(); private_nh_ = getMTPrivateNodeHandle();
int resolution = 0; int request_index = 0;
int format = 0; private_nh_.getParam("request_index", request_index);
int framerate = 20;
private_nh_.getParam("resolution", resolution);
private_nh_.getParam("framerate", framerate);
private_nh_.getParam("format", format);
frame_rate_ = framerate;
initDevice(resolution, format, framerate); initDevice(request_index);
NODELET_FATAL_COND(api_ == nullptr, "No MYNT EYE device selected :("); NODELET_FATAL_COND(api_ == nullptr, "No MYNT EYE device selected :(");
// node params // node params
@ -255,7 +250,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
NODELET_INFO_STREAM("Advertized service " << DEVICE_INFO_SERVICE); NODELET_INFO_STREAM("Advertized service " << DEVICE_INFO_SERVICE);
publishStaticTransforms(); publishStaticTransforms();
ros::Rate loop_rate(frame_rate_); ros::Rate loop_rate(60);
while (private_nh_.ok()) { while (private_nh_.ok()) {
publishTopics(); publishTopics();
loop_rate.sleep(); loop_rate.sleep();
@ -724,7 +719,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
private: private:
void initDevice(int resolution, int format, int framerate) { void initDevice(int request_index) {
NODELET_INFO_STREAM("Detecting MYNT EYE devices"); NODELET_INFO_STREAM("Detecting MYNT EYE devices");
Context context; Context context;
@ -759,9 +754,23 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
} }
api_ = API::Create(device, static_cast<Resolution>(resolution)); api_ = API::Create(device);
api_->SetStreamRequest(static_cast<Format>(format), auto &&requests = device->GetStreamRequests();
static_cast<FrameRate>(framerate)); std::size_t m = requests.size();
NODELET_FATAL_COND(m <= 0, "No MYNT EYE devices :(");
if (m <= 1) {
NODELET_INFO_STREAM("Only one stream request, select index: 0");
api_->ConfigStreamRequest(requests[0]);
} else {
if (request_index >= m) {
NODELET_WARN_STREAM("Resquest_index out of range");
api_->ConfigStreamRequest(requests[0]);
} else {
NODELET_WARN_STREAM("request_index: " << request_index);
api_->ConfigStreamRequest(requests[request_index]);
}
}
computeRectTransforms(); computeRectTransforms();
} }