Adding nodelet file with stereo camera feature

This commit is contained in:
AhmedElsafy 2020-10-05 14:20:08 -03:00
parent 7b7a264e45
commit 00149d2bdc
6 changed files with 163 additions and 43 deletions

View File

@ -4,23 +4,24 @@ camera_name: left
camera_matrix: camera_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [ 873.68467, 0. , 657.89992, data: [907.91127, 0. , 604.06352,
0. , 871.77207, 360.60431, 0. , 906.83626, 390.59699,
0. , 0. , 1. ] 0. , 0. , 1. ]
camera_model: plumb_bob camera_model: plumb_bob
distortion_coefficients: distortion_coefficients:
rows: 1 rows: 1
cols: 5 cols: 5
data: [-0.345111, 0.109548, 0.007497, 0.001845, 0.000000] data: [-0.391252, 0.163859, -0.005408, 0.004813, 0.000000]
rectification_matrix: rectification_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [ 0.999594 , 0.01840935, 0.02174699, data: [ 0.99300094, -0.00822023, -0.11782007,
-0.0188595 , 0.99960827, 0.02067903, 0.00780606, 0.99996163, -0.00397631,
-0.02135778, -0.02108077, 0.99954962] 0.11784823, 0.00302877, 0.993027 ]
projection_matrix: projection_matrix:
rows: 3 rows: 3
cols: 4 cols: 4
data: [ 869.23405, 0. , 590.64311, 0. , data: [896.45054, 0. , 764.25062, 0. ,
0. , 869.23405, 366.70978, 0. , 0. , 896.45054, 388.10495, 0. ,
0. , 0. , 1. , 0. ] 0. , 0. , 1. , 0. ]

View File

