diff --git a/orbbec_camera/launch/gemini_intra_process_demo_launch.py b/orbbec_camera/launch/gemini_intra_process_demo_launch.py index e205bf50..a1bb6796 100644 --- a/orbbec_camera/launch/gemini_intra_process_demo_launch.py +++ b/orbbec_camera/launch/gemini_intra_process_demo_launch.py @@ -52,18 +52,18 @@ def load_parameters(context, args): def generate_launch_description(): args = [ DeclareLaunchArgument('camera_name', default_value='camera'), - DeclareLaunchArgument('depth_registration', default_value='false'), + DeclareLaunchArgument('depth_registration', default_value='true'), DeclareLaunchArgument('serial_number', default_value=''), DeclareLaunchArgument('usb_port', default_value=''), DeclareLaunchArgument('device_num', default_value='1'), DeclareLaunchArgument('point_cloud_qos', default_value='default'), - DeclareLaunchArgument('enable_point_cloud', default_value='true'), - DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), + DeclareLaunchArgument('enable_point_cloud', default_value='false'), + DeclareLaunchArgument('enable_colored_point_cloud', default_value='true'), DeclareLaunchArgument('connection_delay', default_value='10'), - DeclareLaunchArgument('color_width', default_value='0'), - DeclareLaunchArgument('color_height', default_value='0'), - DeclareLaunchArgument('color_fps', default_value='0'), - DeclareLaunchArgument('color_format', default_value='ANY'), + DeclareLaunchArgument('color_width', default_value='1280'), + DeclareLaunchArgument('color_height', default_value='800'), + DeclareLaunchArgument('color_fps', default_value='30'), + DeclareLaunchArgument('color_format', default_value='YUYV'), DeclareLaunchArgument('enable_color', default_value='true'), DeclareLaunchArgument('color_qos', default_value='default'), DeclareLaunchArgument('color_camera_info_qos', default_value='default'), @@ -72,10 +72,10 @@ def generate_launch_description(): DeclareLaunchArgument('color_gain', default_value='-1'), DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'), DeclareLaunchArgument('color_white_balance', default_value='-1'), - DeclareLaunchArgument('depth_width', default_value='0'), - DeclareLaunchArgument('depth_height', default_value='0'), - DeclareLaunchArgument('depth_fps', default_value='0'), - DeclareLaunchArgument('depth_format', default_value='ANY'), + DeclareLaunchArgument('depth_width', default_value='1280'), + DeclareLaunchArgument('depth_height', default_value='800'), + DeclareLaunchArgument('depth_fps', default_value='30'), + DeclareLaunchArgument('depth_format', default_value='Y16'), DeclareLaunchArgument('enable_depth', default_value='true'), DeclareLaunchArgument('depth_qos', default_value='default'), DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), @@ -168,7 +168,7 @@ def generate_launch_description(): DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('enable_heartbeat', default_value='false'), DeclareLaunchArgument('topic_type', default_value='points'), - DeclareLaunchArgument('topic_name', default_value='/camera/depth/points'), + DeclareLaunchArgument('topic_name', default_value='/camera/depth_registered/points'), DeclareLaunchArgument('use_intra_process_comms', default_value='true'), ] diff --git a/orbbec_camera/tools/frame_latency.cpp b/orbbec_camera/tools/frame_latency.cpp index 1ce9456c..3ce86c93 100644 --- a/orbbec_camera/tools/frame_latency.cpp +++ b/orbbec_camera/tools/frame_latency.cpp @@ -33,6 +33,7 @@ #include #include #include +#include using namespace std::chrono_literals; #include "frame_latency.hpp" @@ -50,11 +51,18 @@ template void FrameLatencyNode::createListener(const std::string& topic_name, const rmw_qos_profile_t qos_profile) { RCLCPP_INFO_STREAM(logger_, "createListener"); + using namespace std::chrono_literals; + timer_ = this->create_wall_timer(1s, [this, topic_name=topic_name]() { + // print fps + RCLCPP_INFO_STREAM(logger_, "topic: " << topic_name << " fps: " << frame_count_ / 1.0); + frame_count_ = 0; + }); sub_ = this->create_subscription( topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile), [&, this](const std::shared_ptr msg) { rclcpp::Time curr_time = this->get_clock()->now(); auto latency = (curr_time - msg->header.stamp).seconds(); + frame_count_++; RCLCPP_INFO_STREAM_THROTTLE(logger_, *this->get_clock(), 1000.0, "Got msg with " << msg->header.frame_id << " frame id at address 0x" diff --git a/orbbec_camera/tools/frame_latency.hpp b/orbbec_camera/tools/frame_latency.hpp index bf43a295..d576f52a 100644 --- a/orbbec_camera/tools/frame_latency.hpp +++ b/orbbec_camera/tools/frame_latency.hpp @@ -49,5 +49,7 @@ class FrameLatencyNode : public rclcpp::Node { std::shared_ptr sub_ = nullptr; rclcpp::Logger logger_; + rclcpp::TimerBase::SharedPtr timer_; + size_t frame_count_ = 0; }; } // namespace orbbec_camera