Skip to content

Commit

Permalink
PR #2857 from lge-ros2: Apply camera name in topics
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun authored Aug 29, 2023
2 parents 40ac69f + 180a711 commit c92ddf0
Show file tree
Hide file tree
Showing 2 changed files with 29 additions and 29 deletions.
18 changes: 9 additions & 9 deletions realsense2_camera/src/named_filter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ void NamedFilter::clearParameters()
{
auto name = _parameters_names.back();
_params.getParameters()->removeParam(name);
_parameters_names.pop_back();
_parameters_names.pop_back();
}
}

Expand Down Expand Up @@ -117,12 +117,12 @@ void PointcloudFilter::setParameters()
});
}

void PointcloudFilter::setPublisher()
{
void PointcloudFilter::setPublisher()
{
std::lock_guard<std::mutex> lock_guard(_mutex_publisher);
if ((_is_enabled) && (!_pointcloud_publisher))
{
_pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("depth/color/points",
_pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("~/depth/color/points",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_string_to_qos(_pointcloud_qos)),
qos_string_to_qos(_pointcloud_qos)));
}
Expand Down Expand Up @@ -156,8 +156,8 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2:
if (use_texture)
{
std::set<rs2_format> available_formats{ rs2_format::RS2_FORMAT_RGB8, rs2_format::RS2_FORMAT_Y8 };
texture_frame_itr = std::find_if(frameset.begin(), frameset.end(), [&texture_source_id, &available_formats] (rs2::frame f)

texture_frame_itr = std::find_if(frameset.begin(), frameset.end(), [&texture_source_id, &available_formats] (rs2::frame f)
{return (rs2_stream(f.get_profile().stream_type()) == texture_source_id) &&
(available_formats.find(f.get_profile().format()) != available_formats.end()); });
if (texture_frame_itr == frameset.end())
Expand All @@ -181,7 +181,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2:
sensor_msgs::msg::PointCloud2::UniquePtr msg_pointcloud = std::make_unique<sensor_msgs::msg::PointCloud2>();

sensor_msgs::PointCloud2Modifier modifier(*msg_pointcloud);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(pc.size());
if (_ordered_pc)
{
Expand Down Expand Up @@ -266,7 +266,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2:
*iter_x = vertex->x;
*iter_y = vertex->y;
*iter_z = vertex->z;

++iter_x; ++iter_y; ++iter_z;
++valid_count;
}
Expand All @@ -289,7 +289,7 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2:
}


AlignDepthFilter::AlignDepthFilter(std::shared_ptr<rs2::filter> filter,
AlignDepthFilter::AlignDepthFilter(std::shared_ptr<rs2::filter> filter,
std::function<void(const rclcpp::Parameter&)> update_align_depth_func,
std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled):
NamedFilter(filter, parameters, logger, is_enabled, false)
Expand Down
40 changes: 20 additions & 20 deletions realsense2_camera/src/rs_node_setup.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ void BaseRealSenseNode::setup()

void BaseRealSenseNode::setupFiltersPublishers()
{
_synced_imu_publisher = std::make_shared<SyncedImuPublisher>(_node.create_publisher<sensor_msgs::msg::Imu>("imu", 5));
_synced_imu_publisher = std::make_shared<SyncedImuPublisher>(_node.create_publisher<sensor_msgs::msg::Imu>("~/imu", 5));
}

void BaseRealSenseNode::monitoringProfileChanges()
Expand Down Expand Up @@ -108,12 +108,12 @@ void BaseRealSenseNode::setAvailableSensors()
ROS_INFO_STREAM("Device Product ID: 0x" << pid);

ROS_INFO_STREAM("Sync Mode: " << ((_sync_frames)?"On":"Off"));

std::function<void(rs2::frame)> frame_callback_function = [this](rs2::frame frame){
bool is_filter(_filters.end() != find_if(_filters.begin(), _filters.end(), [](std::shared_ptr<NamedFilter> f){return (f->is_enabled()); }));
if (_sync_frames || is_filter)
this->_asyncer.invoke(frame);
else
else
frame_callback(frame);
};

Expand Down Expand Up @@ -141,7 +141,7 @@ void BaseRealSenseNode::setAvailableSensors()
{
const std::string module_name(rs2_to_ros(sensor.get_info(RS2_CAMERA_INFO_NAME)));
std::unique_ptr<RosSensor> rosSensor;
if (sensor.is<rs2::depth_sensor>() ||
if (sensor.is<rs2::depth_sensor>() ||
sensor.is<rs2::color_sensor>() ||
sensor.is<rs2::fisheye_sensor>())
{
Expand Down Expand Up @@ -225,8 +225,8 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
if (sensor.rs2::sensor::is<rs2::depth_sensor>())
rectified_image = true;

image_raw << stream_name << "/image_" << ((rectified_image)?"rect_":"") << "raw";
camera_info << stream_name << "/camera_info";
image_raw << "~/" << stream_name << "/image_" << ((rectified_image)?"rect_":"") << "raw";
camera_info << "~/" << stream_name << "/camera_info";

// We can use 2 types of publishers:
// Native RCL publisher that support intra-process zero-copy comunication
Expand All @@ -247,8 +247,8 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
if (_align_depth_filter->is_enabled() && (sip != DEPTH) && sip.second < 2)
{
std::stringstream aligned_image_raw, aligned_camera_info;
aligned_image_raw << "aligned_depth_to_" << stream_name << "/image_raw";
aligned_camera_info << "aligned_depth_to_" << stream_name << "/camera_info";
aligned_image_raw << "~/" << "aligned_depth_to_" << stream_name << "/image_raw";
aligned_camera_info << "~/" << "aligned_depth_to_" << stream_name << "/camera_info";

std::string aligned_stream_name = "aligned_depth_to_" + stream_name;

Expand All @@ -271,11 +271,11 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
else if (profile.is<rs2::motion_stream_profile>())
{
std::stringstream data_topic_name, info_topic_name;
data_topic_name << stream_name << "/sample";
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));
// Publish Intrinsics:
info_topic_name << stream_name << "/imu_info";
info_topic_name << "~/" << stream_name << "/imu_info";
_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);
Expand All @@ -284,24 +284,24 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
else if (profile.is<rs2::pose_stream_profile>())
{
std::stringstream data_topic_name, info_topic_name;
data_topic_name << stream_name << "/sample";
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));
}
std::string topic_metadata(stream_name + "/metadata");
_metadata_publishers[sip] = _node.create_publisher<realsense2_camera_msgs::msg::Metadata>(topic_metadata,
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));

if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile))
{

// intra-process do not support latched QoS, so we need to disable intra-process for this topic
rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
rmw_qos_profile_t extrinsics_qos = rmw_qos_profile_latched;
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,

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));
}
}
Expand All @@ -316,7 +316,7 @@ void BaseRealSenseNode::startRGBDPublisherIfNeeded()
{
rmw_qos_profile_t qos = _use_intra_process ? qos_string_to_qos(DEFAULT_QOS) : qos_string_to_qos(IMAGE_QOS);

_rgbd_publisher = _node.create_publisher<realsense2_camera_msgs::msg::RGBD>("rgbd",
_rgbd_publisher = _node.create_publisher<realsense2_camera_msgs::msg::RGBD>("~/rgbd",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos));
}
else {
Expand All @@ -327,7 +327,7 @@ void BaseRealSenseNode::startRGBDPublisherIfNeeded()
}

void BaseRealSenseNode::updateSensors()
{
{
std::lock_guard<std::mutex> lock_guard(_update_sensor_mutex);
try{
for(auto&& sensor : _available_ros_sensors)
Expand All @@ -337,7 +337,7 @@ void BaseRealSenseNode::updateSensors()
std::vector<stream_profile> wanted_profiles;

bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles));
bool is_video_sensor = (sensor->is<rs2::depth_sensor>() || sensor->is<rs2::color_sensor>() || sensor->is<rs2::fisheye_sensor>());
bool is_video_sensor = (sensor->is<rs2::depth_sensor>() || sensor->is<rs2::color_sensor>() || sensor->is<rs2::fisheye_sensor>());

// do all updates if profile has been changed, or if the align depth filter status has been changed
// and we are on a video sensor. TODO: explore better options to monitor and update changes
Expand Down

0 comments on commit c92ddf0

Please sign in to comment.