Skip to content

Commit 2066957

Browse files
adriaroigadriaroig
committed
Merge branch 'multi-realsense-ferrum' into 'ferrum-devel'
Update plugin to support multiple realsense camera's See merge request device/realsense_gazebo_plugin!7
2 parents bd01c2b + 19ef514 commit 2066957

File tree

5 files changed

+77
-75
lines changed

5 files changed

+77
-75
lines changed

‎include/realsense_gazebo_plugin/RealSensePlugin.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,9 @@ class RealSensePlugin : public ModelPlugin {
8686
/// \brief Pointer to the Infrared2 Camera Renderer.
8787
rendering::CameraPtr ired2Cam;
8888

89+
/// \brief String to hold the camera prefix
90+
std::string prefix;
91+
8992
/// \brief Pointer to the transport Node.
9093
transport::NodePtr transportNode;
9194

‎src/RealSensePlugin.cpp‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ RealSensePlugin::RealSensePlugin() {
3434
this->ired1Cam = nullptr;
3535
this->ired2Cam = nullptr;
3636
this->colorCam = nullptr;
37+
this->prefix = "";
3738
}
3839

3940
/////////////////////////////////////////////////
@@ -108,6 +109,8 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
108109
pointCloudTopic_ = _sdf->GetValue()->GetAsString();
109110
else if (name == "pointCloudCutoff")
110111
_sdf->GetValue()->Get(pointCloudCutOff_);
112+
else if (name == "prefix")
113+
this->prefix = _sdf->GetValue()->GetAsString();
111114
else if (name == "robotNamespace")
112115
break;
113116
else
@@ -127,17 +130,17 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
127130

128131
// Get Cameras Renderers
129132
this->depthCam = std::dynamic_pointer_cast<sensors::DepthCameraSensor>(
130-
smanager->GetSensor(DEPTH_CAMERA_NAME))
133+
smanager->GetSensor(prefix + DEPTH_CAMERA_NAME))
131134
->DepthCamera();
132135

133136
this->ired1Cam = std::dynamic_pointer_cast<sensors::CameraSensor>(
134-
smanager->GetSensor(IRED1_CAMERA_NAME))
137+
smanager->GetSensor(prefix + IRED1_CAMERA_NAME))
135138
->Camera();
136139
this->ired2Cam = std::dynamic_pointer_cast<sensors::CameraSensor>(
137-
smanager->GetSensor(IRED2_CAMERA_NAME))
140+
smanager->GetSensor(prefix + IRED2_CAMERA_NAME))
138141
->Camera();
139142
this->colorCam = std::dynamic_pointer_cast<sensors::CameraSensor>(
140-
smanager->GetSensor(COLOR_CAMERA_NAME))
143+
smanager->GetSensor(prefix + COLOR_CAMERA_NAME))
141144
->Camera();
142145

143146
// Check if camera renderers have been found successfuly

‎src/gazebo_ros_realsense.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -31,11 +31,11 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
3131

3232
RealSensePlugin::Load(_model, _sdf);
3333

34-
this->rosnode_ = new ros::NodeHandle("/realsense");
34+
this->rosnode_ = new ros::NodeHandle(this->GetHandle());
3535

3636
// initialize camera_info_manager
3737
this->camera_info_manager_.reset(
38-
new camera_info_manager::CameraInfoManager(*this->rosnode_, "realsense"));
38+
new camera_info_manager::CameraInfoManager(*this->rosnode_, this->GetHandle()));
3939

4040
this->itnode_ = new image_transport::ImageTransport(*this->rosnode_);
4141

‎urdf/d435.gazebo.xacro‎

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,10 @@ This is the Gazebo URDF model for the Intel RealSense D435 camera
99

1010
<robot xmlns:xacro="http://ros.org/wiki/xacro">
1111

12-
<xacro:macro name="simulation_d435" params="camera_name depth_optical_frame color_optical_frame infrared1_optical_frame infrared2_optical_frame" >
12+
<xacro:macro name="simulation_d435" params="camera_name reference_link topics_ns depth_optical_frame color_optical_frame infrared1_optical_frame infrared2_optical_frame" >
1313

