Skip to content

Commit

Permalink
Feat yaml params (SDK v2.8) (stereolabs#362)
Browse files Browse the repository at this point in the history
* Moved parameters from launch files to YAML files. This makes them clearer and unifies parameters between different launch files.
  • Loading branch information
Myzhar authored and adujardin committed Apr 19, 2019
1 parent 111725a commit de035d3
Show file tree
Hide file tree
Showing 39 changed files with 2,296 additions and 1,450 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -17,40 +17,22 @@ THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<launch>
<arg name="zed_namespace" default="zed" />
<arg name="zed_namespace" default="zed" />

<arg name="svo_file" default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->

<arg name="camera_model" default="1" />
<arg name="serial_number" default="0" />
<arg name="verbose" default="true" />
<arg name="resolution" default="2" /> <!--0=RESOLUTION_HD2K, 1=RESOLUTION_HD1080, 2=RESOLUTION_HD720, 3=RESOLUTION_VGA -->
<arg name="frame_rate" default="30" />
<!-- RESOLUTION_HD2K -> 2208*1242, available framerates: 15 fps.
RESOLUTION_HD1080 -> 1920*1080, available framerates: 15, 30 fps.
RESOLUTION_HD720 -> 1280*720, available framerates: 15, 30, 60 fps.
RESOLUTION_VGA -> 672*376, available framerates: 15, 30, 60, 100 fps. -->

<arg name="cam_image_topic" default="rgb/image_rect_color" />
<arg name="cam_info_topic" default="rgb/camera_info" />
<arg name="camera_frame" default="zed_left_camera_frame" />
<arg name="zed_node_name" default="zed_node" />
<arg name="camera_model" default="zedm" />

<group ns="$(arg zed_namespace)">
<!-- Pre configured RVIZ -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find zed_ar_track_alvar_example)/rviz/zed_ar_track_alvar_example.rviz" output="screen"/>

<!-- ZED Wrapper Node-->
<include file="$(find zed_wrapper)/launch/zed_camera.launch">
<arg name="svo_file" value="$(arg svo_file)" />
<arg name="camera_model" value="$(arg camera_model)" />
<arg name="serial_number" value="$(arg serial_number)" />
<arg name="resolution" value="$(arg resolution)" />
<arg name="frame_rate" value="$(arg frame_rate)" />
<arg name="verbose" value="$(arg verbose)" />

<arg name="rgb_topic" value="$(arg cam_image_topic)" />
<arg name="rgb_info_topic" value="$(arg cam_info_topic)" />
<arg name="left_camera_frame" value="$(arg camera_frame)" />
<arg name="svo_file" value="$(arg svo_file)" />
<arg name="node_name" value="$(arg zed_node_name)" />
<arg name="camera_model" value="$(arg camera_model)" />
</include>

<!-- ar_track_alvar Node -->
Expand All @@ -62,10 +44,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<param name="marker_size" type="double" value="$(arg marker_size)" />
<param name="max_new_marker_error" type="double" value="$(arg max_new_marker_error)" />
<param name="max_track_error" type="double" value="$(arg max_track_error)" />
<param name="output_frame" type="string" value="$(arg camera_frame)" />
<param name="output_frame" type="string" value="zed_left_camera_frame" />

