Update mynteye.launch

This commit is contained in:
kalman 2018-12-05 20:43:50 +08:00
parent e832fa66b9
commit 04f32f21ba
3 changed files with 47 additions and 34 deletions

View File

@ -280,15 +280,15 @@ enum class Resolution : std::uint8_t {
*/ */
enum class FrameRate : std::uint8_t { enum class FrameRate : std::uint8_t {
/** 10 fps */ /** 10 fps */
RATE_10_FPS, RATE_10_FPS = 10,
/** 20 fps */ /** 20 fps */
RATE_20_FPS, RATE_20_FPS = 20,
/** 20 fps */ /** 20 fps */
RATE_25_FPS, RATE_25_FPS = 25,
/** 30 fps */ /** 30 fps */
RATE_30_FPS, RATE_30_FPS = 30,
/** 60 fps */ /** 60 fps */
RATE_60_FPS, RATE_60_FPS = 60,
/** Last guard */ /** Last guard */
LAST LAST
}; };
@ -372,6 +372,8 @@ struct MYNTEYE_API StreamRequest {
StreamRequest(Resolution res, Format format, FrameRate rate) StreamRequest(Resolution res, Format format, FrameRate rate)
: format(format) { : format(format) {
fps = static_cast<uint16_t>(rate);
switch (res) { switch (res) {
case Resolution::RES_752x480: case Resolution::RES_752x480:
width = 480, height = 752; width = 480, height = 752;
@ -386,27 +388,6 @@ struct MYNTEYE_API StreamRequest {
width = 480, height = 752; width = 480, height = 752;
break; break;
} }
switch (rate) {
case FrameRate::RATE_10_FPS:
fps = 10;
break;
case FrameRate::RATE_20_FPS:
fps = 20;
break;
case FrameRate::RATE_25_FPS:
fps = 25;
break;
case FrameRate::RATE_30_FPS:
fps = 30;
break;
case FrameRate::RATE_60_FPS:
fps = 60;
break;
default:
fps = 25;
break;
}
} }
bool operator==(const StreamRequest &other) const { bool operator==(const StreamRequest &other) const {

View File

@ -35,6 +35,27 @@
<!-- stream toggles --> <!-- stream toggles -->
<!-- Resolution -->
<arg name="res_752x480" default="0" />
<arg name="res_1280x400" default="1" />
<arg name="res_2560x800" default="2" />
<!-- 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" />
<!-- Format -->
<arg name="grey" default="1497715271" />
<arg name="yuyv" default="1497715271" />
<arg name="bgr888" default="861030210" />
<arg name="resolution" default="$(arg res_1280x400)" />
<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" />
<arg name="enable_disparity" default="false" /> <arg name="enable_disparity" default="false" />
@ -130,6 +151,11 @@
<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 -->
<param name="resolution" value="$(arg resolution)" />
<param name="framerate" value="$(arg framerate)" />
<param name="format" value="$(arg format)" />
<!-- device options --> <!-- device options -->
<param name="brightness" value="$(arg brightness)" /> <param name="brightness" value="$(arg brightness)" />

View File

@ -103,11 +103,18 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
void onInit() override { void onInit() override {
initDevice();
NODELET_FATAL_COND(api_ == nullptr, "No MYNT EYE device selected :(");
nh_ = getMTNodeHandle(); nh_ = getMTNodeHandle();
private_nh_ = getMTPrivateNodeHandle(); 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;
initDevice(resolution, format, framerate);
NODELET_FATAL_COND(api_ == nullptr, "No MYNT EYE device selected :(");
// node params // node params
@ -186,7 +193,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
NODELET_INFO_STREAM(it->first << ": " << api_->GetOptionValue(it->first)); NODELET_INFO_STREAM(it->first << ": " << api_->GetOptionValue(it->first));
} }
frame_rate_ = api_->GetOptionValue(Option::FRAME_RATE);
// publishers // publishers
@ -717,7 +723,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
private: private:
void initDevice() { void initDevice(int resolution, int format, int framerate) {
NODELET_INFO_STREAM("Detecting MYNT EYE devices"); NODELET_INFO_STREAM("Detecting MYNT EYE devices");
Context context; Context context;
@ -752,8 +758,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
} }
api_ = API::Create(device, Resolution::RES_1280x400); api_ = API::Create(device, static_cast<Resolution>(resolution));
api_->SetStreamRequest(Format::BGR888, FrameRate::RATE_20_FPS); api_->SetStreamRequest(static_cast<Format>(format),
static_cast<FrameRate>(framerate));
computeRectTransforms(); computeRectTransforms();
} }
@ -765,7 +772,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
auto &&ex_right_to_left = api_->GetExtrinsics(Stream::RIGHT, Stream::LEFT); auto &&ex_right_to_left = api_->GetExtrinsics(Stream::RIGHT, Stream::LEFT);
cv::Size size{in_left.width, in_left.height}; cv::Size size{in_left.width, in_left.height};
cv::Mat M1 = cv::Mat M1 =
(cv::Mat_<double>(3, 3) << in_left.fx, 0, in_left.cx, 0, in_left.fy, (cv::Mat_<double>(3, 3) << in_left.fx, 0, in_left.cx, 0, in_left.fy,
in_left.cy, 0, 0, 1); in_left.cy, 0, 0, 1);