Adding right tf frames
This commit is contained in:
parent
2d3309ebcb
commit
40bff0db8d
0
.goutputstream-B0TIZ0
Normal file
0
.goutputstream-B0TIZ0
Normal file
27
left_800x600.yaml
Normal file
27
left_800x600.yaml
Normal file
@ -0,0 +1,27 @@
|
|||||||
|
image_width: 800
|
||||||
|
image_height: 600
|
||||||
|
camera_name: left
|
||||||
|
camera_matrix:
|
||||||
|
rows: 3
|
||||||
|
cols: 3
|
||||||
|
data: [546.82833, 0. , 395.41899,
|
||||||
|
0. , 728.53633, 307.08189,
|
||||||
|
0. , 0. , 1. ]
|
||||||
|
camera_model: plumb_bob
|
||||||
|
distortion_coefficients:
|
||||||
|
rows: 1
|
||||||
|
cols: 5
|
||||||
|
data: [-0.394573, 0.162349, -0.002198, -0.001344, 0.000000]
|
||||||
|
rectification_matrix:
|
||||||
|
rows: 3
|
||||||
|
cols: 3
|
||||||
|
data: [ 0.99984948, 0.00197076, -0.0172378 ,
|
||||||
|
-0.00184396, 0.99997115, 0.00736835,
|
||||||
|
0.01725183, -0.00733546, 0.99982427]
|
||||||
|
projection_matrix:
|
||||||
|
rows: 3
|
||||||
|
cols: 4
|
||||||
|
data: [704.41879, 0. , 419.01768, 0. ,
|
||||||
|
0. , 704.41879, 306.11043, 0. ,
|
||||||
|
0. , 0. , 1. , 0. ]
|
||||||
|
|
27
right_800x600.yaml
Normal file
27
right_800x600.yaml
Normal file
@ -0,0 +1,27 @@
|
|||||||
|
image_width: 800
|
||||||
|
image_height: 600
|
||||||
|
camera_name: right
|
||||||
|
camera_matrix:
|
||||||
|
rows: 3
|
||||||
|
cols: 3
|
||||||
|
data: [556.66939, 0. , 402.50992,
|
||||||
|
0. , 741.65684, 304.22578,
|
||||||
|
0. , 0. , 1. ]
|
||||||
|
camera_model: plumb_bob
|
||||||
|
distortion_coefficients:
|
||||||
|
rows: 1
|
||||||
|
cols: 5
|
||||||
|
data: [-0.394982, 0.162894, 0.002350, -0.001648, 0.000000]
|
||||||
|
rectification_matrix:
|
||||||
|
rows: 3
|
||||||
|
cols: 3
|
||||||
|
data: [ 0.99967835, -0.00220518, -0.02526515,
|
||||||
|
0.00201935, 0.99997074, -0.00737829,
|
||||||
|
0.02528068, 0.00732489, 0.99965356]
|
||||||
|
projection_matrix:
|
||||||
|
rows: 3
|
||||||
|
cols: 4
|
||||||
|
data: [704.41879, 0. , 419.01768, -36.86134,
|
||||||
|
0. , 704.41879, 306.11043, 0. ,
|
||||||
|
0. , 0. , 1. , 0. ]
|
||||||
|
|
96
video-daylight-nodelet-800x600.launch
Normal file
96
video-daylight-nodelet-800x600.launch
Normal file
@ -0,0 +1,96 @@
|
|||||||
|
<launch>
|
||||||
|
<!-- <arg name="DEVICE" default="/dev/video0"/> -->
|
||||||
|
<arg name="sync_sink" default="true" /> <!-- Synchronize the app sink -->
|
||||||
|
<arg name="width" default="800" /> <!-- Image Width -->
|
||||||
|
<arg name="height" default="600" /> <!-- Image Height -->
|
||||||
|
<arg name="fps" default="20" />
|
||||||
|
<arg name="respawn" default="false" />
|
||||||
|
<arg name="left" default="left" />
|
||||||
|
<arg name="right" default="right" />
|
||||||
|
<arg name="approximate_sync" default="true" /> <!-- Desired framerate -->
|
||||||
|
<arg if="$(arg respawn)" name="bond" value="" />
|
||||||
|
<arg unless="$(arg respawn)" name="bond" value="--no-bond" />
|
||||||
|
|
||||||
|
<!-- 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_800x600.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="tf" type="static_transform_publisher" name="fcu_to_Left_link" args="0.08505 0.0265 -0.0562 -0.5958106 0.5966497 -0.3801439 0.3801439 fcu left_link 10" />
|
||||||
|
|
||||||
|
<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_800x600.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="tf" type="static_transform_publisher" name="fcu_to_Right_link" args="0.08505 -0.0265 -0.0562 -0.5958106 0.5966497 -0.3801439 0.3801439 fcu right_link 10"/>
|
||||||
|
|
||||||
|
<!--<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>-->
|
||||||
|
<node pkg="nodelet" type="nodelet" name="Image2RTSPNodelet" args="load image2rtsp/Image2RTSPNodelet standalone_nodelet --no-bond" output="screen">
|
||||||
|
<!-- Read the stream setup file -->
|
||||||
|
<rosparam command="load" file="$(find ros_rtsp)/config/stream_setup.yaml" />
|
||||||
|
</node>
|
||||||
|
<!--<include file="$(find gpu_stereo_image_proc)/launch/libsgm_stereo_image_proc.launch">
|
||||||
|
<arg name="manager" value="/stereo/standalone_nodelet"/>
|
||||||
|
</include>-->
|
||||||
|
|
||||||
|
|
||||||
|
</group>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
</launch>
|
@ -33,7 +33,8 @@
|
|||||||
<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" />
|
||||||
<!--<remap from="/set_camera_info" to="left/set_camera_info" />-->
|
<!--<remap from="/set_camera_info" to="left/set_camera_info" />-->
|
||||||
</node>
|
</node>
|
||||||
|
<node pkg="tf" type="static_transform_publisher" name="fcu_to_Left_link" args="0.08505 0.0265 -0.0562 -0.5958106 0.5966497 -0.3801439 0.3801439 fcu left_link 10" />
|
||||||
|
|
||||||
<node pkg="nodelet" type="nodelet" name="GSCamNodeletRight" args="standalone gscam/GSCamNodelet" output="screen">
|
<node pkg="nodelet" type="nodelet" name="GSCamNodeletRight" args="standalone gscam/GSCamNodelet" output="screen">
|
||||||
<param name="camera_name" value="right"/>
|
<param name="camera_name" value="right"/>
|
||||||
@ -48,6 +49,7 @@
|
|||||||
<remap from="camera/camera_info" to="right/camera_info" />
|
<remap from="camera/camera_info" to="right/camera_info" />
|
||||||
<!--<remap from="/set_camera_info" to="right/set_camera_info" />-->
|
<!--<remap from="/set_camera_info" to="right/set_camera_info" />-->
|
||||||
</node>
|
</node>
|
||||||
|
<node pkg="tf" type="static_transform_publisher" name="fcu_to_Right_link" args="0.08505 -0.0265 -0.0562 -0.5958106 0.5966497 -0.3801439 0.3801439 fcu right_link 10"/>
|
||||||
|
|
||||||
<!--<node pkg="nodelet" type="nodelet" name="ImageViewNodeletLeft"
|
<!--<node pkg="nodelet" type="nodelet" name="ImageViewNodeletLeft"
|
||||||
args="load image_view/image image:=/stereo/right/image_raw standalone_nodelet"
|
args="load image_view/image image:=/stereo/right/image_raw standalone_nodelet"
|
||||||
@ -80,9 +82,9 @@
|
|||||||
<!-- Read the stream setup file -->
|
<!-- Read the stream setup file -->
|
||||||
<rosparam command="load" file="$(find ros_rtsp)/config/stream_setup.yaml" />
|
<rosparam command="load" file="$(find ros_rtsp)/config/stream_setup.yaml" />
|
||||||
</node>
|
</node>
|
||||||
<include file="$(find gpu_stereo_image_proc)/launch/libsgm_stereo_image_proc.launch">
|
<!--<include file="$(find gpu_stereo_image_proc)/launch/libsgm_stereo_image_proc.launch">
|
||||||
<arg name="manager" value="/stereo/standalone_nodelet"/>
|
<arg name="manager" value="/stereo/standalone_nodelet"/>
|
||||||
</include>
|
</include>-->
|
||||||
|
|
||||||
|
|
||||||
</group>
|
</group>
|
||||||
|
Loading…
Reference in New Issue
Block a user