1414
<!-- Load parameters to model's main link-->
15-
<gazebo reference="camera_link">
15+
<gazebo reference="${reference_link}">
1616
<self_collide>0</self_collide>
1717
<enable_wind>0</enable_wind>
1818
<kinematic>0</kinematic>
@@ -26,7 +26,7 @@ This is the Gazebo URDF model for the Intel RealSense D435 camera
2626
<kd>1</kd>
2727
<!--<max_vel>0.01</max_vel>
2828
<min_depth>0</min_depth>-->
29-
<sensor name="color" type="camera">
29+
<sensor name="${camera_name}color" type="camera">
3030
<camera name="${camera_name}">
3131
<horizontal_fov>${69.4*deg_to_rad}</horizontal_fov>
3232
<image>
@@ -48,7 +48,7 @@ This is the Gazebo URDF model for the Intel RealSense D435 camera
4848
<update_rate>30</update_rate>
4949
<visualize>1</visualize>
5050
</sensor>
51-
<sensor name="ired1" type="camera">
51+
<sensor name="${camera_name}ired1" type="camera">
5252
<camera name="${camera_name}">
5353
<horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
5454
<image>
@@ -70,7 +70,7 @@ This is the Gazebo URDF model for the Intel RealSense D435 camera
7070
<update_rate>90</update_rate>
7171
<visualize>0</visualize>
7272
</sensor>
73-
<sensor name="ired2" type="camera">
73+
<sensor name="${camera_name}ired2" type="camera">
7474
<camera name="${camera_name}">
7575
<horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
7676
<image>
@@ -92,7 +92,7 @@ This is the Gazebo URDF model for the Intel RealSense D435 camera
9292
<update_rate>90</update_rate>
9393
<visualize>0</visualize>
9494
</sensor>
95-
<sensor name="depth" type="depth">
95+
<sensor name="${camera_name}depth" type="depth">
9696
<camera name="${camera_name}">
9797
<horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
9898
<image>
@@ -116,7 +116,8 @@ This is the Gazebo URDF model for the Intel RealSense D435 camera
116116
</gazebo>
117117

118118
<gazebo>
119-
<plugin name="realsense_plugin" filename="librealsense_gazebo_plugin.so">
119+
<plugin name="${topics_ns}" filename="librealsense_gazebo_plugin.so">
120+
<prefix>${camera_name}</prefix>
120121
<depthUpdateRate>60.0</depthUpdateRate>
121122
<colorUpdateRate>60.0</colorUpdateRate>
122123
<infraredUpdateRate>60.0</infraredUpdateRate>

‎urdf/d435.urdf.xacro‎

Lines changed: 57 additions & 62 deletions
Original file line numberDiff line numberDiff line change
@@ -8,11 +8,11 @@ This is the URDF model for the Intel RealSense 430 camera, in it's
88
aluminum peripherial evaluation case.
99
-->
1010

11-
<robot xmlns:xacro="http://ros.org/wiki/xacro">
11+
<robot name="sensor_d435_gazebo" xmlns:xacro="http://ros.org/wiki/xacro">
12+
<!--File includes-->
13+
<xacro:include filename="$(find realsense_gazebo_plugin)/urdf/d435.gazebo.xacro"/>
1214

13-
<xacro:include filename="$(find realsense_gazebo_plugin)/urdf/d435.gazebo.xacro"/>
14-
15-
<xacro:macro name="sensor_d435" params="parent *origin">
15+
<xacro:macro name="sensor_d435_gazebo" params="parent name:=camera topics_ns:=camera *origin">
1616
<xacro:property name="M_PI" value="3.1415926535897931" />
1717

1818
<!-- The following values are approximate, and the camera node
@@ -35,28 +35,38 @@ aluminum peripherial evaluation case.
3535
<xacro:property name="d435_cam_depth_py" value="0.0175"/>
3636
<xacro:property name="d435_cam_depth_pz" value="${d435_cam_height/2}"/>
3737

38-
<material name="aluminum">
38+
<material name="${name}_aluminum">
3939
<color rgba="0.5 0.5 0.5 1"/>
4040
</material>
4141

4242

