XTDrone/sitl_config/ugv/catvehicle/launch/catvehicle-tf.launch

108 lines
5.2 KiB
XML

<?xml version="1.0"?>
<!--
Author: Jonathan Sprinkle, Rahul Bhadani
Copyright (c) 2016-2020 Arizona Board of Regents
All rights reserved.
Permission is hereby granted, without written agreement and without
license or royalty fees, to use, copy, modify, and distribute this
software and its documentation for any purpose, provided that the
above copyright notice and the following two paragraphs appear in
all copies of this software.
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
Summary:
=======
This launch file is used to broadcast tf and robot parameters for playback
or live visualization of data through rviz
How to execute it:
=================
roslaunch catvehicle catvehicle-tf.launch
-->
<launch>
<arg name="robot_name" default="catvehicle"/>
<arg name="tyre_height" default="0.7"/>
<arg name="halftyre_height" default="0.32"/>
<arg name="car_width" default="0.77"/>
<arg name="car_length" default="1.55"/>
<arg name="car_height" default="1.5837572084" />
<arg name="tyre_front_x" default="1.52"/>
<arg name="tyre_back_x" default="-1.1"/>
<include file="$(find catvehicle)/launch/robotviz.launch"/>
<node pkg="tf" type="static_transform_publisher" name="odom2ins_$(arg robot_name)"
args="-0.8 -$(arg car_width) 0.0 0 0 0 catvehicle/odom catvehicle/base_link 10"/>
<!-- JMS adapted this to refer to base_link rather than ins
<node pkg="tf" type="static_transform_publisher" name="baselink2velodyne_$(arg robot_name)"
args="1.2 0 0.75 0 0 0 catvehicle/ins velodyne 10"/>-->
<node pkg="tf" type="static_transform_publisher" name="baselink2velodyne_$(arg robot_name)"
args="-0.6 0 2.12 0 0 0 catvehicle/base_link velodyne 10"/>
<!-- convert from the velodyne to a /catvehicle/velodyne_link frame -->
<node pkg="tf" type="static_transform_publisher" name="velodyne2velodyne_link_$(arg robot_name)"
args="0 0 0 0 0 0 velodyne catvehicle/velodyne_link 10"/>
<node pkg="tf" type="static_transform_publisher" name="baselinkr2front_laser_link_tf_$(arg robot_name)"
args="2.5 0.0 1.1 0.0 0 0 catvehicle/base_link catvehicle/front_laser_link 75"/>
<node pkg="tf" type="static_transform_publisher" name="baselink_laser_tf_$(arg robot_name)"
args="2.5 0.0 1.1 0.0 0 0 catvehicle/base_link laser 75"/>
<!-- JMS: Note, I think that if we stop doing this, things will make more sense; should be a tf broadcasted
from the odometrybroadcaster_node in a package w/ hwil capabilities -->
<!--node pkg="tf" type="static_transform_publisher" name="baselink2odom_tf_$(arg robot_name)"
args="-1.25 0 $(arg halftyre_height) 0 0 0 catvehicle/base_link catvehicle/odom 75"/-->
<node pkg="tf" type="static_transform_publisher" name="baselink2mainmass_tf_$(arg robot_name)"
args="0 0 0 0 0 0 catvehicle/base_link catvehicle/main_mass 20"/>
<node pkg="tf" type="static_transform_publisher" name="baselink2leftFtire_tf_$(arg robot_name)"
args="$(arg tyre_front_x) -$(arg car_width) $(arg tyre_height) 3.14159265359 0 0 catvehicle/base_link catvehicle/front_left_wheel_link 10"/>
<node pkg="tf" type="static_transform_publisher" name="baselink2rightFtire_tf_$(arg robot_name)"
args="$(arg tyre_front_x) $(arg car_width) $(arg tyre_height) 3.14159265359 0 0 catvehicle/base_link catvehicle/front_right_wheel_link 10"/>
<node pkg="tf" type="static_transform_publisher" name="baselink2leftRtire_tf_$(arg robot_name)"
args="$(arg tyre_back_x) -$(arg car_width) $(arg tyre_height) 3.14159265359 0 0 catvehicle/base_link catvehicle/back_left_wheel_link 10"/>
<node pkg="tf" type="static_transform_publisher" name="baselink2rightRtire_tf_$(arg robot_name)"
args="$(arg tyre_back_x) $(arg car_width) $(arg tyre_height) 3.14159265359 0 0 catvehicle/base_link catvehicle/back_right_wheel_link 10"/>
<node pkg="tf" type="static_transform_publisher" name="rightFtire2rightFtiresteering_tf_$(arg robot_name)"
args="0 0 0 0 0 0 catvehicle/front_right_wheel_link catvehicle/front_right_steering_link 10"/>
<node pkg="tf" type="static_transform_publisher" name="leftFtire2leftFtiresteering_tf_$(arg robot_name)"
args="0 0 0 0 0 0 catvehicle/front_left_wheel_link catvehicle/front_left_steering_link 10"/>
<node pkg="tf" type="static_transform_publisher" name="velodynelink2cameraleft_tf_$(arg robot_name)"
args="-0.56 0.44 0 0 0 0 catvehicle/velodyne_link catvehicle/camera_left_link 5"/>
<node pkg="tf" type="static_transform_publisher" name="velodynelink2cameraright_tf_$(arg robot_name)"
args="-0.56 -0.44 0 0 0 0 catvehicle/velodyne_link catvehicle/camera_right_link 5"/>
<node pkg="tf" type="static_transform_publisher" name="velodynelink2camera_tf_$(arg robot_name)"
args="0.54 0 -0.08 0 0 0 catvehicle/velodyne_link catvehicle/triclops_link 5"/>
</launch>