diff --git a/config/realsense_d435_default_config.yaml b/config/realsense_d435_default_config.yaml
index 1ed73e9265d5da5a3698f6cf152435d45ed98146..0fe19f4d72f180ae3a5212277e01ac5cb36afed3 100644
--- a/config/realsense_d435_default_config.yaml
+++ b/config/realsense_d435_default_config.yaml
@@ -1,19 +1,6 @@
-manager: 'realsense2_camera_manager'
serial_no: ''
-tf_prefix: ''
json_file_path: ''
rosbag_filename: ''
-depth: 'depth'
-infra1: 'infra1'
-infra2: 'infra2'
-rgb: 'color'
-fisheye: 'fisheye'
-accel: 'accel'
-gyro: 'gyro'
-
-fisheye_width: 640
-fisheye_height: 480
-enable_fisheye: true
depth_width: 640
depth_height: 480
@@ -31,13 +18,10 @@ color_width: 640
color_height: 480
enable_color: true
-fisheye_fps: 30
depth_fps: 30
infra1_fps: 30
infra2_fps: 30
color_fps: 30
-gyro_fps: 1000
-accel_fps: 1000
enable_imu: true
enable_pointcloud: false
@@ -45,26 +29,6 @@ pointcloud_texture_stream: 'RS2_STREAM_COLOR'
pointcloud_texture_index: 0
enable_sync: false
-align_depth: false
-
-base_frame_id: camera_link
-depth_frame_id: camera_depth_frame
-infra1_frame_id: camera_infra1_frame
-infra2_frame_id: camera_infra2_frame
-color_frame_id: camera_color_frame
-
-depth_optical_frame_id: camera_depth_optical_frame
-infra1_optical_frame_id: camera_infra1_optical_frame
-infra2_optical_frame_id: camera_infra2_optical_frame
-color_optical_frame_id: camera_color_optical_frame
-fisheye_optical_frame_id: camera_fisheye_optical_frame
-accel_optical_frame_id: camera_accel_optical_frame
-gyro_optical_frame_id: camera_gyro_optical_frame
+align_depth: true
-aligned_depth_to_color_frame_id: camera_aligned_depth_to_color_frame
-aligned_depth_to_infra1_frame_id: camera_aligned_depth_to_infra1_frame
-aligned_depth_to_infra2_frame_id: camera_aligned_depth_to_infra2_frame
-aligned_depth_to_fisheye_frame_id: camera_aligned_depth_to_fisheye_frame
-
filters: ''
-
diff --git a/launch/realsense.launch b/launch/realsense.launch
index 96af79e4feddbaf11505052809406fe0bec2fa5b..9fb636b05e942357b58df56f7dd5bf6c9f8d1629 100644
--- a/launch/realsense.launch
+++ b/launch/realsense.launch
@@ -1,10 +1,48 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="config_file" default="$(find iri_bno055_imu_bringup)/config/bno055_default_config.yaml" />
+ <arg name="config_file" default="$(find iri_realsense_depth_bringup)/config/realsense_d435_default_config.yaml" />
+ <arg name="camera_name" default="camera"/>
+ <arg name="tf_prefix" default=""/>
+ <arg name="output" default="log"/>
+ <arg name="launch_prefix" default=""/>
+ <arg name="manager" default="$(arg camera_name)_manager"/>
+
+ <group ns="$(arg camera_name)">
+ <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="$(arg output)" launch-prefix="$(arg launch_prefix)"/>
+ <node pkg="nodelet" type="nodelet" name="realsense2_camera" args="load realsense2_camera/RealSenseNodeFactory $(arg manager)">
+ <rosparam file="$(arg config_file)" command="load" />
+ <param name="base_frame_id" type="str" value="$(arg tf_prefix)/camera_link"/>
+ <param name="depth_frame_id" type="str" value="$(arg tf_prefix)/camera_depth_frame"/>
+ <param name="infra1_frame_id" type="str" value="$(arg tf_prefix)/camera_infra1_frame"/>
+ <param name="infra2_frame_id" type="str" value="$(arg tf_prefix)/camera_infra2_frame"/>
+ <param name="color_frame_id" type="str" value="$(arg tf_prefix)/camera_color_frame"/>
+
+ <param name="depth_optical_frame_id" type="str" value="$(arg tf_prefix)/camera_depth_optical_frame"/>
+ <param name="infra1_optical_frame_id" type="str" value="$(arg tf_prefix)/camera_infra1_optical_frame"/>
+ <param name="infra2_optical_frame_id" type="str" value="$(arg tf_prefix)/camera_infra2_optical_frame"/>
+ <param name="color_optical_frame_id" type="str" value="$(arg tf_prefix)/camera_color_optical_frame"/>
+
+ <param name="aligned_depth_to_color_frame_id" type="str" value="$(arg tf_prefix)/camera_aligned_depth_to_color_frame"/>
+ <param name="aligned_depth_to_infra1_frame_id" type="str" value="$(arg tf_prefix)/camera_aligned_depth_to_infra1_frame"/>
+ <param name="aligned_depth_to_infra2_frame_id" type="str" value="$(arg tf_prefix)/camera_aligned_depth_to_infra2_frame"/>
+ </node>
+
+ <node pkg="nodelet" type="nodelet" name="color_rectify_color"
+ args="load image_proc/rectify $(arg manager)" respawn="false">
+ <remap from="image_mono" to="color/image_raw" />
+ <remap from="image_rect" to="color/image_rect_color" />
+ </node>
+
+ <!-- Publish registered XYZRGB point cloud with hardware registered input (ROS Realsense depth alignment) -->
+ <node pkg="nodelet" type="nodelet" name="points_xyzrgb_hw_registered"
+ args="load depth_image_proc/point_cloud_xyzrgb $(arg manager)" respawn="false">
+ <remap from="rgb/image_rect_color" to="color/image_rect_color" />
+ <remap from="rgb/camera_info" to="color/camera_info" />
+ <remap from="depth_registered/image_rect" to="aligned_depth_to_color/image_raw" />
+ <remap from="depth_registered/points" to="depth_registered/points" />
+ </node>
+ </group>
- <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
- <rosparam file="$(arg config_file)" command="load" />
- </include>
</launch>
diff --git a/launch/realsense_test.launch b/launch/realsense_test.launch
index d42812ab27421175ab0523902e6096f26cddac0e..98a05904bb41e3091e728056571568c10d6e2ab3 100644
--- a/launch/realsense_test.launch
+++ b/launch/realsense_test.launch
@@ -1,19 +1,21 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="config_file" default="$(find iri_bno055_imu_bringup)/config/bno055_default_config.yaml" />
+ <arg name="config_file" default="$(find iri_realsense_depth_bringup)/config/realsense_d435_default_config.yaml" />
+ <arg name="camera_name" default="camera"/>
<param name="robot_description"
command="$(find xacro)/xacro --inorder '$(find realsense2_camera)/urdf/test_d435_camera.urdf.xacro'" />
<include file="$(find iri_realsense_depth_bringup)/launch/realsense.launch">
- <arg name="config_file" value="$(arg config_file)"/>
+ <arg name="config_file" value="$(arg config_file)"/>
+ <arg name="camera_name" value="$(arg camera_name)"/>
</include>
<node name="rviz"
pkg="rviz"
type="rviz"
- args="-d $(find iri_bno055_imu_bringup)/rviz/bno055_imu.rviz"/>
+ args="-d $(find iri_realsense_depth_bringup)/rviz/realsense_depth.rviz"/>
</launch>