Skip to content

Commit

Permalink
Attempt to move two-zed configuation into the cart_endpoints package.…
Browse files Browse the repository at this point in the history 
… (untested)
  • Loading branch information
@spragunr
spragunr committed Jun 24, 2022
1 parent 5536a98 commit 1dae867
Show file tree
Hide file tree
Showing 7 changed files with 437 additions and 1 deletion.
95 changes: 95 additions & 0 deletions cart_endpoints/launch/include/zed_camera_mod.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
<?xml version="1.0"?>
<!--
Copyright (c) 2020, STEREOLABS.
All rights reserved.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<launch>
<!-- Camera Model and Name -->
<arg name="camera_name" default="zed" /> <!-- The name you want -->
<arg name="camera_model" default="zed" /> <!-- 'zed' or 'zedm' or 'zed2' -->
<arg name="node_name" default="zed_node" />
<arg name="serial_number" default="0" />

<arg name="common_params" default="$(find zed_wrapper)/params/common.yaml" />
<arg name="model_params" default="$(find zed_wrapper)/params/$(arg camera_model).yaml" />

<arg name="svo_file" default="" /><!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
<!-- Remote stream -->
<arg name="stream" default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->

<!-- Publish ZED urdf -->
<arg name="publish_urdf" default="true" />

<arg name="camera_id" default="0" />
<arg name="gpu_id" default="-1" />

<!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_x" default="0.0" />
<arg name="cam_pos_y" default="0.0" />
<arg name="cam_pos_z" default="0.0" />

<!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_roll" default="0.0" />
<arg name="cam_pitch" default="0.0" />
<arg name="cam_yaw" default="0.0" />

<arg name="publish_tf" default="true" />

<!-- ROS URDF description of the ZED -->
<group if="$(arg publish_urdf)">
<param name="zed_description"
command="$(find xacro)/xacro '$(find zed_wrapper)/urdf/zed_descr.urdf.xacro'
camera_name:=$(arg camera_name)
camera_model:=$(arg camera_model)
cam_pos_x:=$(arg cam_pos_x)
cam_pos_y:=$(arg cam_pos_y)
cam_pos_z:=$(arg cam_pos_z)
cam_roll:=$(arg cam_roll)
cam_pitch:=$(arg cam_pitch)
cam_yaw:=$(arg cam_yaw)
serial_number:=$(arg serial_number)
"
/>

<node name="zed_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" required="true">
<remap from="robot_description" to="zed_description" />
</node>
</group>

<node name="$(arg node_name)" pkg="zed_wrapper" type="zed_wrapper_node" output="screen" required="true"><!-- launch-prefix="valgrind" -->
<rosparam file="$(arg common_params)" command="load" />
<rosparam file="$(arg model_params)" command="load" />

<!-- Camera name -->
<param name="general/camera_name" value="$(arg camera_name)" />

<!-- SVO file path -->
<param name="general/svo_file" value="$(arg svo_file)" />

<!-- Remote stream -->
<param name="general/stream" value="$(arg stream)" />

<!-- Camera ID -->
<param name="general/zed_id" value="$(arg camera_id)" />

<!-- GPU ID -->
<param name="general/gpu_id" value="$(arg gpu_id)" />

<!-- TF -->
<param name="pos_tracking/publish_tf" value="$(arg publish_tf)" />
<param name="pos_tracking/publish_map_tf" value="$(arg publish_tf)" />
</node>
</launch>
135 changes: 135 additions & 0 deletions cart_endpoints/launch/jacart_multi_cam.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,135 @@
<?xml version="1.0"?>
<!--
Copyright (c) 2020, STEREOLABS.
All rights reserved.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<launch>
<arg name="publish_urdf" default="true"/>

<!-- X is out the front of the cart -->
<!-- Y is left/right -->
<!-- Z is actually up -->