4343
<!-- camera body, with origin at bottom screw mount -->
44-
<joint name="camera_joint" type="fixed">
44+
<joint name="${name}_joint" type="fixed">
4545
<xacro:insert_block name="origin" />
4646
<parent link="${parent}"/>
47-
<child link="camera_link" />
47+
<child link="${name}_bottom_screw_frame" />
48+
</joint>
49+
<link name="${name}_bottom_screw_frame"/>
50+
51+
<joint name="${name}_link_joint" type="fixed">
52+
<origin xyz="0 ${d435_cam_depth_py} ${d435_cam_depth_pz}" rpy="0 0 0"/>
53+
<parent link="${name}_bottom_screw_frame"/>
54+
<child link="${name}_link" />
4855
</joint>
4956

50-
<link name="camera_link">
57+
<link name="${name}_link">
5158
<visual>
52-
<origin xyz="${d435_cam_mount_from_center_offset} 0.0 ${d435_cam_height/2}" rpy="${M_PI/2} 0 ${M_PI/2}"/>
59+
<origin xyz="${d435_cam_mount_from_center_offset} ${-d435_cam_depth_py} 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
5360
<geometry>
54-
<mesh filename="package://realsense_gazebo_plugin/meshes/d435.dae" />
61+
<!-- <box size="${d435_cam_width} ${d435_cam_height} ${d435_cam_depth}"/> -->
62+
<mesh filename="package://realsense2_camera/meshes/d435.dae" />
63+
<!--<mesh filename="package://realsense2_camera/meshes/d435/d435.dae" />-->
64+
5565
</geometry>
56-
<material name="aluminum"/>
66+
<material name="${name}_aluminum"/>
5767
</visual>
5868
<collision>
59-
<origin xyz="0.0 0.0 ${d435_cam_height/2}" rpy="0 0 0"/>
69+
<origin xyz="0 ${-d435_cam_depth_py} 0" rpy="0 0 0"/>
6070
<geometry>
6171
<box size="${d435_cam_depth} ${d435_cam_width} ${d435_cam_height}"/>
6272
</geometry>
@@ -70,82 +80,67 @@ aluminum peripherial evaluation case.
7080
</link>
7181

7282
<!-- camera depth joints and links -->
73-
<joint name="camera_depth_joint" type="fixed">
74-
<origin xyz="${d435_cam_depth_px} ${d435_cam_depth_py} ${d435_cam_depth_pz}" rpy="0 0 0"/>
75-
<parent link="camera_link"/>
76-
<child link="camera_depth_frame" />
83+
<joint name="${name}_depth_joint" type="fixed">
84+
<origin xyz="0 0 0" rpy="0 0 0"/>
85+
<parent link="${name}_link"/>
86+
<child link="${name}_depth_frame" />
7787
</joint>
78-
<link name="camera_depth_frame"/>
88+
<link name="${name}_depth_frame"/>
7989

80-
<joint name="camera_depth_optical_joint" type="fixed">
90+
<joint name="${name}_depth_optical_joint" type="fixed">
8191
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
82-
<parent link="camera_depth_frame" />
83-
<child link="camera_depth_optical_frame" />
92+
<parent link="${name}_depth_frame" />
93+
<child link="${name}_depth_optical_frame" />
8494
</joint>
85-
<link name="camera_depth_optical_frame"/>
95+
<link name="${name}_depth_optical_frame"/>
8696

8797
<!-- camera left IR joints and links -->
88-
<joint name="camera_left_ir_joint" type="fixed">
98+
<joint name="${name}_left_ir_joint" type="fixed">
8999
<origin xyz="0 ${d435_cam_depth_to_left_ir_offset} 0" rpy="0 0 0" />
90-
<parent link="camera_depth_frame" />
91-
<child link="camera_left_ir_frame" />
100+
<parent link="${name}_depth_frame" />
101+
<child link="${name}_left_ir_frame" />
92102
</joint>
93-
<link name="camera_left_ir_frame"/>
103+
<link name="${name}_left_ir_frame"/>
94104

