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(
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.

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(
int argc, char *argv[], const std::shared_ptr<Device> &device) {
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);
}

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

@ -35,26 +35,27 @@
<!-- stream toggles -->
<!-- Resolution -->
<arg name="res_752x480" default="0" />
<arg name="res_1280x400" default="1" />
<arg name="res_2560x800" default="2" />
<!-- Request index -->
<!-- FrameRate -->
<arg name="rate_10_fps" default="10" />
<arg name="rate_20_fps" default="20" />
<arg name="rate_25_fps" default="25" />
<arg name="rate_30_fps" default="30" />
<arg name="rate_60_fps" default="60" />
<!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 10 -->
<arg name="index_s210a_0" default="0" />
<!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 20 -->
<arg name="index_s210a_1" default="1" />
<!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 30 -->
<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 -->
<arg name="grey" default="1497715271" />
<arg name="yuyv" default="1497715271" />
<arg name="bgr888" default="861030210" />
<!-- MYNTEYE-S1030, Reslution: 752x480, Format: YUYV, Fps: 25 -->
<arg name="index_s1030_0" default="0" />
<arg name="resolution" default="$(arg res_1280x400)" />
<arg name="framerate" default="$(arg rate_30_fps)" />
<arg name="format" default="$(arg bgr888)" />
<arg name="request_index" default="$(arg index_s210a_2)" />
<arg name="enable_left_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_depth" value="$(arg enable_depth)" />
<!-- init params -->
<param name="resolution" value="$(arg resolution)" />
<param name="framerate" value="$(arg framerate)" />
<param name="format" value="$(arg format)" />
<!-- stream request index -->
<param name="request_index" value="$(arg request_index)" />
<!-- 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 {
nh_ = getMTNodeHandle();
private_nh_ = getMTPrivateNodeHandle();
int resolution = 0;
int format = 0;
int framerate = 20;
private_nh_.getParam("resolution", resolution);
private_nh_.getParam("framerate", framerate);
private_nh_.getParam("format", format);
frame_rate_ = framerate;
int request_index = 0;
private_nh_.getParam("request_index", request_index);
initDevice(resolution, format, framerate);
initDevice(request_index);
NODELET_FATAL_COND(api_ == nullptr, "No MYNT EYE device selected :(");
// node params
@ -255,7 +250,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
NODELET_INFO_STREAM("Advertized service " << DEVICE_INFO_SERVICE);
publishStaticTransforms();
ros::Rate loop_rate(frame_rate_);
ros::Rate loop_rate(60);
while (private_nh_.ok()) {
publishTopics();
loop_rate.sleep();
@ -724,7 +719,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
private:
void initDevice(int resolution, int format, int framerate) {
void initDevice(int request_index) {
NODELET_INFO_STREAM("Detecting MYNT EYE devices");
Context context;
@ -759,9 +754,23 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
}
api_ = API::Create(device, static_cast<Resolution>(resolution));
api_->SetStreamRequest(static_cast<Format>(format),
static_cast<FrameRate>(framerate));
api_ = API::Create(device);
auto &&requests = device->GetStreamRequests();
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();
}