Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Cleanups and name fixes #2759

Merged
merged 1 commit into from
Jun 29, 2023
Merged

Cleanups and name fixes #2759

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 4 additions & 13 deletions realsense2_camera/include/base_realsense_node.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,12 +68,6 @@ namespace realsense2_camera
{
typedef std::pair<rs2_stream, int> stream_index_pair;

const std::vector<stream_index_pair> IMAGE_STREAMS = {DEPTH, INFRA0, INFRA1, INFRA2,
COLOR,
FISHEYE,
FISHEYE1, FISHEYE2};

const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL, POSE};
class image_publisher; // forward declaration

class PipelineSyncer : public rs2::asynchronous_syncer
Expand Down Expand Up @@ -214,9 +208,6 @@ namespace realsense2_camera

void publishFrame(rs2::frame f, const rclcpp::Time& t,
const stream_index_pair& stream,
std::map<stream_index_pair, cv::Mat>& images,
const std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>& info_publishers,
const std::map<stream_index_pair, std::shared_ptr<image_publisher>>& image_publishers,
const bool is_publishMetadata = true);
void publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id);

Expand Down Expand Up @@ -275,12 +266,12 @@ namespace realsense2_camera
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> _imu_publishers;
std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> _odom_publisher;
std::shared_ptr<SyncedImuPublisher> _synced_imu_publisher;
std::map<unsigned int, int> _image_format;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr> _info_publisher;
std::map<unsigned int, int> _image_formats;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr> _info_publishers;
std::map<stream_index_pair, rclcpp::Publisher<realsense2_camera_msgs::msg::Metadata>::SharedPtr> _metadata_publishers;
std::map<stream_index_pair, rclcpp::Publisher<IMUInfo>::SharedPtr> _imu_info_publisher;
std::map<stream_index_pair, rclcpp::Publisher<IMUInfo>::SharedPtr> _imu_info_publishers;
std::map<stream_index_pair, rclcpp::Publisher<Extrinsics>::SharedPtr> _extrinsics_publishers;
std::map<stream_index_pair, cv::Mat> _image;
std::map<stream_index_pair, cv::Mat> _images;
std::map<unsigned int, std::string> _encoding;

std::map<stream_index_pair, sensor_msgs::msg::CameraInfo> _camera_info;
Expand Down
8 changes: 0 additions & 8 deletions realsense2_camera/include/constants.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down
3 changes: 0 additions & 3 deletions realsense2_camera/include/ros_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

48 changes: 15 additions & 33 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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())
{
Expand All @@ -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<rs2::video_frame>())
Expand All @@ -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
Expand Down Expand Up @@ -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<stream_index_pair, cv::Mat>& images,
const std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>& info_publishers,
const std::map<stream_index_pair, std::shared_ptr<image_publisher>>& image_publishers,
const bool is_publishMetadata)
{
ROS_DEBUG("publishFrame(...)");
Expand All @@ -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();

Expand All @@ -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())
{
Expand Down
42 changes: 0 additions & 42 deletions realsense2_camera/src/ros_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}

}
20 changes: 10 additions & 10 deletions realsense2_camera/src/rs_node_setup.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -184,14 +184,14 @@ void BaseRealSenseNode::stopPublishers(const std::vector<stream_profile>& profil
if (profile.is<rs2::video_stream_profile>())
{
_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<rs2::motion_stream_profile>())
{
_imu_publishers.erase(sip);
_imu_info_publisher.erase(sip);
_imu_info_publishers.erase(sip);
}
_metadata_publishers.erase(sip);
_extrinsics_publishers.erase(sip);
Expand Down Expand Up @@ -237,7 +237,7 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
ROS_DEBUG_STREAM("image transport publisher was created for topic" << image_raw.str());
}

_info_publisher[sip] = _node.create_publisher<sensor_msgs::msg::CameraInfo>(camera_info.str(),
_info_publishers[sip] = _node.create_publisher<sensor_msgs::msg::CameraInfo>(camera_info.str(),
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));

if (_align_depth_filter->is_enabled() && (sip != DEPTH) && sip.second < 2)
Expand All @@ -261,32 +261,32 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
ROS_DEBUG_STREAM("image transport publisher was created for topic" << image_raw.str());
}
_depth_aligned_info_publisher[sip] = _node.create_publisher<sensor_msgs::msg::CameraInfo>(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<rs2::motion_stream_profile>())
{
std::stringstream data_topic_name, info_topic_name;
data_topic_name << stream_name << "/sample";
_imu_publishers[sip] = _node.create_publisher<sensor_msgs::msg::Imu>(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<IMUInfo>(info_topic_name.str(),
_imu_info_publishers[sip] = _node.create_publisher<IMUInfo>(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<rs2::pose_stream_profile>())
{
std::stringstream data_topic_name, info_topic_name;
data_topic_name << stream_name << "/sample";
_odom_publisher = _node.create_publisher<nav_msgs::msg::Odometry>(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<realsense2_camera_msgs::msg::Metadata>(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))
{
Expand All @@ -298,7 +298,7 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& 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<realsense2_camera_msgs::msg::Extrinsics>(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));
}
}
}
Expand Down