diff --git a/samples/util_cv_painter.cc b/samples/util_cv_painter.cc index ac14c4d..ab694c9 100644 --- a/samples/util_cv_painter.cc +++ b/samples/util_cv_painter.cc @@ -116,18 +116,48 @@ cv::Rect CVPainter::DrawImuData( int sign = 1; if (gravity == BOTTOM_LEFT || gravity == BOTTOM_RIGHT) sign = -1; - - Clear(ss) << "stamp: " << data.timestamp + static auto fmt_time = NewFormat(0, 2); + Clear(ss) << "stamp: " << fmt_time << (0.001f * data.timestamp) << ", temp: " << fmt_temp << data.temperature; cv::Rect rect_i = DrawText(img, ss.str(), gravity, 5); - Clear(ss) << "accel(x,y,z): " << fmt_imu << data.accel[0] << "," << fmt_imu - << data.accel[1] << "," << fmt_imu << data.accel[2]; + static double accel1_s = 0.0; + static double accel2_s = 0.0; + static double accel3_s = 0.0; + + if (data.accel[0] > 0.000001 || + data.accel[1] > 0.000001 || + data.accel[2] > 0.000001 || + data.accel[0] < -0.000001 || + data.accel[1] < -0.000001 || + data.accel[2] < -0.000001) { + accel1_s = data.accel[0]; + accel2_s = data.accel[1]; + accel3_s = data.accel[2]; + } + + Clear(ss) << "accel(x,y,z): " << fmt_imu << accel1_s << "," << fmt_imu + << accel2_s << "," << fmt_imu << accel3_s; cv::Rect rect_a = DrawText(img, ss.str(), gravity, 5, 0, sign * (5 + rect_i.height)); - Clear(ss) << "gyro(x,y,z): " << fmt_imu << data.gyro[0] << "," << fmt_imu - << data.gyro[1] << "," << fmt_imu << data.gyro[2]; + static double gyro1_s = 0.0; + static double gyro2_s = 0.0; + static double gyro3_s = 0.0; + + if (data.gyro[0] > 0.01 || + data.gyro[1] > 0.01 || + data.gyro[2] > 0.01 || + data.gyro[0] < -0.01 || + data.gyro[1] < -0.01 || + data.gyro[2] < -0.01 ) { + gyro1_s = data.gyro[0]; + gyro2_s = data.gyro[1]; + gyro3_s = data.gyro[2]; + } + + Clear(ss) << "gyro(x,y,z): " << fmt_imu << gyro1_s << "," << fmt_imu + << gyro2_s << "," << fmt_imu << gyro3_s; cv::Rect rect_g = DrawText( img, ss.str(), gravity, 5, 0, sign * (10 + rect_i.height + rect_a.height)); diff --git a/src/mynteye/device/utils.cc b/src/mynteye/device/utils.cc index bff87b7..7acb444 100644 --- a/src/mynteye/device/utils.cc +++ b/src/mynteye/device/utils.cc @@ -89,10 +89,16 @@ MYNTEYE_NAMESPACE::StreamRequest select_request( *ok = false; return {}; } + auto requests_display = requests; + if (Model::STANDARD == device->GetModel()) { + for (std::size_t i = 0; i < n; i++) { + requests_display[i].fps = 25; + } + } LOG(INFO) << "MYNT EYE requests:"; for (std::size_t i = 0; i < n; i++) { - auto &&request = requests[i]; + auto &&request = requests_display[i]; LOG(INFO) << " index: " << i << ", request: " << request; } diff --git a/src/mynteye/types.cc b/src/mynteye/types.cc index 1d82b02..67dea60 100644 --- a/src/mynteye/types.cc +++ b/src/mynteye/types.cc @@ -190,13 +190,8 @@ std::size_t bytes_per_pixel(const Format &value) { } std::ostream &operator<<(std::ostream &os, const StreamRequest &request) { - if (request.fps == 0) { - return os << "width: " << request.width << ", height: " << request.height - << ", format: " << request.format; - } else { - return os << "width: " << request.width << ", height: " << request.height + return os << "width: " << request.width << ", height: " << request.height << ", format: " << request.format << ", fps: " << request.fps; - } } const char *to_string(const CalibrationModel &model) { diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/slam/vins_fusion.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/slam/vins_fusion.launch index 1a84ed4..8d52a3b 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/slam/vins_fusion.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/slam/vins_fusion.launch @@ -77,6 +77,8 @@ + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz index 7b09bdd..9779741 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz @@ -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: 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 diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index d5a656c..29e4283 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -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 imu_align_; + int skip_tag; + int skip_tmp_left_tag; + int skip_tmp_right_tag; std::uint64_t unit_hard_time = std::numeric_limits::max(); };