From 5d734a68fd941193ccca4ba733023edebb7f9810 Mon Sep 17 00:00:00 2001 From: duongtd Date: Tue, 21 Jul 2026 11:10:55 +0700 Subject: [PATCH] test costmap + cam depth --- CMakeLists.txt | 59 +++ README.md | 73 +++ config/costmap_params.yaml | 31 ++ config/inflation_layer_params.yaml | 5 + config/obstacle_layer_params.yaml | 39 ++ config/voxel_layer_params.yaml | 15 + launch/depth_local_costmap_test.launch | 47 ++ package.xml | 24 + src/depth_local_costmap_noetic_test_node.cpp | 461 +++++++++++++++++++ 9 files changed, 754 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 config/costmap_params.yaml create mode 100644 config/inflation_layer_params.yaml create mode 100644 config/obstacle_layer_params.yaml create mode 100644 config/voxel_layer_params.yaml create mode 100644 launch/depth_local_costmap_test.launch create mode 100644 package.xml create mode 100644 src/depth_local_costmap_noetic_test_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..3046d1e --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.0.2) +project(depth_local_costmap_noetic_test LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +find_package(catkin REQUIRED COMPONENTS + data_convert + geometry_msgs + nav_msgs + robot_depth_image_proc + robot_costmap_2d + robot_cpp + robot_geometry_msgs + robot_sensor_msgs + robot_time + roscpp + sensor_msgs + tf2_ros +) + +find_library(TF3_LIBRARY + NAMES tf3 + PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu +) + +if(NOT TF3_LIBRARY) + message(FATAL_ERROR "tf3 library not found") +endif() + +catkin_package() + +add_executable(depth_local_costmap_noetic_test_node + src/depth_local_costmap_noetic_test_node.cpp +) + +target_include_directories(depth_local_costmap_noetic_test_node + PRIVATE + ${catkin_INCLUDE_DIRS} +) + +target_link_libraries(depth_local_costmap_noetic_test_node + PRIVATE + ${catkin_LIBRARIES} + ${TF3_LIBRARY} +) + +add_dependencies(depth_local_costmap_noetic_test_node + ${catkin_EXPORTED_TARGETS} +) + +install(TARGETS depth_local_costmap_noetic_test_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY config launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/README.md b/README.md new file mode 100644 index 0000000..78333a0 --- /dev/null +++ b/README.md @@ -0,0 +1,73 @@ +# depth_local_costmap_noetic_test + +Package ROS Noetic C++ tối thiểu để chạy **local costmap only** bằng stack T800: + +- `/home/duongtd/T800_ws/src/AMR_T800/pnkx_nav_core/src/Libraries/costmap_2d` +- package catkin: `robot_costmap_2d` + +Node test nhận dữ liệu ROS Noetic và convert sang message `robot_*` trước khi feed trực tiếp vào `VoxelLayer` bằng `layer->dataCallBack(...)`. + +Luồng dữ liệu chính: + +- `/camera/depth/points_proc` -> `robot_sensor_msgs::PointCloud2` -> source `pc_marking` +- `/camera/depth/image_raw` + `/camera/depth/camera_info` -> `robot_sensor_msgs::DepthCameraData::ConstPtr` -> source `pc_clearing` + +`/camera/depth/data` không cần là ROS topic thật trong gói test này; nó là tên source logic để match với config `robot_costmap_2d`, giống contract trong `sensor_converter.cpp`. + +Topic phụ được node subscribe để log kiểm tra dữ liệu: + +- `/camera_right/depth/camera_info` +- `/odom` +- `/tf` +- `/tf_static` + +## Build + +```bash +cd /home/duongtd/T800_ws +catkin_make --pkg depth_local_costmap_noetic_test +source devel/setup.bash +``` + +## Chạy + +```bash +roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch +``` + +Nếu robot dùng `base_footprint`: + +```bash +roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch base_frame:=base_footprint +``` + +Nếu hệ thống chỉ có `/odom` nhưng chưa publish TF `odom -> base_link`, bật bridge tạm: + +```bash +roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch publish_odom_tf:=true +``` + +Không bật `publish_odom_tf` nếu đã có node khác publish TF này. + +## Kiểm tra runtime + +```bash +rostopic echo -n1 /camera/depth/points_proc/header +rostopic echo -n1 /camera/depth/image_raw/header +rostopic echo -n1 /camera/depth/camera_info/header +rosrun tf tf_echo odom base_link +rostopic list | grep local_costmap +``` + +Frame của `/camera/depth/points_proc` và `/camera/depth/image_raw` phải transform được về `odom`; nếu không, obstacle/voxel layer sẽ drop dữ liệu. + +Log runtime cần thấy: + +- `Fed depth cloud to robot_costmap_2d` +- `Fed DepthCameraData to robot_costmap_2d` + +Config local cho `robot_costmap_2d` nằm trong thư mục `config/` của package này. Launch file set: + +```xml + +``` diff --git a/config/costmap_params.yaml b/config/costmap_params.yaml new file mode 100644 index 0000000..8343c20 --- /dev/null +++ b/config/costmap_params.yaml @@ -0,0 +1,31 @@ +robot_costmap_2d: + global_frame: odom + robot_base_frame: base_link + rolling_window: true + track_unknown_space: false + + plugins: + - name: voxel_layer + type: VoxelLayer + - name: inflation_layer + type: InflationLayer + + library_path: /home/duongtd/T800_ws/devel/lib/libplugins.so + + footprint: + - [0.30, 0.30] + - [0.30, -0.30] + - [-0.30, -0.30] + - [-0.30, 0.30] + + transform_tolerance: 0.5 + performance_metrics_enabled: false + performance_metrics_period: 5.0 + update_frequency: 5.0 + width: 6.0 + height: 6.0 + resolution: 0.05 + origin_x: 0.0 + origin_y: 0.0 + footprint_padding: 0.0 + robot_radius: 0.0 diff --git a/config/inflation_layer_params.yaml b/config/inflation_layer_params.yaml new file mode 100644 index 0000000..931d5ea --- /dev/null +++ b/config/inflation_layer_params.yaml @@ -0,0 +1,5 @@ +inflation_layer: + enabled: true + inflate_unknown: false + cost_scaling_factor: 3.0 + inflation_radius: 0.45 diff --git a/config/obstacle_layer_params.yaml b/config/obstacle_layer_params.yaml new file mode 100644 index 0000000..7dc672b --- /dev/null +++ b/config/obstacle_layer_params.yaml @@ -0,0 +1,39 @@ +obstacle_layer: + track_unknown_space: false + transform_tolerance: 0.5 + footprint_clearing_enabled: true + max_obstacle_height: 1.5 + combination_method: 1 + + observation_sources: pc_marking pc_clearing + + pc_marking: + topic: /camera/depth/points_proc + data_type: PointCloud2 + clearing: false + marking: true + inf_is_valid: false + observation_persistence: 0.0 + expected_update_rate: 0.5 + obstacle_range: 2.5 + raytrace_range: 3.0 + min_obstacle_height: 0.10 + max_obstacle_height: 1.0 + frustum_clearing_enabled: false + + pc_clearing: + topic: /camera/depth/data + data_type: DepthCameraData + clearing: false + marking: true + inf_is_valid: false + observation_persistence: 0.0 + expected_update_rate: 0.5 + obstacle_range: 2.5 + raytrace_range: 3.5 + min_obstacle_height: 0.10 + max_obstacle_height: 1.0 + frustum_clearing_enabled: true + frustum_clearing_pixel_step: 8 + frustum_min_range: 0.20 + frustum_max_range: 3.5 diff --git a/config/voxel_layer_params.yaml b/config/voxel_layer_params.yaml new file mode 100644 index 0000000..c94275d --- /dev/null +++ b/config/voxel_layer_params.yaml @@ -0,0 +1,15 @@ +voxel_layer: + enabled: true + footprint_clearing_enabled: true + max_obstacle_height: 1.5 + origin_z: 0.0 + z_resolution: 0.15 + z_voxels: 8 + unknown_threshold: 15.0 + mark_threshold: 0 + combination_method: 1 + frustum_clearing_enabled: false + frustum_clearing_pixel_step: 8 + frustum_min_range: 0.20 + frustum_max_range: 3.5 + frustum_depth_camera_topic: /camera/depth/data diff --git a/launch/depth_local_costmap_test.launch b/launch/depth_local_costmap_test.launch new file mode 100644 index 0000000..27bb7be --- /dev/null +++ b/launch/depth_local_costmap_test.launch @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..d21be72 --- /dev/null +++ b/package.xml @@ -0,0 +1,24 @@ + + + depth_local_costmap_noetic_test + 0.1.0 + Minimal ROS Noetic C++ local test package for the T800 robot_costmap_2d stack. + + duongtd + BSD + + catkin + + data_convert + geometry_msgs + nav_msgs + robot_depth_image_proc + robot_costmap_2d + robot_cpp + robot_geometry_msgs + robot_sensor_msgs + robot_time + roscpp + sensor_msgs + tf2_ros + diff --git a/src/depth_local_costmap_noetic_test_node.cpp b/src/depth_local_costmap_noetic_test_node.cpp new file mode 100644 index 0000000..5ec4175 --- /dev/null +++ b/src/depth_local_costmap_noetic_test_node.cpp @@ -0,0 +1,461 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ +robot_sensor_msgs::PointCloud2 toRobotPointCloud2(const sensor_msgs::PointCloud2& msg) +{ + robot_sensor_msgs::PointCloud2 robot_pc; + + robot_pc.header.seq = msg.header.seq; + robot_pc.header.stamp = robot::Time(msg.header.stamp.toSec()); + robot_pc.header.frame_id = msg.header.frame_id; + robot_pc.height = msg.height; + robot_pc.width = msg.width; + + robot_pc.fields.clear(); + robot_pc.fields.reserve(msg.fields.size()); + for (const auto& field : msg.fields) + { + robot_sensor_msgs::PointField robot_field; + robot_field.name = field.name; + robot_field.offset = field.offset; + robot_field.datatype = field.datatype; + robot_field.count = field.count; + robot_pc.fields.push_back(robot_field); + } + + robot_pc.is_bigendian = msg.is_bigendian; + robot_pc.point_step = msg.point_step; + robot_pc.row_step = msg.row_step; + robot_pc.is_dense = msg.is_dense; + robot_pc.data = msg.data; + return robot_pc; +} + +bool isVoxelLayer(const robot_costmap_2d::Layer& layer) +{ + return layer.getType() == robot_costmap_2d::LayerType::VOXEL_LAYER || + layer.getName() == "local_costmap/voxel_layer"; +} + +template +void feedVoxelLayers(robot_costmap_2d::Costmap2DROBOT& local_costmap, + const MessageT& message, + const std::string& topic) +{ + if (local_costmap.getLayeredCostmap() == nullptr || + local_costmap.getLayeredCostmap()->getPlugins() == nullptr) + { + return; + } + + for (const auto& layer : *local_costmap.getLayeredCostmap()->getPlugins()) + { + if (layer && isVoxelLayer(*layer)) + layer->dataCallBack(message, topic); + } +} + +class RosTfToTf3Bridge +{ +public: + RosTfToTf3Bridge(std::shared_ptr tf2_buffer, tf3::BufferCore& tf3_buffer) + : tf2_buffer_(std::move(tf2_buffer)) + , tf3_buffer_(tf3_buffer) + , worker_(&RosTfToTf3Bridge::run, this) + { + tf3_buffer_.setUsingDedicatedThread(true); + } + + ~RosTfToTf3Bridge() + { + stop_ = true; + if (worker_.joinable()) + worker_.join(); + } + +private: + struct TfEdge + { + std::string parent; + std::string child; + }; + + static std::vector parseEdges(const std::string& tree) + { + std::vector edges; + std::istringstream stream(tree); + std::string line; + + while (std::getline(stream, line)) + { + const std::size_t frame_pos = line.find("Frame "); + const std::size_t parent_pos = line.find(" exists with parent "); + if (frame_pos == std::string::npos || parent_pos == std::string::npos) + continue; + + std::string child = line.substr(frame_pos + 6, parent_pos - (frame_pos + 6)); + std::string parent = line.substr(parent_pos + 20); + if (!parent.empty() && parent.back() == '.') + parent.pop_back(); + if (!parent.empty() && !child.empty()) + edges.push_back({parent, child}); + } + + return edges; + } + + void run() + { + ros::Rate rate(50.0); + + while (ros::ok() && !stop_) + { + if (!tf2_buffer_) + { + rate.sleep(); + continue; + } + + const std::vector edges = parseEdges(tf2_buffer_->allFramesAsString()); + for (const auto& edge : edges) + { + try + { + if (!tf2_buffer_->canTransform(edge.parent, edge.child, ros::Time(0), ros::Duration(0.01))) + continue; + + const geometry_msgs::TransformStamped ros_tf = + tf2_buffer_->lookupTransform(edge.parent, edge.child, ros::Time(0)); + + tf3::TransformStampedMsg tf3_msg; + tf3_msg.header.stamp = tf3::Time::now(); + tf3_msg.header.frame_id = ros_tf.header.frame_id; + tf3_msg.child_frame_id = ros_tf.child_frame_id; + tf3_msg.transform.translation.x = ros_tf.transform.translation.x; + tf3_msg.transform.translation.y = ros_tf.transform.translation.y; + tf3_msg.transform.translation.z = ros_tf.transform.translation.z; + tf3_msg.transform.rotation.x = ros_tf.transform.rotation.x; + tf3_msg.transform.rotation.y = ros_tf.transform.rotation.y; + tf3_msg.transform.rotation.z = ros_tf.transform.rotation.z; + tf3_msg.transform.rotation.w = ros_tf.transform.rotation.w; + tf3_buffer_.setTransform(tf3_msg, "ros_tf_bridge"); + } + catch (const tf2::TransformException& ex) + { + ROS_WARN_THROTTLE(5.0, "TF bridge failed %s -> %s: %s", + edge.parent.c_str(), + edge.child.c_str(), + ex.what()); + } + } + + rate.sleep(); + } + } + + std::shared_ptr tf2_buffer_; + tf3::BufferCore& tf3_buffer_; + std::atomic stop_{false}; + std::thread worker_; +}; + +class OdomTfBridge +{ +public: + OdomTfBridge(ros::NodeHandle& private_nh, const std::string& base_frame) + : base_frame_(base_frame) + { + private_nh.param("odom_topic", odom_topic_, std::string("/odom")); + odom_sub_ = private_nh.subscribe(odom_topic_, 10, &OdomTfBridge::odomCallback, this); + ROS_WARN("Publishing ROS TF from %s. Keep disabled if another node already publishes odom -> %s.", + odom_topic_.c_str(), + base_frame_.c_str()); + } + +private: + void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) + { + if (!msg) + return; + + geometry_msgs::TransformStamped transform; + transform.header = msg->header; + transform.child_frame_id = msg->child_frame_id.empty() ? base_frame_ : msg->child_frame_id; + transform.transform.translation.x = msg->pose.pose.position.x; + transform.transform.translation.y = msg->pose.pose.position.y; + transform.transform.translation.z = msg->pose.pose.position.z; + transform.transform.rotation = msg->pose.pose.orientation; + tf_broadcaster_.sendTransform(transform); + } + + std::string base_frame_; + std::string odom_topic_; + ros::Subscriber odom_sub_; + tf2_ros::TransformBroadcaster tf_broadcaster_; +}; + +class DepthCloudFeeder +{ +public: + DepthCloudFeeder(ros::NodeHandle& private_nh, robot_costmap_2d::Costmap2DROBOT& local_costmap) + : local_costmap_(local_costmap) + { + private_nh.param("depth_cloud_topic", depth_cloud_topic_, std::string("/camera/depth/points_proc")); + depth_cloud_sub_ = + private_nh.subscribe(depth_cloud_topic_, 1, &DepthCloudFeeder::depthCloudCallback, this); + } + +private: + void depthCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) + { + if (!msg) + return; + + const robot_sensor_msgs::PointCloud2 robot_cloud = toRobotPointCloud2(*msg); + feedVoxelLayers(local_costmap_, robot_cloud, depth_cloud_topic_); + + ROS_INFO_THROTTLE(5.0, "Fed depth cloud to robot_costmap_2d: %ux%u frame=%s", + msg->width, + msg->height, + msg->header.frame_id.c_str()); + } + + robot_costmap_2d::Costmap2DROBOT& local_costmap_; + std::string depth_cloud_topic_; + ros::Subscriber depth_cloud_sub_; +}; + +class DepthCameraDataFeeder +{ +public: + DepthCameraDataFeeder(ros::NodeHandle& private_nh, robot_costmap_2d::Costmap2DROBOT& local_costmap) + : local_costmap_(local_costmap) + { + private_nh.param("depth_image_topic", depth_image_topic_, std::string("/camera/depth/image_raw")); + private_nh.param("camera_info_topic", camera_info_topic_, std::string("/camera/depth/camera_info")); + private_nh.param("depth_camera_data_topic", + depth_camera_data_topic_, + std::string("/camera/depth/data")); + + camera_info_sub_ = + private_nh.subscribe(camera_info_topic_, 1, &DepthCameraDataFeeder::cameraInfoCallback, this); + depth_image_sub_ = + private_nh.subscribe(depth_image_topic_, 1, &DepthCameraDataFeeder::depthImageCallback, this); + } + +private: + void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg) + { + if (!msg) + return; + + std::lock_guard lock(camera_info_mutex_); + camera_info_ = depth_image_proc::toRobotCameraInfo(*msg); + has_camera_info_ = true; + + ROS_INFO_THROTTLE(5.0, "Cached camera_info for depth clearing: %ux%u frame=%s", + msg->width, + msg->height, + msg->header.frame_id.c_str()); + } + + void depthImageCallback(const sensor_msgs::Image::ConstPtr& msg) + { + if (!msg) + return; + + robot_sensor_msgs::CameraInfo camera_info; + { + std::lock_guard lock(camera_info_mutex_); + if (!has_camera_info_) + { + ROS_WARN_THROTTLE(5.0, + "Waiting for %s before feeding DepthCameraData to robot_costmap_2d", + camera_info_topic_.c_str()); + return; + } + camera_info = camera_info_; + } + + if (camera_info.K[0] <= 0.0 || camera_info.K[4] <= 0.0) + { + ROS_ERROR_THROTTLE(5.0, "Invalid depth camera intrinsics on %s", camera_info_topic_.c_str()); + return; + } + + if (msg->encoding != "16UC1" && msg->encoding != "mono16" && msg->encoding != "32FC1") + { + ROS_ERROR_THROTTLE( + 5.0, + "Unsupported depth encoding [%s], expected 16UC1/mono16/32FC1", + msg->encoding.c_str()); + return; + } + + robot_sensor_msgs::DepthCameraData::Ptr depth_camera_data(new robot_sensor_msgs::DepthCameraData()); + depth_camera_data->depth = depth_image_proc::toRobotImage(*msg); + depth_camera_data->camera_info = camera_info; + depth_camera_data->header = depth_camera_data->depth.header; + if (depth_camera_data->header.frame_id.empty()) + depth_camera_data->header.frame_id = depth_camera_data->camera_info.header.frame_id; + + robot_sensor_msgs::DepthCameraData::ConstPtr const_depth_camera_data = depth_camera_data; + feedVoxelLayers(local_costmap_, const_depth_camera_data, depth_camera_data_topic_); + + ROS_INFO_THROTTLE(5.0, "Fed DepthCameraData to robot_costmap_2d: %ux%u encoding=%s frame=%s topic=%s", + msg->width, + msg->height, + msg->encoding.c_str(), + depth_camera_data->header.frame_id.c_str(), + depth_camera_data_topic_.c_str()); + } + + robot_costmap_2d::Costmap2DROBOT& local_costmap_; + std::string depth_image_topic_; + std::string camera_info_topic_; + std::string depth_camera_data_topic_; + robot_sensor_msgs::CameraInfo camera_info_; + bool has_camera_info_{false}; + std::mutex camera_info_mutex_; + ros::Subscriber depth_image_sub_; + ros::Subscriber camera_info_sub_; +}; + +class RightCameraInfoWatchdog +{ +public: + explicit RightCameraInfoWatchdog(ros::NodeHandle& private_nh) + { + std::string camera_right_info_topic; + private_nh.param("camera_right_info_topic", + camera_right_info_topic, + std::string("/camera_right/depth/camera_info")); + + camera_right_info_sub_ = + private_nh.subscribe(camera_right_info_topic, 1, &RightCameraInfoWatchdog::cameraRightInfoCallback, this); + } + +private: + void cameraRightInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg) + { + if (msg) + ROS_INFO_THROTTLE(5.0, "Right camera info OK: %ux%u frame=%s", + msg->width, + msg->height, + msg->header.frame_id.c_str()); + } + + ros::Subscriber camera_right_info_sub_; +}; +} // namespace + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "depth_local_costmap_noetic_test"); + + ros::NodeHandle private_nh("~"); + ros::AsyncSpinner spinner(2); + spinner.start(); + + std::string base_frame; + bool publish_odom_tf = false; + private_nh.param("base_frame", base_frame, std::string("base_link")); + private_nh.param("publish_odom_tf", publish_odom_tf, false); + + auto tf2_buffer = std::make_shared(ros::Duration(10.0)); + tf2_ros::TransformListener tf2_listener(*tf2_buffer); + tf3::BufferCore tf3_buffer; + RosTfToTf3Bridge tf_bridge(tf2_buffer, tf3_buffer); + + std::unique_ptr odom_tf_bridge; + if (publish_odom_tf) + odom_tf_bridge = std::make_unique(private_nh, base_frame); + + RightCameraInfoWatchdog right_camera_info_watchdog(private_nh); + + try + { + robot_costmap_2d::Costmap2DROBOT local_costmap("local_costmap", tf3_buffer); + DepthCloudFeeder depth_cloud_feeder(private_nh, local_costmap); + DepthCameraDataFeeder depth_camera_data_feeder(private_nh, local_costmap); + local_costmap.start(); + + ROS_INFO("T800 robot_costmap_2d local costmap test started."); + + ros::Rate rate(2.0); + while (ros::ok()) + { + robot_geometry_msgs::PoseStamped robot_pose; + const bool pose_ok = local_costmap.getRobotPose(robot_pose); + const robot_costmap_2d::Costmap2D* costmap = local_costmap.getCostmap(); + + if (costmap == nullptr) + { + ROS_WARN_THROTTLE(2.0, "robot_costmap_2d local costmap pointer is null"); + } + else if (!pose_ok) + { + ROS_WARN_THROTTLE( + 2.0, + "robot_costmap_2d has no robot pose. Check TF: odom -> %s and cloud frame -> odom.", + base_frame.c_str()); + } + else + { + ROS_INFO_THROTTLE( + 2.0, + "robot_costmap_2d OK: size=%ux%u res=%.3f origin=(%.2f, %.2f) robot=(%.2f, %.2f)", + costmap->getSizeInCellsX(), + costmap->getSizeInCellsY(), + costmap->getResolution(), + costmap->getOriginX(), + costmap->getOriginY(), + robot_pose.pose.position.x, + robot_pose.pose.position.y); + } + + rate.sleep(); + } + + local_costmap.stop(); + } + catch (const std::exception& e) + { + ROS_FATAL("depth_local_costmap_noetic_test failed: %s", e.what()); + return 1; + } + + return 0; +}