<!-- CAMERA 1 (Front) -->
<arg name="node_name_1" default="front"/>
<arg name="camera_model_1" default="zed2i"/>
<arg name="camera_name_1" default="front_cam"/>
<arg name="common_params_1" default="$(find cart_endpoints)/zed_params/common_front.yaml"/>
<arg name="model_params_1" default="$(find cart_endpoints)/zed_params/zed2i_front.yaml"/>
<arg name="cam_pos_x_1" default="0.0"/> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_y_1" default="-0.102"/> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_z_1" default="0.5"/> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_roll_1" default="3.14"/> <!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_pitch_1" default="0.383972"/> <!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_yaw_1" default="0"/> <!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="svo_file_1" default="/home/jacart/videos/front.svo" />
<arg name="camera_sn_1" default="37963597"/>


<!-- CAMERA 2 (passenger) -->
<arg name="node_name_2" default="passenger"/>
<arg name="camera_model_2" default="zed2i"/>
<arg name="camera_name_2" default="passenger_cam"/>
<arg name="common_params_2" default="$(find cart_endpoints)/zed_params/common_passenger.yaml"/>
<arg name="model_params_2" default="$(find cart_endpoints)/zed_params/zed2i_passenger.yaml"/>
<arg name="cam_pos_x_2" default="0.0"/> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_y_2" default="0.102"/> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_z_2" default="0.5"/> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_roll_2" default="3.14"/> <!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_pitch_2" default="0.0"/> <!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_yaw_2" default="-3.14"/>
<arg name="svo_file_2" default="/home/jacart/videos/passenger.svo" />
<arg name="camera_sn_2" default="31061594"/>


<arg name="cloud" default="/$(arg node_name_1)/point_cloud/cloud_registered"/>
<arg name="laserscan" default="/front_camera_laserscan"/>



<group ns="$(arg camera_name_1)">
<include file="$(find cart_endpoints)/launch/include/zed_camera_mod.launch">
<arg name="camera_name" value="$(arg camera_name_1)"/>
<arg name="node_name" value="$(arg node_name_1)"/>
<arg name="camera_model" value="$(arg camera_model_1)"/>
<arg name="common_params" value="$(arg common_params_1)"/>
<arg name="model_params" value="$(arg model_params_1"/>
<arg name="publish_urdf" value="$(arg publish_urdf)"/>
<arg name="publish_tf" value="false"/>
<arg name="camera_id" value="0"/>
<arg name="serial_number" value="$(arg camera_sn_1)"/>
<arg name="cam_pos_x" value="$(arg cam_pos_x_1)"/>
<arg name="cam_pos_y" value="$(arg cam_pos_y_1)"/>
<arg name="cam_pos_z" value="$(arg cam_pos_z_1)"/>
<arg name="cam_roll" value="$(arg cam_roll_1)"/>
<arg name="cam_pitch" value="$(arg cam_pitch_1)"/>
<arg name="cam_yaw" value="$(arg cam_yaw_1)"/>
<arg name="svo_file" value="$(arg svo_file_1)"/>
</include>
</group>

<group ns="$(arg camera_name_2)">
<include file="$(find cart_endpoints)/launch/include/zed_camera_mod.launch">
<arg name="camera_name" value="$(arg camera_name_2)"/>
<arg name="node_name" value="$(arg node_name_2)"/>
<arg name="camera_model" value="$(arg camera_model_2)"/>
<arg name="common_params" value="$(arg common_params_2)"/>
<arg name="model_params" value="$(arg model_params_2"/>
<arg name="publish_urdf" value="true"/>
<arg name="publish_tf" value="false"/>
<!-- Only the main camera must publish TF! -->
<arg name="camera_id" value="1"/>
<arg name="serial_number" value="$(arg camera_sn_2)"/>
<arg name="cam_pos_x" value="$(arg cam_pos_x_2)"/>
<arg name="cam_pos_y" value="$(arg cam_pos_y_2)"/>
<arg name="cam_pos_z" value="$(arg cam_pos_z_2)"/>
<arg name="cam_roll" value="$(arg cam_roll_2)"/>
<arg name="cam_pitch" value="$(arg cam_pitch_2)"/>
<arg name="cam_yaw" value="$(arg cam_yaw_2)"/>
<arg name="svo_file" value="$(arg svo_file_2)"/>
</include>
</group>