@ -4,17 +4,17 @@ camera_name: left
camera_matrix: camera_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [216.587179, 0.000000, 152.272854, 0.000000, 288.814935, 123.514917, 0.000000, 0.000000, 1.000000] data: [220.501902, 0.000000, 161.128732, 0.000000, 295.161136, 119.838802, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob distortion_model: plumb_bob
distortion_coefficients: distortion_coefficients:
rows: 1 rows: 1
cols: 5 cols: 5
data: [-0.400277, 0.190072, -0.003888, 0.002995, 0.000000] data: [-0.377372, 0.137398, -0.003643, -0.002721, 0.000000]
rectification_matrix: rectification_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [0.990367, 0.059907, -0.124836, -0.059914, 0.998197, 0.003704, 0.124833, 0.003811, 0.992170] data: [0.997904, 0.008937, 0.064086, -0.009348, 0.999938, 0.006124, -0.064028, -0.006710, 0.997926]
projection_matrix: projection_matrix:
rows: 3 rows: 3
cols: 4 cols: 4
data: [296.195350, 0.000000, 215.261782, 0.000000, 0.000000, 296.195350, 120.768448, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] data: [284.160707, 0.000000, 129.425322, 0.000000, 0.000000, 284.160707, 118.174007, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

View File

@ -4,23 +4,24 @@ camera_name: right
camera_matrix: camera_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [ 879.81628, 0. , 631.55074, data: [935.14307, 0. , 620.65919,
0. , 877.90835, 349.78586, 0. , 932.02901, 398.37732,
0. , 0. , 1. ] 0. , 0. , 1. ]
camera_model: plumb_bob camera_model: plumb_bob
distortion_coefficients: distortion_coefficients:
rows: 1 rows: 1
cols: 5 cols: 5
data: [-0.359859, 0.129457, 0.002187, 0.006292, 0.000000] data: [-0.397349, 0.154127, -0.004348, 0.004203, 0.000000]
rectification_matrix: rectification_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [ 0.99680464, 0.0451326 , 0.06590573, data: [ 0.99413297, -0.00792457, -0.10787415,
-0.04374539, 0.99879285, -0.02234257, 0.00830358, 0.99996083, 0.00306465,
-0.06683455, 0.0193881 , 0.99757568] 0.10784563, -0.00394241, 0.99415983]
projection_matrix: projection_matrix:
rows: 3 rows: 3
cols: 4 cols: 4
data: [ 869.23405, 0. , 590.64311, -46.17111, data: [896.45054, 0. , 764.25062, -48.59953,
0. , 869.23405, 366.70978, 0. , 0. , 896.45054, 388.10495, 0. ,
0. , 0. , 1. , 0. ] 0. , 0. , 1. , 0. ]

View File

@ -4,17 +4,17 @@ camera_name: right
camera_matrix: camera_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [223.401364, 0.000000, 161.232191, 0.000000, 297.556234, 124.851974, 0.000000, 0.000000, 1.000000] data: [223.530916, 0.000000, 162.038856, 0.000000, 298.909254, 118.283092, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob distortion_model: plumb_bob
distortion_coefficients: distortion_coefficients:
rows: 1 rows: 1
cols: 5 cols: 5
data: [-0.390714, 0.159344, -0.002090, -0.002267, 0.000000] data: [-0.378272, 0.137125, 0.000916, -0.002117, 0.000000]
rectification_matrix: rectification_matrix:
rows: 3 rows: 3
cols: 3 cols: 3
data: [0.988569, 0.058607, -0.138916, -0.058599, 0.998273, 0.004148, 0.138919, 0.004040, 0.990296] data: [0.997975, 0.006016, 0.063321, -0.005609, 0.999962, -0.006601, -0.063359, 0.006233, 0.997971]
projection_matrix: projection_matrix:
rows: 3 rows: 3
cols: 4 cols: 4
data: [296.195350, 0.000000, 215.261782, -18.019714, 0.000000, 296.195350, 120.768448, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] data: [284.160707, 0.000000, 129.425322, -16.623746, 0.000000, 284.160707, 118.174007, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

View File

@ -0,0 +1,76 @@
<launch>
<!-- <arg name="DEVICE" default="/dev/video0"/> -->
<arg name="sync_sink" default="true" /> <!-- Synchronize the app sink -->
<arg name="width" default="1280" /> <!-- Image Width -->
<arg name="height" default="720" /> <!-- Image Height -->
<arg name="fps" default="20" /> <!-- Desired framerate -->
<!-- Make arguments available to parameter server -->
<param name="image_width" type="int" value="$(arg width)" />
<param name="image_height" type="int" value="$(arg height)" />
<param name="target_fps" type="int" value="$(arg fps)" />
<group ns="stereo">
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="GSCamNodeletLeft" args="load gscam/GSCamNodelet standalone_nodelet respawn:=true" output="screen">
<param name="camera_name" value="left"/>
<param name="camera_info_url" value="package://jetson_csi_cam/left_1280x720.yaml"/>
<param name="gscam_config" value="nvarguscamerasrc sensor-id=1 aelock=true awblock=true ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)NV12,framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
<param name="frame_id" value="left_link"/>
<param name="sync_sink" value="true"/>
<param name="left/image_width" type="int" value="$(arg width)" />
<param name="left/image_height" type="int" value="$(arg height)" />
<param name="left/target_fps" type="int" value="$(arg fps)" />
<remap from="camera/image_raw" to="left/image_raw" />
<remap from="camera/camera_info" to="left/camera_info" />
<!--<remap from="/set_camera_info" to="left/set_camera_info" />-->
</node>
<node pkg="nodelet" type="nodelet" name="GSCamNodeletRight" args="standalone gscam/GSCamNodelet" output="screen">
<param name="camera_name" value="right"/>
<param name="camera_info_url" value="package://jetson_csi_cam/right_1280x720.yaml"/>
<param name="gscam_config" value="nvarguscamerasrc sensor-id=0 aelock=true awblock=true ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)NV12,framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
<param name="frame_id" value="right_link"/>
<param name="sync_sink" value="true"/>
<param name="right/image_width" type="int" value="$(arg width)" />
<param name="right/image_height" type="int" value="$(arg height)" />
<param name="right/target_fps" type="int" value="$(arg fps)" />
<remap from="camera/image_raw" to="right/image_raw" />
<remap from="camera/camera_info" to="right/camera_info" />
<!--<remap from="/set_camera_info" to="right/set_camera_info" />-->
</node>
<!--<node pkg="nodelet" type="nodelet" name="ImageViewNodeletLeft"
args="load image_view/image image:=/stereo/right/image_raw standalone_nodelet"
respawn="false" output="screen" >
</node>
<node pkg="nodelet" type="nodelet" name="ImageViewNodeletRight"
args="standalone image_view/image image:=/stereo/left/image_raw"
respawn="false" output="screen" >
</node>
<include file="$(find image_proc)/launch/image_proc.launch"
ns="left">
<arg name="manager" value="standalone_nodelet" />
<arg name="respawn" value="false" />
</include>
<! Basic processing for right camera
<include file="$(find image_proc)/launch/image_proc.launch"
ns="right">
<arg name="manager" value="standalone_nodelet" />
<arg name="respawn" value="false" />
</include>
<node pkg="nodelet" type="nodelet" name="disparity"
args="load gpu_stereo_image_proc/libsgm_disparity standalone_nodelet"
respawn="false" output="screen">
<param name="approximate_sync" value="true" />
</node>-->
</group>
</launch>

View File

@ -10,25 +10,67 @@
<param name="image_height" type="int" value="$(arg height)" /> <param name="image_height" type="int" value="$(arg height)" />
<param name="target_fps" type="int" value="$(arg fps)" /> <param name="target_fps" type="int" value="$(arg fps)" />
<node pkg="nodelet" type="nodelet"
name="standalone_nodelet" args="manager"
output="screen"/>
<node pkg="nodelet" type="nodelet" <group ns="stereo">
name="GSCamNodelet"
args="load gscam/GSCamNodelet standalone_nodelet" <node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
output="screen">
<param name="camera_name" value="left"/> <node pkg="nodelet" type="nodelet" name="GSCamNodeletLeft" args="load gscam/GSCamNodelet standalone_nodelet respawn:=true" output="screen">
<param name="camera_info_url" value="package://jetson_csi_cam/left_320x240.yaml"/> <param name="camera_name" value="left"/>
<param name="gscam_config" value="nvarguscamerasrc sensor-id=0 aelock=true awblock=true ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)NV12,framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=0 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" /> <param name="camera_info_url" value="package://jetson_csi_cam/left_320x240.yaml"/>
<param name="frame_id" value="left_link"/> <param name="gscam_config" value="nvarguscamerasrc sensor-id=1 aelock=true awblock=true ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)NV12,framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
<param name="sync_sink" value="true"/> <param name="frame_id" value="left_link"/>
<param name="left/image_width" type="int" value="$(arg width)" /> <param name="sync_sink" value="true"/>
<param name="left/image_width" type="int" value="$(arg width)" />
<param name="left/image_height" type="int" value="$(arg height)" /> <param name="left/image_height" type="int" value="$(arg height)" />
<param name="left/target_fps" type="int" value="$(arg fps)" /> <param name="left/target_fps" type="int" value="$(arg fps)" />
<remap from="camera/image_raw" to="left/image_raw" /> <remap from="camera/image_raw" to="left/image_raw" />
<remap from="camera/camera_info" to="left/camera_info" /> <remap from="camera/camera_info" to="left/camera_info" />
</node> <!--<remap from="/set_camera_info" to="left/set_camera_info" />-->
</node>
<node pkg="nodelet" type="nodelet" name="GSCamNodeletRight" args="standalone gscam/GSCamNodelet" output="screen">
<param name="camera_name" value="right"/>
<param name="camera_info_url" value="package://jetson_csi_cam/right_320x240.yaml"/>
<param name="gscam_config" value="nvarguscamerasrc sensor-id=0 aelock=true awblock=true ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)NV12,framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
<param name="frame_id" value="right_link"/>
<param name="sync_sink" value="true"/>
<param name="right/image_width" type="int" value="$(arg width)" />
<param name="right/image_height" type="int" value="$(arg height)" />
<param name="right/target_fps" type="int" value="$(arg fps)" />
<remap from="camera/image_raw" to="right/image_raw" />
<remap from="camera/camera_info" to="right/camera_info" />
<!--<remap from="/set_camera_info" to="right/set_camera_info" />-->
</node>
<!--<node pkg="nodelet" type="nodelet" name="ImageViewNodeletLeft"
args="load image_view/image image:=/stereo/right/image_raw standalone_nodelet"
respawn="false" output="screen" >
</node>
<node pkg="nodelet" type="nodelet" name="ImageViewNodeletRight"
args="standalone image_view/image image:=/stereo/left/image_raw"
respawn="false" output="screen" >
</node>
<include file="$(find image_proc)/launch/image_proc.launch"
ns="left">
<arg name="manager" value="standalone_nodelet" />
<arg name="respawn" value="false" />
</include>
<! Basic processing for right camera
<include file="$(find image_proc)/launch/image_proc.launch"
ns="right">
<arg name="manager" value="standalone_nodelet" />
<arg name="respawn" value="false" />
</include>
<node pkg="nodelet" type="nodelet" name="disparity"
args="load gpu_stereo_image_proc/libsgm_disparity standalone_nodelet"
respawn="false" output="screen">
<param name="approximate_sync" value="true" />
</node>-->
</group>
</launch> </launch>