<remap from="camera_image" to="$(arg cam_image_topic)" />
<remap from="camera_info" to="$(arg cam_info_topic)" />
<remap from="camera_image" to="$(arg zed_node_name)/rgb/image_rect_color" />
<remap from="camera_info" to="$(arg zed_node_name)/rgb/camera_info" />
</node>
</group>
</launch>
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,9 @@ Panels:
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1
- /Camera Model1/Status1
- /TF1/Frames1
- /Left Camera1/Visibility1
- /Pose1
- /Tags1
- /Left Image1
Splitter Ratio: 0.483870953
Tree Height: 853
- Class: rviz/Selection
Expand Down Expand Up @@ -63,7 +60,7 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
imu_link:
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Expand All @@ -72,6 +69,10 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
zed_imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
zed_left_camera_frame:
Alpha: 1
Show Axes: false
Expand Down Expand Up @@ -103,33 +104,25 @@ Visualization Manager:
Value: true
ar_marker_1:
Value: true
ar_marker_14:
Value: true
ar_marker_144:
Value: true
ar_marker_192:
Value: true
ar_marker_2:
Value: true
ar_marker_3:
Value: true
ar_marker_4:
Value: true
ar_marker_48:
Value: true
ar_marker_5:
Value: true
ar_marker_6:
Value: true
ar_marker_64:
Value: true
ar_marker_7:
Value: true
ar_marker_8:
Value: true
ar_marker_80:
ar_marker_96:
Value: true
ar_marker_9:
base_link:
Value: true
map:
Value: true
Expand All @@ -153,50 +146,41 @@ Visualization Manager:
Tree:
map:
odom:
zed_camera_center:
zed_left_camera_frame:
zed_left_camera_optical_frame:
ar_marker_0:
{}
ar_marker_1:
{}
ar_marker_14:
{}
ar_marker_144:
{}
ar_marker_192:
{}
ar_marker_2:
{}
ar_marker_3:
{}
ar_marker_4:
{}
ar_marker_48:
{}
ar_marker_5:
{}
ar_marker_6:
{}
ar_marker_64:
{}
ar_marker_7:
{}
ar_marker_8:
{}
ar_marker_80:
{}
ar_marker_9:
base_link:
zed_camera_center:
zed_left_camera_frame:
zed_left_camera_optical_frame:
ar_marker_0:
{}
ar_marker_1:
{}
ar_marker_144:
{}
ar_marker_2:
{}
ar_marker_3:
{}
ar_marker_4:
{}
ar_marker_5:
{}
ar_marker_6:
{}
ar_marker_7:
{}
ar_marker_8:
{}
ar_marker_96:
{}
zed_right_camera_frame:
zed_right_camera_optical_frame:
{}
zed_right_camera_frame:
zed_right_camera_optical_frame:
{}
Update Interval: 0
Value: true
- Class: rviz/Camera
Enabled: true
Image Rendering: background and overlay
Image Topic: /zed/left/image_rect_color
Image Topic: /zed/zed_node/left/image_rect_color
Name: Left Camera
Overlay Alpha: 0.25
Queue Size: 1
Expand Down Expand Up @@ -224,7 +208,7 @@ Visualization Manager:
Shaft Length: 1
Shaft Radius: 0.0500000007
Shape: Axes
Topic: /zed/pose
Topic: /zed/zed_node/pose
Unreliable: false
Value: true
- Class: rviz/Marker
Expand All @@ -237,7 +221,7 @@ Visualization Manager:
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /zed/left/image_rect_color
Image Topic: /zed/zed_node/left/image_rect_color
Max Value: 1
Median window: 5
Min Value: 0
Expand Down Expand Up @@ -312,5 +296,5 @@ Window Geometry:
Views:
collapsed: false
Width: 1855
X: 1985
X: 65
Y: 24
29 changes: 7 additions & 22 deletions examples/zed_nodelet_example/launch/zed_nodelet_laserscan.launch
Original file line number Diff line number Diff line change
Expand Up @@ -19,22 +19,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<launch>
<arg name="svo_file" default=""/>

<!-- Coordinate frame -->
<arg name="camera_model" default="0" /> <!-- 0=ZED, 1=ZEDM-->
<arg name="serial_number" default="0" />
<arg name="verbose" default="true" />

<arg name="left_camera_frame" default="zed_left_camera_frame" />
<arg name="depth_topic" default="depth/depth_registered" />

<arg name="resolution" default="2" /> <!--0=RESOLUTION_HD2K, 1=RESOLUTION_HD1080, 2=RESOLUTION_HD720, 3=RESOLUTION_VGA -->
<arg name="frame_rate" default="30" />
<!-- RESOLUTION_HD2K -> 2208*1242, available framerates: 15 fps.
RESOLUTION_HD1080 -> 1920*1080, available framerates: 15, 30 fps.
RESOLUTION_HD720 -> 1280*720, available framerates: 15, 30, 60 fps.
RESOLUTION_VGA -> 672*376, available framerates: 15, 30, 60, 100 fps. -->

