From c82f6dc6b5f49313d17d70b74b34df7e7a93daf0 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 30 May 2023 16:24:20 +0300 Subject: [PATCH] fix names --- .../include/base_realsense_node.h | 17 ++----- realsense2_camera/include/constants.h | 8 ---- realsense2_camera/include/ros_utils.h | 3 -- realsense2_camera/src/base_realsense_node.cpp | 48 ++++++------------- realsense2_camera/src/ros_utils.cpp | 42 ---------------- realsense2_camera/src/rs_node_setup.cpp | 20 ++++---- 6 files changed, 29 insertions(+), 109 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index cd9aaba667..76d7bfe032 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -68,12 +68,6 @@ namespace realsense2_camera { typedef std::pair stream_index_pair; - const std::vector IMAGE_STREAMS = {DEPTH, INFRA0, INFRA1, INFRA2, - COLOR, - FISHEYE, - FISHEYE1, FISHEYE2}; - - const std::vector HID_STREAMS = {GYRO, ACCEL, POSE}; class image_publisher; // forward declaration class PipelineSyncer : public rs2::asynchronous_syncer @@ -214,9 +208,6 @@ namespace realsense2_camera void publishFrame(rs2::frame f, const rclcpp::Time& t, const stream_index_pair& stream, - std::map& images, - const std::map::SharedPtr>& info_publishers, - const std::map>& image_publishers, const bool is_publishMetadata = true); void publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id); @@ -275,12 +266,12 @@ namespace realsense2_camera std::map::SharedPtr> _imu_publishers; std::shared_ptr> _odom_publisher; std::shared_ptr _synced_imu_publisher; - std::map _image_format; - std::map::SharedPtr> _info_publisher; + std::map _image_formats; + std::map::SharedPtr> _info_publishers; std::map::SharedPtr> _metadata_publishers; - std::map::SharedPtr> _imu_info_publisher; + std::map::SharedPtr> _imu_info_publishers; std::map::SharedPtr> _extrinsics_publishers; - std::map _image; + std::map _images; std::map _encoding; std::map _camera_info; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index f451222a74..3894a08071 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -98,17 +98,9 @@ namespace realsense2_camera const std::string DEFAULT_QOS = "DEFAULT"; const std::string HID_QOS = "SENSOR_DATA"; - - const bool ENABLE_DEPTH = true; - const bool ENABLE_INFRA1 = true; - const bool ENABLE_INFRA2 = true; - const bool ENABLE_COLOR = true; - const bool ENABLE_FISHEYE = true; - const bool ENABLE_IMU = true; const bool HOLD_BACK_IMU_FOR_FRAMES = false; const bool PUBLISH_ODOM_TF = true; - const std::string DEFAULT_BASE_FRAME_ID = "link"; const std::string DEFAULT_ODOM_FRAME_ID = "odom_frame"; const std::string DEFAULT_IMU_OPTICAL_FRAME_ID = "camera_imu_optical_frame"; diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index 55b3dafa13..4fa45899e7 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -46,8 +46,5 @@ namespace realsense2_camera const rmw_qos_profile_t qos_string_to_qos(std::string str); const std::string list_available_qos_strings(); - rs2_stream rs2_string_to_stream(std::string str); - - stream_index_pair rs2_string_to_sip(const std::string& str); } diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 310bb25bd5..708b8f548a 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -120,9 +120,9 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, ROS_INFO("Intra-Process communication enabled"); } - _image_format[1] = CV_8UC1; // CVBridge type - _image_format[2] = CV_16UC1; // CVBridge type - _image_format[3] = CV_8UC3; // CVBridge type + _image_formats[1] = CV_8UC1; // CVBridge type + _image_formats[2] = CV_16UC1; // CVBridge type + _image_formats[3] = CV_8UC3; // CVBridge type _encoding[1] = sensor_msgs::image_encodings::MONO8; // ROS message type _encoding[2] = sensor_msgs::image_encodings::TYPE_16UC1; // ROS message type _encoding[3] = sensor_msgs::image_encodings::RGB8; // ROS message type @@ -219,7 +219,7 @@ cv::Mat& BaseRealSenseNode::fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image.create(from_image.rows, from_image.cols, from_image.type()); } - CV_Assert(from_image.depth() == _image_format[2]); + CV_Assert(from_image.depth() == _image_formats[2]); int nRows = from_image.rows; int nCols = from_image.cols; @@ -531,18 +531,11 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) sent_depth_frame = true; if (_align_depth_filter->is_enabled()) { - publishFrame(f, t, COLOR, - _depth_aligned_image, - _depth_aligned_info_publisher, - _depth_aligned_image_publishers, - false); + publishFrame(f, t, COLOR, false); continue; } } - publishFrame(f, t, sip, - _image, - _info_publisher, - _image_publishers); + publishFrame(f, t, sip); } if (original_depth_frame && _align_depth_filter->is_enabled()) { @@ -552,11 +545,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) else frame_to_send = original_depth_frame; - publishFrame(frame_to_send, t, - DEPTH, - _image, - _info_publisher, - _image_publishers); + publishFrame(frame_to_send, t, DEPTH); } } else if (frame.is()) @@ -574,11 +563,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) clip_depth(frame, _clipping_distance); } } - publishFrame(frame, t, - sip, - _image, - _info_publisher, - _image_publishers); + publishFrame(frame, t, sip); } _synced_imu_publisher->Resume(); } // frame_callback @@ -832,9 +817,6 @@ IMUInfo BaseRealSenseNode::getImuInfo(const rs2::stream_profile& profile) void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t, const stream_index_pair& stream, - std::map& images, - const std::map::SharedPtr>& info_publishers, - const std::map>& image_publishers, const bool is_publishMetadata) { ROS_DEBUG("publishFrame(...)"); @@ -848,11 +830,11 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t, height = timage.get_height(); bpp = timage.get_bytes_per_pixel(); } - auto& image = images[stream]; + auto& image = _images[stream]; - if (image.size() != cv::Size(width, height) || image.depth() != _image_format[bpp]) + if (image.size() != cv::Size(width, height) || image.depth() != _image_formats[bpp]) { - image.create(height, width, _image_format[bpp]); + image.create(height, width, _image_formats[bpp]); } image.data = (uint8_t*)f.get_data(); @@ -861,14 +843,14 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t, image = fix_depth_scale(image, _depth_scaled_image[stream]); } - if (info_publishers.find(stream) == info_publishers.end() || - image_publishers.find(stream) == image_publishers.end()) + if (_info_publishers.find(stream) == _info_publishers.end() || + _image_publishers.find(stream) == _image_publishers.end()) { // Stream is already disabled. return; } - auto& info_publisher = info_publishers.at(stream); - auto& image_publisher = image_publishers.at(stream); + auto& info_publisher = _info_publishers.at(stream); + auto& image_publisher = _image_publishers.at(stream); if(0 != info_publisher->get_subscription_count() || 0 != image_publisher->get_subscription_count()) { diff --git a/realsense2_camera/src/ros_utils.cpp b/realsense2_camera/src/ros_utils.cpp index e5fa1c3817..b543be2af0 100644 --- a/realsense2_camera/src/ros_utils.cpp +++ b/realsense2_camera/src/ros_utils.cpp @@ -129,46 +129,4 @@ const std::string list_available_qos_strings() return res.str(); } -rs2_stream rs2_string_to_stream(std::string str) -{ - if (str == "RS2_STREAM_ANY") - return RS2_STREAM_ANY; - if (str == "RS2_STREAM_COLOR") - return RS2_STREAM_COLOR; - if (str == "RS2_STREAM_INFRARED") - return RS2_STREAM_INFRARED; - if (str == "RS2_STREAM_FISHEYE") - return RS2_STREAM_FISHEYE; - throw std::runtime_error("Unknown stream string " + str); -} - -stream_index_pair rs2_string_to_sip(const std::string& str) -{ - if (str == "color") - return realsense2_camera::COLOR; - if (str == "depth") - return DEPTH; - if (str == "infra") - return INFRA0; - if (str == "infra1") - return INFRA1; - if (str == "infra2") - return INFRA2; - if (str == "fisheye") - return FISHEYE; - if (str == "fisheye1") - return FISHEYE1; - if (str == "fisheye2") - return FISHEYE2; - if (str == "gyro") - return GYRO; - if (str == "accel") - return ACCEL; - if (str == "pose") - return POSE; - std::stringstream ss; - ss << "Unknown parameter " << str << " in" << __FILE__ << ":" << __LINE__; - throw std::runtime_error(ss.str()); -} - } diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 2293856f62..57d7420489 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -184,14 +184,14 @@ void BaseRealSenseNode::stopPublishers(const std::vector& profil if (profile.is()) { _image_publishers.erase(sip); - _info_publisher.erase(sip); + _info_publishers.erase(sip); _depth_aligned_image_publishers.erase(sip); _depth_aligned_info_publisher.erase(sip); } else if (profile.is()) { _imu_publishers.erase(sip); - _imu_info_publisher.erase(sip); + _imu_info_publishers.erase(sip); } _metadata_publishers.erase(sip); _extrinsics_publishers.erase(sip); @@ -237,7 +237,7 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi ROS_DEBUG_STREAM("image transport publisher was created for topic" << image_raw.str()); } - _info_publisher[sip] = _node.create_publisher(camera_info.str(), + _info_publishers[sip] = _node.create_publisher(camera_info.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); if (_align_depth_filter->is_enabled() && (sip != DEPTH) && sip.second < 2) @@ -261,7 +261,7 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi ROS_DEBUG_STREAM("image transport publisher was created for topic" << image_raw.str()); } _depth_aligned_info_publisher[sip] = _node.create_publisher(aligned_camera_info.str(), - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); } } else if (profile.is()) @@ -269,24 +269,24 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi std::stringstream data_topic_name, info_topic_name; data_topic_name << stream_name << "/sample"; _imu_publishers[sip] = _node.create_publisher(data_topic_name.str(), - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); // Publish Intrinsics: info_topic_name << stream_name << "/imu_info"; - _imu_info_publisher[sip] = _node.create_publisher(info_topic_name.str(), + _imu_info_publishers[sip] = _node.create_publisher(info_topic_name.str(), rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); IMUInfo info_msg = getImuInfo(profile); - _imu_info_publisher[sip]->publish(info_msg); + _imu_info_publishers[sip]->publish(info_msg); } else if (profile.is()) { std::stringstream data_topic_name, info_topic_name; data_topic_name << stream_name << "/sample"; _odom_publisher = _node.create_publisher(data_topic_name.str(), - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); } std::string topic_metadata(stream_name + "/metadata"); _metadata_publishers[sip] = _node.create_publisher(topic_metadata, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos)); if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile)) { @@ -298,7 +298,7 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi std::string topic_extrinsics("extrinsics/" + create_graph_resource_name(ros_stream_to_string(_base_profile.stream_type()) + "_to_" + stream_name)); _extrinsics_publishers[sip] = _node.create_publisher(topic_extrinsics, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(extrinsics_qos), extrinsics_qos), std::move(options)); + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(extrinsics_qos), extrinsics_qos), std::move(options)); } } }