<!-- run pointcloud_to_laserscan node -->
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="front_cam_pointcloud_to_laserscan">
<remap from="cloud_in" to="/voxel_grid/output"/>
<remap from="scan" to="/front_cam_scan"/>
<rosparam>
target_frame: velodyne

min_height: -0.6
max_height: 1.0
range_min: 0.01
concurrency_level: 1
angle_increment: 0.02616
</rosparam>
</node>
<node name="front_cam_obstacle_detector" pkg="cart_endpoints" type="zed_obstacle_detector.py"/>
<node name="front_cam_obj_det_to_obstacle" pkg="cart_endpoints" type="zed_object_to_obstacle.py"/>

<node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" />
<node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid pcl_manager" output="screen">
<remap from="~input" to="/front_cam/front/point_cloud/cloud_registered" />
<rosparam>
filter_field_name: x
filter_limit_min: 0.01
filter_limit_max: 8
filter_limit_negative: False
leaf_size: 0.1
</rosparam>
</node>

</launch>
69 changes: 69 additions & 0 deletions cart_endpoints/zed_params/common_front.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,69 @@
# params/common.yaml
# Common parameters to Stereolabs ZED and ZED mini cameras
---

# Dynamic parameters cannot have a namespace
brightness: 4 # Dynamic
contrast: 4 # Dynamic
hue: 0 # Dynamic
saturation: 4 # Dynamic
sharpness: 4 # Dynamic
gamma: 8 # Dynamic - Requires SDK >=v3.1
auto_exposure_gain: true # Dynamic
gain: 100 # Dynamic - works only if `auto_exposure_gain` is false
exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false
auto_whitebalance: true # Dynamic
whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false
depth_confidence: 50 # Dynamic
depth_texture_conf: 100 # Dynamic
pub_frame_rate: 30.0 # Dynamic - frequency of publishing of video and depth data
point_cloud_freq: 10.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value)

general:
camera_name: zed # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file)
zed_id: 0
serial_number: 0
resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA
grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations
gpu_id: -1
base_frame: 'base_link' # must be equal to the frame_id used in the URDF file
verbose: false # Enable info message by the ZED SDK
svo_compression: 2 # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC
self_calib: true # enable/disable self calibration at starting
camera_flip: false

video:
img_downsample_factor: 1.0 # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image.
extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1]

depth:
quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA
sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications)
depth_stabilization: 1 # `0`: disabled, `1`: enabled
openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters
depth_downsample_factor: 1.0 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...)

pos_tracking:
pos_tracking_enabled: true # True to enable positional tracking from start
publish_tf: false # publish `odom -> base_link` TF
publish_map_tf: false # publish `map -> odom` TF
map_frame: 'map' # main frame
odometry_frame: 'odom' # odometry frame
area_memory_db_path: 'zed_area_memory.area' # file loaded when the node starts to restore the "known visual features" map.
save_area_memory_db_on_exit: false # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path`
area_memory: true # Enable to detect loop closure
floor_alignment: false # Enable to automatically calculate camera/floor offset
initial_base_pose: [0.0,0.0,0.0, 0.0,0.0,0.0] # Initial position of the `base_frame` -> [X, Y, Z, R, P, Y]
init_odom_with_first_valid_pose: true # Enable to initialize the odometry with the first valid pose
path_pub_rate: 2.0 # Camera trajectory publishing frequency
path_max_count: -1 # use '-1' for unlimited path size
two_d_mode: false # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero
fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true

mapping:
mapping_enabled: false # True to enable mapping and fused point cloud pubblication
resolution: 0.05 # maps resolution in meters [0.01f, 0.2f]
max_mapping_range: -1 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0]
fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud


Loading

0 comments on commit 1dae867

Please sign in to comment.