<arg name="nodelet_manager_name" default="zed_nodelet_manager" />
<arg name="zed_node_name" default="zed_node" />
<arg name="camera_model" default="zedm" /> <!-- 0=ZED, 1=ZEDM-->

<arg name="nodelet_manager_name" default="zed_nodelet_manager" />

<group ns="zed">
<!-- Nodelet Manager -->
Expand All @@ -45,19 +33,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<arg name="svo_file" value="$(arg svo_file)"/>
<arg name="nodelet_manager_name" value="$(arg nodelet_manager_name)" />
<arg name="camera_model" value="$(arg camera_model)" />
<arg name="serial_number" value="$(arg serial_number)" />
<arg name="resolution" value="$(arg resolution)" />
<arg name="frame_rate" value="$(arg frame_rate)" />
<arg name="verbose" value="$(arg verbose)" />
<arg name="node_name" value="$(arg zed_node_name)" />
</include>

<!-- Virtual laser scan as nodelet -->
<!-- "$ sudo apt install ros-kinetic-depthimage-to-laserscan" -->
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet zed_nodelet_manager">
<param name="scan_height" value="10"/>
<param name="output_frame_id" value="$(arg left_camera_frame)"/>
<param name="output_frame_id" value="zed_left_camera_frame"/>
<param name="range_min" value="0.45"/>
<remap from="image" to="$(arg depth_topic)"/>
<remap from="image" to="$(arg zed_node_name)/depth/depth_registered"/>
</node>

</group>
Expand Down
57 changes: 18 additions & 39 deletions examples/zed_rtabmap_example/launch/zed_rtabmap.launch
Original file line number Diff line number Diff line change
Expand Up @@ -19,54 +19,33 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<launch>
<arg name="zed_namespace" default="zed" />

<arg name="svo_file" default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
<arg name="svo_file" default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->

<arg name="camera_model" default="1" /> <!-- 0=ZED, 1=ZEDM-->
<arg name="serial_number" default="0" />
<arg name="verbose" default="true" />
<arg name="resolution" default="2" /> <!--0=RESOLUTION_HD2K, 1=RESOLUTION_HD1080, 2=RESOLUTION_HD720, 3=RESOLUTION_VGA -->
<arg name="frame_rate" default="30" />
<!-- RESOLUTION_HD2K -> 2208*1242, available framerates: 15 fps.
RESOLUTION_HD1080 -> 1920*1080, available framerates: 15, 30 fps.
RESOLUTION_HD720 -> 1280*720, available framerates: 15, 30, 60 fps.
RESOLUTION_VGA -> 672*376, available framerates: 15, 30, 60, 100 fps. -->

<arg name="rgb_topic" default="rgb/image_rect_color" />
<arg name="depth_topic" default="depth/depth_registered" />
<arg name="camera_info_topic" default="rgb/camera_info" />
<arg name="depth_camera_info_topic" default="depth/camera_info" />
<arg name="camera_frame" default="zed_camera_center" />
<arg name="zed_node_name" default="zed_node" />
<arg name="camera_model" default="zedm" /> <!-- 'zed' or 'zedm' -->
<arg name="publish_urdf" default="true" />

<group ns="$(arg zed_namespace)">
<!-- ZED Wrapper Node-->
<include file="$(find zed_wrapper)/launch/zed_camera.launch">
<arg name="svo_file" value="$(arg svo_file)" />
<arg name="camera_model" value="$(arg camera_model)" />
<arg name="serial_number" value="$(arg serial_number)" />
<arg name="resolution" value="$(arg resolution)" />
<arg name="frame_rate" value="$(arg frame_rate)" />
<arg name="verbose" value="$(arg verbose)" />

<arg name="rgb_topic" value="$(arg rgb_topic)" />
<arg name="depth_topic" value="$(arg depth_topic)" />
<arg name="rgb_info_topic" value="$(arg camera_info_topic)" />
<arg name="depth_cam_info_topic" value="$(arg depth_camera_info_topic)" />
<arg name="base_frame" value="$(arg camera_frame)" />

