From b07527e2080dcbb6693bd2195368655a7c9d2792 Mon Sep 17 00:00:00 2001 From: Huang77 <2366619700@qq.com> Date: Thu, 13 Aug 2026 11:01:21 +0800 Subject: [PATCH] update --- .../launch/bcr_bot_gazebo_spawn.launch.py | 2 +- src/bcr_bot/launch/bcr_bot_gz_spawn.launch.py | 4 ++-- src/bcr_bot/launch/bcr_bot_ign_spawn.launch.py | 2 +- src/bcr_bot/urdf/bcr_bot.xacro | 2 +- src/ros_viz_adapter/README.md | 8 ++++---- src/ros_viz_adapter/src/adapter.cpp | 18 ++++++++++++++---- 6 files changed, 23 insertions(+), 13 deletions(-) diff --git a/src/bcr_bot/launch/bcr_bot_gazebo_spawn.launch.py b/src/bcr_bot/launch/bcr_bot_gazebo_spawn.launch.py index 9cc11f5..eb61507 100644 --- a/src/bcr_bot/launch/bcr_bot_gazebo_spawn.launch.py +++ b/src/bcr_bot/launch/bcr_bot_gazebo_spawn.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): position_y = LaunchConfiguration("position_y") orientation_yaw = LaunchConfiguration("orientation_yaw") camera_enabled = LaunchConfiguration("camera_enabled", default=True) - stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False) + stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=True) two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True) odometry_source = LaunchConfiguration("odometry_source", default="world") robot_namespace = LaunchConfiguration("robot_namespace", default='bcr_bot') diff --git a/src/bcr_bot/launch/bcr_bot_gz_spawn.launch.py b/src/bcr_bot/launch/bcr_bot_gz_spawn.launch.py index 6faec90..3be3750 100644 --- a/src/bcr_bot/launch/bcr_bot_gz_spawn.launch.py +++ b/src/bcr_bot/launch/bcr_bot_gz_spawn.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): position_y = LaunchConfiguration("position_y") orientation_yaw = LaunchConfiguration("orientation_yaw") camera_enabled = LaunchConfiguration("camera_enabled", default=True) - stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False) + stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=True) two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True) odometry_source = LaunchConfiguration("odometry_source") @@ -126,4 +126,4 @@ def generate_launch_description(): DeclareLaunchArgument("odometry_source", default_value="world"), robot_state_publisher, gz_spawn_entity, transform_publisher, gz_ros2_bridge - ]) \ No newline at end of file + ]) diff --git a/src/bcr_bot/launch/bcr_bot_ign_spawn.launch.py b/src/bcr_bot/launch/bcr_bot_ign_spawn.launch.py index e7671d3..ba7ee63 100644 --- a/src/bcr_bot/launch/bcr_bot_ign_spawn.launch.py +++ b/src/bcr_bot/launch/bcr_bot_ign_spawn.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): position_y = LaunchConfiguration("position_y") orientation_yaw = LaunchConfiguration("orientation_yaw") camera_enabled = LaunchConfiguration("camera_enabled", default=True) - stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=False) + stereo_camera_enabled = LaunchConfiguration("stereo_camera_enabled", default=True) two_d_lidar_enabled = LaunchConfiguration("two_d_lidar_enabled", default=True) odometry_source = LaunchConfiguration("odometry_source") diff --git a/src/bcr_bot/urdf/bcr_bot.xacro b/src/bcr_bot/urdf/bcr_bot.xacro index 7cac6a9..0575ce4 100644 --- a/src/bcr_bot/urdf/bcr_bot.xacro +++ b/src/bcr_bot/urdf/bcr_bot.xacro @@ -38,7 +38,7 @@ - + diff --git a/src/ros_viz_adapter/README.md b/src/ros_viz_adapter/README.md index c24f369..bcf981e 100644 --- a/src/ros_viz_adapter/README.md +++ b/src/ros_viz_adapter/README.md @@ -6,10 +6,10 @@ YAML 配置文件。节点自动发现 `sensor_msgs/msg/Image` 和 ## 输出 -- RGB：直接加载 `image_transport/compressed_pub`，只发布其 - `/viz//rgb/compressed` 传输 topic，不创建 raw base topic。 -- 深度：直接加载 `image_transport/compressedDepth_pub`，只发布其 - `/viz//depth/compressedDepth` 传输 topic，不创建 raw base topic。 +- RGB：直接加载 `image_transport/compressed_pub`，插件发布 + `/viz//rgb/compressed`，不创建 raw base topic。 +- 深度：直接加载 `image_transport/compressedDepth_pub`，插件发布 + `/viz//depth/compressedDepth`，不创建 raw base topic。 - 点云：`/viz//points`，消息仍为 `sensor_msgs/msg/PointCloud2`，但只保留 `x/y/z`，去除 NaN，执行体素降采样、最大点数限制和限频。 diff --git a/src/ros_viz_adapter/src/adapter.cpp b/src/ros_viz_adapter/src/adapter.cpp index bfa979b..305dc94 100644 --- a/src/ros_viz_adapter/src/adapter.cpp +++ b/src/ros_viz_adapter/src/adapter.cpp @@ -56,7 +56,10 @@ private: std::string e = encoding; std::transform(e.begin(), e.end(), e.begin(), [](unsigned char c) { return std::tolower(c); }); if (e == "16uc1" || e == "32fc1" || e == "mono16" || e == "32sc1") return ImageKind::DEPTH; - if (e == "rgb8" || e == "bgr8" || e == "rgba8" || e == "bgra8" || e == "mono8") return ImageKind::RGB; + if (e == "rgb8" || e == "bgr8" || e == "rgba8" || e == "bgra8" || + e == "mono8" || e == "8uc3" || e == "8uc4" || e == "r8g8b8" || + e == "b8g8r8" || e == "rgb_int8" || e == "rgba_int8" || + e == "bgr_int8" || e == "bgra_int8") return ImageKind::RGB; return ImageKind::UNKNOWN; } @@ -77,6 +80,7 @@ private: if (subscriptions_.count(topic) || cloud_subscriptions_.count(topic)) continue; const auto & types = entry.second; if (std::find(types.begin(), types.end(), "sensor_msgs/msg/Image") != types.end()) { + RCLCPP_INFO(get_logger(), "Discovered image topic: %s", topic.c_str()); subscriptions_[topic] = create_subscription(topic, qos_, [this, topic](Image::ConstSharedPtr msg) { on_image(topic, msg); }); } else if (std::find(types.begin(), types.end(), "sensor_msgs/msg/PointCloud2") != types.end()) { cloud_subscriptions_[topic] = create_subscription(topic, qos_, [this, topic](PointCloud2::ConstSharedPtr msg) { on_cloud(topic, msg); }); @@ -87,7 +91,11 @@ private: void on_image(const std::string & topic, Image::ConstSharedPtr msg) { if (!due(topic, input_fps_, false)) return; const auto kind = classify(msg->encoding); - if (kind == ImageKind::UNKNOWN) return; + if (kind == ImageKind::UNKNOWN) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Ignoring image topic %s with encoding '%s'", topic.c_str(), msg->encoding.c_str()); + return; + } + RCLCPP_INFO_ONCE(get_logger(), "Classified image topic %s as %s (encoding=%s)", topic.c_str(), kind == ImageKind::RGB ? "RGB" : "DEPTH", msg->encoding.c_str()); std::lock_guard lock(data_mutex_); auto & slot = kind == ImageKind::RGB ? rgb_latest_ : depth_latest_; slot[topic] = msg; @@ -105,7 +113,8 @@ private: } std::string image_topic(const std::string & topic, bool depth) const { - return (output_ns_.empty() ? "/viz" : output_ns_) + "/" + safe_name(topic) + (depth ? "/depth/compressedDepth" : "/rgb/compressed"); + // Publisher plugins append their own transport suffix. + return (output_ns_.empty() ? "/viz" : output_ns_) + "/" + safe_name(topic) + (depth ? "/depth" : "/rgb"); } void image_loop(std::unordered_map & slots, bool depth) { @@ -126,9 +135,10 @@ private: try { auto plugin = image_loader_->createSharedInstance(lookup); plugin->advertise(this, image_topic(item.first, depth), qos_.get_rmw_qos_profile()); + RCLCPP_INFO(get_logger(), "Loaded %s -> %s", lookup, plugin->getTopic().c_str()); it = image_publishers_.emplace(key, std::move(plugin)).first; } catch (const pluginlib::PluginlibException & ex) { - RCLCPP_ERROR(get_logger(), "Cannot load image_transport plugin %s: %s", lookup, ex.what()); + RCLCPP_ERROR(get_logger(), "Cannot load image_transport plugin %s for %s: %s", lookup, item.first.c_str(), ex.what()); continue; } }