Skip to content

Commit

Permalink
PR IntelRealSense#2850 from SamerKhshiboun: Merge ros2-development in…
Browse files Browse the repository at this point in the history 
…to ros2-hkr
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun authored Aug 20, 2023
2 parents b49111f + 29b2ead commit 18a1149
Show file tree
Hide file tree
Showing 4 changed files with 155 additions and 60 deletions.
22 changes: 13 additions & 9 deletions realsense2_camera/include/base_realsense_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -207,24 +207,23 @@ namespace realsense2_camera
void publishLabeledPointCloud(rs2::labeled_points points, const rclcpp::Time& t);
bool shouldPublishCameraInfo(const stream_index_pair& sip);
Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const;

IMUInfo getImuInfo(const rs2::stream_profile& profile);

void fillMessageImage(
void initializeFormatsMaps();

bool fillROSImageMsgAndReturnStatus(
const cv::Mat& cv_matrix_image,
const stream_index_pair& stream,
unsigned int width,
unsigned int height,
unsigned int bpp,
const rs2_format& stream_format,
const rclcpp::Time& t,
sensor_msgs::msg::Image* img_msg_ptr);

cv::Mat& getCVMatImage(
bool fillCVMatImageAndReturnStatus(
rs2::frame& frame,
std::map<stream_index_pair, cv::Mat>& images,
unsigned int width,
unsigned int height,
unsigned int bpp,
const stream_index_pair& stream);

void publishFrame(
Expand All @@ -236,7 +235,12 @@ namespace realsense2_camera
const std::map<stream_index_pair, std::shared_ptr<image_publisher>>& image_publishers,
const bool is_publishMetadata = true);

void publishRGBD(const cv::Mat& rgb_cv_matrix, const cv::Mat& depth_cv_matrix, const rclcpp::Time& t);
void publishRGBD(
const cv::Mat& rgb_cv_matrix,
const rs2_format& color_format,
const cv::Mat& depth_cv_matrix,
const rs2_format& depth_format,
const rclcpp::Time& t);

void publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id);

Expand Down Expand Up @@ -295,14 +299,14 @@ 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_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_publishers;
std::map<stream_index_pair, rclcpp::Publisher<Extrinsics>::SharedPtr> _extrinsics_publishers;
rclcpp::Publisher<realsense2_camera_msgs::msg::RGBD>::SharedPtr _rgbd_publisher;
std::map<stream_index_pair, cv::Mat> _images;
std::map<unsigned int, std::string> _encoding;
std::map<rs2_format, std::string> _rs_format_to_ros_format;
std::map<rs2_format, int> _rs_format_to_cv_format;

std::map<stream_index_pair, sensor_msgs::msg::CameraInfo> _camera_info;
std::atomic_bool _is_initialized_time_base;
Expand Down
19 changes: 10 additions & 9 deletions realsense2_camera/launch/rs_launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@


configurable_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'},
{'name': 'camera_namespace', 'default': 'camera', 'description': 'namespace for camera'},
{'name': 'serial_no', 'default': "''", 'description': 'choose device by serial number'},
{'name': 'usb_port_id', 'default': "''", 'description': 'choose device by usb port id'},
{'name': 'device_type', 'default': "''", 'description': 'choose device by type'},
Expand All @@ -30,7 +31,7 @@
{'name': 'json_file_path', 'default': "''", 'description': 'allows advanced configuration'},
{'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
{'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'},
{'name': 'depth_module.profile', 'default': '0,0,0', 'description': 'depth module profile'},
{'name': 'depth_module.profile', 'default': '0,0,0', 'description': 'depth module profile'},
{'name': 'depth_module.depth_format', 'default': 'Z16', 'description': 'depth stream format'},
{'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'},
{'name': 'depth_module.infra1_format', 'default': 'Y8', 'description': 'infra1 stream format'},
Expand Down Expand Up @@ -59,15 +60,15 @@
{'name': 'pointcloud.enable', 'default': 'false', 'description': ''},
{'name': 'pointcloud.stream_filter', 'default': '2', 'description': 'texture stream for pointcloud'},
{'name': 'pointcloud.stream_index_filter','default': '0', 'description': 'texture stream index for pointcloud'},
{'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''},
{'name': 'enable_sync', 'default': 'false', 'description': "'enable sync mode'"},
{'name': 'enable_rgbd', 'default': 'false', 'description': "'enable rgbd topic'"},
{'name': 'align_depth.enable', 'default': 'false', 'description': "'enable align depth filter'"},
{'name': 'colorizer.enable', 'default': 'false', 'description': "''"},
{'name': 'clip_distance', 'default': '-2.', 'description': "''"},
{'name': 'linear_accel_cov', 'default': '0.01', 'description': "''"},
{'name': 'initial_reset', 'default': 'false', 'description': "''"},
{'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"},
{'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''},
{'name': 'clip_distance', 'default': '-2.', 'description': "''"},
{'name': 'linear_accel_cov', 'default': '0.01', 'description': "''"},
{'name': 'initial_reset', 'default': 'false', 'description': "''"},
{'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"},
{'name': 'publish_tf', 'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'},
{'name': 'tf_publish_rate', 'default': '0.0', 'description': '[double] rate in Hz for publishing dynamic TF'},
{'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'},
Expand Down Expand Up @@ -103,7 +104,7 @@ def launch_setup(context, params, param_name_suffix=''):
return [
launch_ros.actions.Node(
package='realsense2_camera',
node_namespace=LaunchConfiguration('camera_name' + param_name_suffix),
node_namespace=LaunchConfiguration('camera_namespace' + param_name_suffix),
node_name=LaunchConfiguration('camera_name' + param_name_suffix),
node_executable='realsense2_camera_node',
prefix=['stdbuf -o L'],
Expand All @@ -118,7 +119,7 @@ def launch_setup(context, params, param_name_suffix=''):
return [
launch_ros.actions.Node(
package='realsense2_camera',
namespace=LaunchConfiguration('camera_name' + param_name_suffix),
namespace=LaunchConfiguration('camera_namespace' + param_name_suffix),
name=LaunchConfiguration('camera_name' + param_name_suffix),
executable='realsense2_camera_node',
parameters=[params
Expand All @@ -129,7 +130,7 @@ def launch_setup(context, params, param_name_suffix=''):
emulate_tty=True,
)
]

def generate_launch_description():
return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [
OpaqueFunction(function=launch_setup, kwargs = {'params' : set_configurable_parameters(configurable_parameters)})
Expand Down
14 changes: 8 additions & 6 deletions realsense2_camera/launch/rs_multi_camera_launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,11 @@
sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
import rs_launch

local_parameters = [{'name': 'camera_name1', 'default': 'camera1', 'description': 'camera unique name'},
{'name': 'camera_name2', 'default': 'camera2', 'description': 'camera unique name'},
]
local_parameters = [{'name': 'camera_name1', 'default': 'camera1', 'description': 'camera1 unique name'},
{'name': 'camera_name2', 'default': 'camera2', 'description': 'camera2 unique name'},
{'name': 'camera_namespace1', 'default': 'camera1', 'description': 'camera1 namespace'},
{'name': 'camera_namespace2', 'default': 'camera2', 'description': 'camera2 namespace'},
]

def set_configurable_parameters(local_params):
return dict([(param['original_name'], LaunchConfiguration(param['name'])) for param in local_params])
Expand All @@ -46,7 +48,7 @@ def duplicate_params(general_params, posix):
param['original_name'] = param['name']
param['name'] += posix
return local_params

def add_node_action(context : LaunchContext):
# dummy static transformation from camera1 to camera2
node = launch_ros.actions.Node(
Expand All @@ -63,8 +65,8 @@ def generate_launch_description():
params2 = duplicate_params(rs_launch.configurable_parameters, '2')
return LaunchDescription(
rs_launch.declare_configurable_parameters(local_parameters) +
rs_launch.declare_configurable_parameters(params1) +
rs_launch.declare_configurable_parameters(params2) +
rs_launch.declare_configurable_parameters(params1) +
rs_launch.declare_configurable_parameters(params2) +
[
OpaqueFunction(function=rs_launch.launch_setup,
kwargs = {'params' : set_configurable_parameters(params1),
Expand Down
Loading

0 comments on commit 18a1149

Please sign in to comment.