Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
executable file 202 lines (193 sloc) 14.8 KB
<launch>
<!-- ############################################################### -->
<!-- NMEA NAVSAT DRIVER (GPS) ###################################### -->
<!-- ############################################################### -->
<node pkg="nmea_navsat_driver" type="nmea_serial_driver" name="navsat" respawn="true">
<param name="port" value="/dev/rfcomm0" />
<param name="baud" value="9600" />
</node>
<!-- ############################################################### -->
<!-- SICKLDMRS NODE (Laserscaner) ################################## -->
<!-- ############################################################### -->
<node name="sickldmrs" pkg="sick_ldmrs" type="sickldmrs.py" output="screen">
<param name="frame_id_prefix" value="/cloud" type="str" />
<param name="start_angle" value="30" type="int" /> <!-- 50 -->
<param name="end_angle" value="-30" type="int" /> <!-- -20 -->
</node>
<!-- ############################################################### -->
<!-- TINKERFORGE LASER TRANSFORM NODE ############################## -->
<!-- ############################################################### -->
<node name="tinkerforge_laser_transform_node" pkg="tinkerforge_laser_transform" type="tinkerforge_laser_transform_node" output="screen">
<!-- set topic names -->
<param name="pcl_in_topic" value="sickldmrs/cloud" type="str" />
<param name="pcl_out_topic" value="/cloud_in" type="str" />
<param name="imu_topic" value="/imu/data" type="str" />
<param name="imu_msgs" value="true" type="bool" />
<param name="gps_msgs" value="false" type="bool" />
<param name="odo_msgs" value="true" type="bool" />
<param name="imu_convergence_speed" value="5" type="int" />
<!-- Set Laser Pose x, y, z, yaw, pitch, roll in m and deg -->
<rosparam param="laser_pose">[3.17, 0.7, 2.1, -25.0, 20.0, -90.0]</rosparam>
</node>
<!-- ############################################################### -->
<!-- OCTOMAP SERVER NODE ########################################### -->
<!-- ############################################################### -->
<node name="octomap_server_node" pkg="octomap_server" type="octomap_server_node" output="screen">
<!-- Static global frame in which the map will be published -->
<param name="frame_id" value="/odom" type="str" />
<!-- Resolution in meter for the map -->
<param name="resolution" value="0.10" />
<!-- Maximum range in meter for inserting point cloud data -->
<param name="sensor_model/max_range" value="20.0" type="double" />
<param name="latch" value="false" type="bool" />
</node>
<!-- ############################################################### -->
<!-- NAVSAT TRANSFORM NODE ######################################### -->
<!-- ############################################################### -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">
<param name="magnetic_declination_radians" value="0"/>
<param name="yaw_offset" value="0"/>
<remap from="/imu/data" to="/imu/data" />
<remap from="/gps/fix" to="/fix" />
<remap from="/odometry/filtered" to="odometry/filtered" />
</node>
<!-- ############################################################### -->
<!-- EKF LOCALIZATION NODE ######################################### -->
<!-- ############################################################### -->
<!-- This node will take in measurements from odometry, IMU, stamped pose, and stamped twist messages. It tracks
the state of the robot, with the state vector being defined as X position, Y position, Z position,
roll, pitch, yaw, their respective velocites, and linear acceleration. Units for all measurements are assumed
to conform to the SI standard as specified in REP-103. -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
<!-- ======== STANDARD PARAMETERS ======== -->
<!-- The frequency, in Hz, at which the filter will output a position estimate. Note that
the filter will not begin computation until it receives at least one message from
one of the inputs. It will then run continuously at the frequency specified here,
regardless of whether it receives more measurements. Defaults to 30 if unspecified. -->
<param name="frequency" value="30"/>
<!-- The period, in seconds, after which we consider a sensor to have timed out. In this event, we
carry out a predict cycle on the EKF without correcting it. This parameter can be thought of
as the minimum frequency with which the filter will generate new output. Defaults to 1 / frequency
if not specified. -->
<param name="sensor_timeout" value="0.09"/>
<!-- If this is set to true, no 3D information will be used in your state estimate. Use this if you
are operating in a planar environment and want to ignore the effect of small variations in the
ground plane that might otherwise be detected by, for example, an IMU. Defaults to false if
unspecified. -->
<param name="two_d_mode" value="false"/>
<!-- REP-105 (http://www.ros.org/reps/rep-0105.html) specifies three principal coordinate frames: map,
odom, and base_link. base_link is the coordinate frame that is affixed to the robot. The robot's
position in the odom frame will drift over time, but is accurate in the short term and should be
continuous. The odom frame is therefore the best frame for executing local motion plans. The map
frame, like the odom frame, is a world-fixed coordinate frame, and while it contains the most
globally accurate position estimate for your robot, it is subject to discrete jumps, e.g., due to
the fusion of GPS data. Here is how to use the following settings:
1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
* If your system does not have a map_frame, just remove it, and make sure "world_frame" is set
to the value of odom_frame.
2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data,
set "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state
estimation nodes.
3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position
updates from landmark observations) then:
3a. Set your "world_frame" to your map_frame value
3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be
another instance of robot_localization! However, that instance should *not* fuse the global data. -->
<!-- Defaults to "map" if unspecified -->
<!-- <param name="map_frame" value="map"/> -->
<!-- Defaults to "odom" if unspecified -->
<param name="odom_frame" value="odom"/>
<!-- Defaults to "base_link" if unspecified -->
<param name="base_link_frame" value="base_link"/>
<!-- Defaults to the value of "odom_frame" if unspecified -->
<param name="world_frame" value="odom"/>
<!-- The filter accepts an arbitrary number of inputs from each input message type (Odometry, PoseStamped,
TwistStamped, Imu). To add a new one, simply append the next number in the sequence to its base name,
e.g., odom0, odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These
parameters obviously have no default values, and must be specified. -->
<param name="odom0" value="odom"/>
<param name="imu0" value="/imu/data"/>
<param name="odom1" value="/odometry/gps" />
<!-- Each sensor reading updates some or all of the filter's state. These options give you greater control over
which values from each measurement are fed to the filter. For example, if you have an odometry message as input,
but only want to use its Z position value, then set the entire vector to false, except for the third entry.
The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some
message types lack certain variables. For example, a TwistWithCovarianceStamped message has no pose information, so
the first six values would be meaningless in that case. Each vector defaults to all false if unspecified, effectively
making this parameter required for each sensor. -->
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<rosparam param="odom1_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<!-- The best practice for including new sensors in robot_localization's state estimation nodes is to pass in velocity
measurements and let the nodoes integrate them. However, this isn't always feasible, and so the state estimation
nodes support fusion of absolute measurements. If you have more than one sensor providing absolute measurements,
however, you will quickly run into problems, as the sensors will inevitably diverge from one another, causing the
filter to jump back and forth rapidly. To combat this situation, you can set the sensor's differential parameter
to true. This will carry out differential integration of the sensor data, i.e., it will repeatedly integrate the
difference between a given measurement and the previous measurement from that sensor.
NOTE: this only applies to sensors that provide absolute measurements, so Twist messages are not supported. -->
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom1_differential" value="false"/>
<!-- If we're including accelerations in our state estimate, then we'll probably want to remove any acceleration that
is due to gravity for each IMU. If you don't want to, then set this to false. Defaults to false if unspecified. -->
<param name="imu0_remove_gravitational_acceleration" value="false"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<!-- If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to control
how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
numeric_limits<double>::max() if unspecified. -->
<param name="odom0_pose_rejection_threshold" value="5"/>
<param name="odom0_twist_rejection_threshold" value="1"/>
<param name="imu0_twist_rejection_threshold" value="0.3"/>
<param name="imu0_angular_velocity_rejection_threshold" value="0.1"/>
<param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/>
<!-- Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file
specified by debug_out_file. I hope you like matrices! Defaults to false if unspecified. -->
<!-- Defaults to false if unspecified. -->
<param name="debug" value="false"/>
<!-- Defaults to "robot_localization_debug.txt" if unspecified. -->
<param name="debug_out_file" value="/home/vmuser/debug_ekf_localization.txt"/>
<!-- The process noise covariance matrix can be difficult to tune, and can vary for each application, so it
is exposed as a configuration parameter. PLEASE NOTE that every value in this matrix *must* be specified
as real-valued, i.e., it must have a decimal point. Integers don't load correctly. The values are ordered
as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
unspecified. -->
<rosparam param="process_noise_covariance">[0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.00, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]</rosparam>
<!-- Placeholder for output topic remapping
<remap from="odometry/filtered" to=""/>
-->
</node>
<!-- ############################################################### -->
<!-- ############################################################### -->
<param name="robot_description" textfile="/home/vmuser/multicar/ROS_MoveIt/multicar.urdf" />
<param name="use_gui" value="True"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d /home/vmuser/urdf.rviz" required="true" />
</launch>
You can’t perform that action at this time.