forked from xtdrone/XTDrone
62 lines
3.0 KiB
XML
62 lines
3.0 KiB
XML
<?xml version="1.0"?>
|
|
<!-- Example of ROS localization using the example wamv with sensors -->
|
|
<launch>
|
|
<!-- Publishes static joint transforms (lidar, cameras, etc) to /tf -->
|
|
<node ns="wamv" pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub">
|
|
<param name="tf_prefix" value="wamv" />
|
|
</node>
|
|
|
|
<!-- Publishes revolute joint static transforms (gps and imu) to /tf -->
|
|
<node ns="wamv" pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
|
|
<param name="gui" value="false" />
|
|
</node>
|
|
|
|
<!-- Kalman filter fusing imu and gps into combined odometry/tf -->
|
|
<node ns="wamv/robot_localization" pkg="robot_localization" type="ekf_localization_node"
|
|
name="ekf_localization" clear_params="false">
|
|
<param name="sensor_timeout" value="2.0"/>
|
|
<param name="two_d_mode" value="false"/>
|
|
<param name="map_frame" value="map"/>
|
|
<param name="odom_frame" value="wamv/odom"/>
|
|
<param name="base_link_frame" value="wamv/base_link"/>
|
|
<param name="world_frame" value="wamv/odom"/>
|
|
<param name="publish_tf" value="true"/>
|
|
<param name="frequency" value="60"/>
|
|
<param name="imu0" value="/wamv/sensors/imu/imu/data"/>
|
|
<!-- IMU measures orientation, angular velocity, and linear acceleration -->
|
|
<rosparam param="imu0_config">[false, false, false,
|
|
true, true, true,
|
|
false, false, false,
|
|
true, true, true,
|
|
true, true, true]</rosparam>
|
|
<param name="imu0_differential" value="false"/>
|
|
<param name="imu0_remove_gravitational_acceleration" value="true"/>
|
|
|
|
<param name="odom0" value="/wamv/robot_localization/odometry/gps"/>
|
|
<!-- GPS only reliably measures absolute position -->
|
|
<rosparam param="odom0_config">[true, true, true,
|
|
false, false, false,
|
|
false, false, false,
|
|
false, false, false,
|
|
false, false, false]</rosparam>
|
|
|
|
<param name="odom0_differential" value="false"/>
|
|
<param name="smooth_lagged_data" value="true"/>
|
|
</node>
|
|
|
|
<!-- Produces local odometry from GPS to be used in Kalman filter -->
|
|
<node ns="wamv/robot_localization" pkg="robot_localization" type="navsat_transform_node"
|
|
name="navsat_transform_node" respawn="true" output="screen">
|
|
<param name="tf_prefix" value="wamv" />
|
|
<param name="frequency" value="60"/>
|
|
<param name="magnetic_declination_radians" value="0"/>
|
|
<param name="broadcast_utm_transform" value="true"/>
|
|
<param name="wait_for_datum" value="true"/>
|
|
<param name="use_odometry_yaw" value="true"/>
|
|
<rosparam param="datum">[21.30996, -157.8901]</rosparam>
|
|
<param name="yaw_offset" value="0"/>
|
|
<param name="publish_filtered_gps" value="true"/>
|
|
<remap to="/wamv/sensors/gps/gps/fix" from="/wamv/robot_localization/gps/fix" />
|
|
</node>
|
|
</launch>
|