95-
<joint name="camera_left_ir_optical_joint" type="fixed">
105+
<joint name="${name}_left_ir_optical_joint" type="fixed">
96106
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
97-
<parent link="camera_left_ir_frame" />
98-
<child link="camera_left_ir_optical_frame" />
107+
<parent link="${name}_left_ir_frame" />
108+
<child link="${name}_left_ir_optical_frame" />
99109
</joint>
100-
<link name="camera_left_ir_optical_frame"/>
110+
<link name="${name}_left_ir_optical_frame"/>
101111

102112
<!-- camera right IR joints and links -->
103-
<joint name="camera_right_ir_joint" type="fixed">
113+
<joint name="${name}_right_ir_joint" type="fixed">
104114
<origin xyz="0 ${d435_cam_depth_to_right_ir_offset} 0" rpy="0 0 0" />
105-
<parent link="camera_depth_frame" />
106-
<child link="camera_right_ir_frame" />
115+
<parent link="${name}_depth_frame" />
116+
<child link="${name}_right_ir_frame" />
107117
</joint>
108-
<link name="camera_right_ir_frame"/>
118+
<link name="${name}_right_ir_frame"/>
109119

110-
<joint name="camera_right_ir_optical_joint" type="fixed">
120+
<joint name="${name}_right_ir_optical_joint" type="fixed">
111121
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
112-
<parent link="camera_right_ir_frame" />
113-
<child link="camera_right_ir_optical_frame" />
122+
<parent link="${name}_right_ir_frame" />
123+
<child link="${name}_right_ir_optical_frame" />
114124
</joint>
115-
<link name="camera_right_ir_optical_frame"/>
125+
<link name="${name}_right_ir_optical_frame"/>
116126

117127
<!-- camera color joints and links -->
118-
<joint name="camera_color_joint" type="fixed">
128+
<joint name="${name}_color_joint" type="fixed">
119129
<origin xyz="0 ${d435_cam_depth_to_color_offset} 0" rpy="0 0 0" />
120-
<parent link="camera_depth_frame" />
121-
<child link="camera_color_frame" />
130+
<parent link="${name}_depth_frame" />
131+
<child link="${name}_color_frame" />
122132
</joint>
123-
<link name="camera_color_frame">
124-
<inertial>
125-
<mass value="0.01" />
126-
<origin xyz="0 0 0" rpy="0 0 0"/>
127-
<inertia ixx="0.0" ixy="0.0" ixz="0.0"
128-
iyy="0.0" iyz="0.0"
129-
izz="0.0" />
130-
</inertial>
131-
</link>
133+
<link name="${name}_color_frame"/>
132134

133-
<joint name="camera_color_optical_joint" type="fixed">
135+
<joint name="${name}_color_optical_joint" type="fixed">
134136
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
135-
<parent link="camera_color_frame" />
136-
<child link="camera_color_optical_frame" />
137+
<parent link="${name}_color_frame" />
138+
<child link="${name}_color_optical_frame" />
137139
</joint>
138-
<link name="camera_color_optical_frame">
139-
<inertial>
140-
<mass value="0.01" />
141-
<origin xyz="0 0 0" rpy="0 0 0"/>
142-
<inertia ixx="0.0" ixy="0.0" ixz="0.0"
143-
iyy="0.0" iyz="0.0"
144-
izz="0.0" />
145-
</inertial>
146-
</link>
140+
<link name="${name}_color_optical_frame"/>
147141

148-
<xacro:simulation_d435 camera_name="realsense" depth_optical_frame="camera_depth_optical_frame" color_optical_frame="camera_color_optical_frame" infrared1_optical_frame="camera_left_ir_optical_frame" infrared2_optical_frame="camera_right_ir_optical_frame"/>
142+
<!-- Realsense Gazebo Plugin -->
143+
<xacro:simulation_d435 camera_name="${name}" reference_link="${name}_link" topics_ns="${topics_ns}" depth_optical_frame="${name}_depth_optical_frame" color_optical_frame="${name}_color_optical_frame" infrared1_optical_frame="${name}_left_ir_optical_frame" infrared2_optical_frame="${name}_right_ir_optical_frame"/>
149144

150145
</xacro:macro>
151146
</robot>

0 commit comments

Comments
 (0)