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

fix(*): vinsfusion framerate cut feature.

Browse Source
This commit is contained in:
TinyO 2019-08-28 17:09:44 +08:00
parent a04da7dafa
commit 4afa3fd311
3 changed files with 40 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
wrappers/ros/src/mynt_eye_ros_wrapper/launch/slam/vins_fusion.launch
View File

@ -77,6 +77,8 @@
<rosparam file="$(find mynt_eye_ros_wrapper)/config/slam/vins_fusion.yaml" command="load" />
<param name="gravity" value="$(arg gravity)" />
<param name="ros_output_framerate_cut" value="2" />
</node>
<!-- disable compressed depth plugin for image topics -->

20
wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz
View File

@ -58,7 +58,7 @@ Visualization Manager:
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: false
Enabled: true
Image Topic: /mynteye/left/image_raw
Max Value: 1
Median window: 5
@ -68,7 +68,7 @@ Visualization Manager:
Queue Size: 1
Transport Hint: raw
Unreliable: false
Value: false
Value: true
- Class: rviz/Image
Enabled: false
Image Topic: /mynteye/right/image_raw
@ -114,7 +114,7 @@ Visualization Manager:
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: true
Enabled: false
Image Topic: /mynteye/disparity/image_raw
Max Value: 1
Median window: 5
@ -124,7 +124,7 @@ Visualization Manager:
Queue Size: 1
Transport Hint: raw
Unreliable: false
Value: true
Value: false
- Class: rviz/Image
Enabled: true
Image Topic: /mynteye/disparity/image_norm
@ -154,7 +154,7 @@ Visualization Manager:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.98300004
Max Value: 2.1099999
Min Value: 0
Value: true
Axis: X
@ -223,7 +223,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 2.80799317
Distance: 2.23245931
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
@ -238,10 +238,10 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.520203829
Pitch: 0.180204123
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.3935833
Yaw: 3.14358234
Saved: ~
Window Geometry:
Depth:
@ -259,7 +259,7 @@ Window Geometry:
collapsed: false
LeftRect:
collapsed: false
QMainWindow State: 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
QMainWindow State: 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
Right:
collapsed: false
RightRect:
@ -272,6 +272,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 1855
Width: 2309
X: 65
Y: 24

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

@ -58,6 +58,9 @@ inline double compute_time(const double end, const double start) {
class ROSWrapperNodelet : public nodelet::Nodelet {
public:
ROSWrapperNodelet() {
skip_tag = -1;
skip_tmp_left_tag = 0;
skip_tmp_right_tag = 0;
unit_hard_time *= 10;
}
@ -408,6 +411,12 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
}
int ros_output_framerate = -1;
private_nh_.getParamCached("ros_output_framerate_cut", ros_output_framerate);
if (ros_output_framerate > 0 && ros_output_framerate < 7) {
skip_tag = ros_output_framerate;
}
// services
const std::string DEVICE_INFO_SERVICE = "get_info";
@ -672,6 +681,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::LEFT);
if (skip_tag > 0) {
if (skip_tmp_left_tag == 0) {
skip_tmp_left_tag = skip_tag;
} else {
skip_tmp_left_tag--;
return;
}
}
publishCamera(Stream::LEFT, data, left_count_, stamp);
publishMono(Stream::LEFT, data, left_count_, stamp);
NODELET_DEBUG_STREAM(
@ -696,6 +713,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
// ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
ros::Time stamp = checkUpTimeStamp(
data.img->timestamp, Stream::RIGHT);
if (skip_tag > 0) {
if (skip_tmp_right_tag == 0) {
skip_tmp_right_tag = skip_tag;
} else {
skip_tmp_right_tag--;
return;
}
}
publishCamera(Stream::RIGHT, data, right_count_, stamp);
publishMono(Stream::RIGHT, data, right_count_, stamp);
NODELET_DEBUG_STREAM(
@ -1603,6 +1628,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
int frame_rate_;
bool is_intrinsics_enable_;
std::vector<ImuData> imu_align_;
int skip_tag;
int skip_tmp_left_tag;
int skip_tmp_right_tag;
std::uint64_t unit_hard_time = std::numeric_limits<std::uint32_t>::max();
};