<arg name="publish_map_tf" value="false" />
<arg name="svo_file" value="$(arg svo_file)" />
<arg name="node_name" value="$(arg zed_node_name)" />
<arg name="camera_model" value="$(arg camera_model)" />
<arg name="publish_urdf" value="$(arg publish_urdf)" />
</include>
</group>

<!-- RTAB-map Node-->
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="rtabmap_args" value="--delete_db_on_start" />
<arg name="rgb_topic" value="/$(arg zed_namespace)/$(arg rgb_topic)" />
<arg name="depth_topic" value="/$(arg zed_namespace)/$(arg depth_topic)" />
<arg name="camera_info_topic" value="/$(arg zed_namespace)/$(arg camera_info_topic)" />
<arg name="depth_camera_info_topic" value="/$(arg zed_namespace)/$(arg depth_camera_info_topic)" />
<arg name="frame_id" value="$(arg camera_frame)" />
<arg name="approx_sync" value="false" />
<arg name="visual_odometry" value="false" />
<arg name="odom_topic" value="/$(arg zed_namespace)/odom" />
<arg name="rtabmap_args" value="--delete_db_on_start" />
<arg name="rgb_topic" value="/$(arg zed_namespace)/$(arg zed_node_name)/rgb/image_rect_color" />
<arg name="depth_topic" value="/$(arg zed_namespace)/$(arg zed_node_name)/depth/depth_registered" />
<arg name="camera_info_topic" value="/$(arg zed_namespace)/$(arg zed_node_name)/rgb/camera_info" />
<arg name="depth_camera_info_topic" value="/$(arg zed_namespace)/$(arg zed_node_name)/depth/camera_info" />
<arg name="frame_id" value="base_link" />
<arg name="approx_sync" value="false" />
<arg name="visual_odometry" value="false" />
<arg name="odom_topic" value="/$(arg zed_namespace)/$(arg zed_node_name)/odom" />
<arg name="rviz" value="true" />
</include>
</launch>
Binary file modified images/Picto+STEREOLABS_Black.jpg
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ int main(int argc, char** argv) {
* is the number of messages that will be buffered up before beginning to throw
* away the oldest ones.
*/
ros::Subscriber subDepth = n.subscribe("/zed/depth/depth_registered", 10, depthCallback);
ros::Subscriber subDepth = n.subscribe("/zed/zed_node/depth/depth_registered", 10, depthCallback);


/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -131,8 +131,8 @@ int main(int argc, char** argv) {
* is the number of messages that will be buffered up before beginning to throw
* away the oldest ones.
*/
ros::Subscriber subOdom = n.subscribe("/zed/odom", 10, odomCallback);
ros::Subscriber subPose = n.subscribe("/zed/pose", 10, poseCallback);
ros::Subscriber subOdom = n.subscribe("/zed/zed_node/odom", 10, odomCallback);
ros::Subscriber subPose = n.subscribe("/zed/zed_node/pose", 10, poseCallback);

/**
* ros::spin() will enter a loop, pumping callbacks. With this version, all
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,10 @@ int main(int argc, char** argv) {
* is the number of messages that will be buffered up before beginning to throw
* away the oldest ones.
*/
ros::Subscriber subRightRectified = n.subscribe("/zed/right/image_rect_color", 10, imageRightRectifiedCallback);
ros::Subscriber subLeftRectified = n.subscribe("/zed/left/image_rect_color", 10, imageLeftRectifiedCallback);
ros::Subscriber subRightRectified = n.subscribe("/zed/zed_node/right/image_rect_color", 10,
imageRightRectifiedCallback);
ros::Subscriber subLeftRectified = n.subscribe("/zed/zed_node/left/image_rect_color", 10,
imageLeftRectifiedCallback);

/**
* ros::spin() will enter a loop, pumping callbacks. With this version, all
Expand Down
Loading

0 comments on commit de035d3

Please sign in to comment.