Move gps nodes back to wild_thumper.launch
authorErik Andresen <erik@vontaene.de>
Tue, 5 Sep 2017 14:28:06 +0000 (16:28 +0200)
committerErik Andresen <erik@vontaene.de>
Tue, 5 Sep 2017 14:28:06 +0000 (16:28 +0200)
added rosconsole.config

config/robot_localization.yaml
config/rosconsole.config [new file with mode: 0644]
launch/gps.launch [deleted file]
launch/wild_thumper.launch

index ca599f9..630e24d 100644 (file)
@@ -46,13 +46,14 @@ world_frame: odom
 # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This
 # can be used for future dating the transform, which is required for interaction with some other
 # packages. Defaults to 0.0 if unspecified.
 # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This
 # can be used for future dating the transform, which is required for interaction with some other
 # packages. Defaults to 0.0 if unspecified.
-#transform_time_offset: 0.0
+#transform_time_offset: 0.05
 
 # 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.
 odom0: 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.
 odom0: odom
+odom1: gps
 imu0: imu
 imu0_queue_size: 10
 
 imu0: imu
 imu0_queue_size: 10
 
@@ -66,10 +67,15 @@ imu0_queue_size: 10
 # x/y not included because of redundancy with velocities
 # vyaw not included in odom because too inaccurate
 odom0_config: [false, false, false,
 # x/y not included because of redundancy with velocities
 # vyaw not included in odom because too inaccurate
 odom0_config: [false, false, false,
-               false, false, false, 
+               false, false, false,
                true,  true,  true,
                false, false, true,
                false, false, false]
                true,  true,  true,
                false, false, true,
                false, false, false]
+odom1_config: [true,  true,  false,
+               false, false, false,
+               false, false, false,
+               false, false, false,
+               false, false, false]
 imu0_config:  [false, false, false,
                true,  true,   true,
                false, false, false,
 imu0_config:  [false, false, false,
                true,  true,   true,
                false, false, false,
@@ -84,15 +90,16 @@ imu0_config:  [false, false, false,
 #  either increase the covariances for the variables in question, or you can simply set the sensor's differential
 #  parameter to true. When differential mode is enabled, all absolute pose data is converted to velocity data by
 #  differentiating the absolute pose measurements. These velocities are then integrated as usual. NOTE: this only
 #  either increase the covariances for the variables in question, or you can simply set the sensor's differential
 #  parameter to true. When differential mode is enabled, all absolute pose data is converted to velocity data by
 #  differentiating the absolute pose measurements. These velocities are then integrated as usual. NOTE: this only
-#  applies to sensors that provide absolute measurements, so setting differential to true for twit measurements has
+#  applies to sensors that provide absolute measurements, so setting differential to true for twist measurements has
 #  no effect.
 #  no effect.
-#  
+#
 #  Users should take care when setting this to true for orientation variables: if you have only one source of
 #  absolute orientation data, you should not set the differential parameter to true. This is due to the fact that
 #  integration of velocities leads to slowly increasing error in the absolute (pose) variable. For position variables,
 #  this is acceptable. For orientation variables, it can lead to trouble. Users should make sure that all orientation
 #  variables have at least one source of absolute measurement.
 odom0_differential: false
 #  Users should take care when setting this to true for orientation variables: if you have only one source of
 #  absolute orientation data, you should not set the differential parameter to true. This is due to the fact that
 #  integration of velocities leads to slowly increasing error in the absolute (pose) variable. For position variables,
 #  this is acceptable. For orientation variables, it can lead to trouble. Users should make sure that all orientation
 #  variables have at least one source of absolute measurement.
 odom0_differential: false
+odom1_differential: false
 imu0_differential: false
 
 #  When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" for all
 imu0_differential: false
 
 #  When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" for all
@@ -100,6 +107,7 @@ imu0_differential: false
 #  that the relative parameter doesn't cause the measurement to be converted to a velocity before integrating it. If
 #  you simply want your measurements to start at 0 for a given sensor, set this to true.
 odom0_relative: false
 #  that the relative parameter doesn't cause the measurement to be converted to a velocity before integrating it. If
 #  you simply want your measurements to start at 0 for a given sensor, set this to true.
 odom0_relative: false
+odom1_relative: false
 imu0_relative: false
 
 # If we're including accelerations in our state estimate, then we'll probably want to remove any acceleration that
 imu0_relative: false
 
 # If we're including accelerations in our state estimate, then we'll probably want to remove any acceleration that
@@ -113,3 +121,26 @@ print_diagnostics: true
 # If true, will dynamically scale the process_noise_covariance based on the robot?s velocity. This is useful, e.g., when you want your
 # robots estimate error covariance to stop growing when the robot is stationary. Defaults to false.
 dynamic_process_noise_covariance: true
 # If true, will dynamically scale the process_noise_covariance based on the robot?s velocity. This is useful, e.g., when you want your
 # robots estimate error covariance to stop growing when the robot is stationary. Defaults to false.
 dynamic_process_noise_covariance: true
+
+# The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+# However, if users find that a given variable is slow to converge, one approach is to increase the
+# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.
+process_noise_covariance: [0.5,  0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0.5,  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.03, 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.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0.04, 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,    0,     0,     0,    0,    0.01, 0,    0,    0,    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,    0.01, 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,    0,    0,    0,    0,    0.015]
diff --git a/config/rosconsole.config b/config/rosconsole.config
new file mode 100644 (file)
index 0000000..26e0a28
--- /dev/null
@@ -0,0 +1,9 @@
+#
+#   rosconsole will find this file by default at $ROS_ROOT/config/rosconsole.config
+#
+#   You can define your own by e.g. copying this file and setting
+#   ROSCONSOLE_CONFIG_FILE (in your environment) to point to the new file
+#
+log4j.logger.ros=INFO
+log4j.logger.ros.roscpp.superdebug=WARN
+log4j.logger.ros.robot_localization=WARN
diff --git a/launch/gps.launch b/launch/gps.launch
deleted file mode 100644 (file)
index aac62a7..0000000
+++ /dev/null
@@ -1,164 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-       <node pkg="gpsd_client" type="gpsd_client"  name="gpsd_client" output="screen">
-               <param name="use_gps_time" value="false"/>
-               <param name="frame_id" value="base_footprint"/>
-       </node>
-
-       <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" output="screen">
-               <param name="magnetic_declination_radians" value="0.047"/>
-               <param name="yaw_offset" value="0.0"/>
-               <param name="zero_altitude" value="true"/>
-               <param name="broadcast_utm_transform" value="true"/>
-
-               <remap from="/imu/data" to="imu"/>
-               <remap from="/gps/fix" to="fix" />
-               <remap from="/odometry/filtered" to="odom_combined"/>
-               <remap from="/odometry/gps" to="gps"/>
-       </node>
-
-       <node pkg="robot_localization" type="ekf_localization_node" name="ekf_gps" clear_params="true" output="screen">
-               <!-- 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="10"/>
-
-               <!-- 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="2.0"/>
-
-               <!-- 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="true"/>
-
-               <!-- 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="gps"/>
-               <!-- Defaults to "odom" if unspecified -->
-               <param name="odom_frame" value="odom"/>
-               <!-- Defaults to "base_link" if unspecified -->
-               <param name="base_link_frame" value="base_footprint"/>
-               <!-- Defaults to the value of "odom_frame" if unspecified -->
-               <param name="world_frame" value="gps"/>
-
-               <!-- We already have a publisher for odom->base transform -->
-               <param name="publish_tf" value="true"/>
-
-               <!-- Use this parameter to provide an offset to the transform generated by ekf_localization_node. This
-                  can be used for future dating the transform, which is required for interaction with some other
-                  packages. Defaults to 0.0 if unspecified. -->
-               <!-- <param name="transform_time_offset" value="0.0"/> -->
-
-               <!-- 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="odom1" value="gps"/>
-               <param name="imu0" value="imu"/>
-               <rosparam param="imu0_queue_size">10</rosparam>
-
-               <!-- 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. -->
-               <!-- x/y not included because of redundancy with velocities -->
-               <!-- vyaw not included in odom because too inaccurate -->
-               <rosparam param="odom0_config">[false,  false,  false,
-                                                false, false, false,
-                                                true, true, false,
-                                                false, false, true,
-                                                false, false, false]</rosparam>
-               <rosparam param="odom1_config">[true,  true,  false,
-                                                false, false, false,
-                                                false, false, false,
-                                                false, false, false,
-                                                false, false, false]</rosparam>
-               <rosparam param="imu0_config">[false, false, false,
-                                               false, false, true,
-                                               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 nodes 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 may run into problems if your covariances are not large enough, as the sensors will inevitably
-                  diverge from one another, causing the filter to jump back and forth rapidly. To combat this situation, you can
-                  either increase the covariances for the variables in question, or you can simply set the sensor's differential
-                  parameter to true. When differential mode is enabled, all absolute pose data is converted to velocity data by
-                  differentiating the absolute pose measurements. These velocities are then integrated as usual. NOTE: this only
-                  applies to sensors that provide absolute measurements, so setting differential to true for twist measurements has
-                  no effect.
-
-                  Users should take care when setting this to true for orientation variables: if you have only one source of
-                  absolute orientation data, you should not set the differential parameter to true. This is due to the fact that
-                  integration of velocities leads to slowly increasing error in the absolute (pose) variable. For position variables,
-                  this is acceptable. For orientation variables, it can lead to trouble. Users should make sure that all orientation
-                  variables have at least one source of absolute measurement. -->
-               <param name="odom0_differential" value="false"/>
-               <param name="odom1_differential" value="false"/>
-               <param name="imu0_differential" value="false"/>
-
-               <!-- When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" for all
-                  future measurements. While you can achieve the same effect with the differential paremeter, the key difference is
-                  that the relative parameter doesn't cause the measurement to be converted to a velocity before integrating it. If
-                  you simply want your measurements to start at 0 for a given sensor, set this to true. -->
-               <param name="odom0_relative" value="false"/>
-               <param name="odom1_relative" value="false"/>
-               <param name="imu0_relative" 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="true"/>
-
-               <!-- If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see
-                  if the node is unhappy with any settings or data. -->
-               <param name="print_diagnostics" value="true"/>
-
-               <remap from="odometry/filtered" to="gps_combined"/>
-
-               <rosparam param="process_noise_covariance">
-                       [0.01, 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,    0,    0,
-                        0,    0,    0.06, 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.03, 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.025, 0,     0,    0,    0,    0,    0,    0,    0,
-                        0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
-                        0,    0,    0,    0,    0,    0,    0,     0,     0.04, 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,    0,     0,     0,    0,    0.01, 0,    0,    0,    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,    0.01, 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,    0,    0,    0,    0,    0.015]
-                </rosparam>
-       </node>
-</launch>
index 8736acc..1d304d8 100644 (file)
@@ -1,5 +1,7 @@
 <?xml version="1.0"?>
 <launch>
 <?xml version="1.0"?>
 <launch>
+       <env name="ROSCONSOLE_CONFIG_FILE" value="$(find wild_thumper)/config/rosconsole.config"/>
+
        <arg name="use_imu" default="true"/>
 
        <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
        <arg name="use_imu" default="true"/>
 
        <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
                <arg name="nodelet_manager_name" value="nodelet_manager" />
        </include>
 
                <arg name="nodelet_manager_name" value="nodelet_manager" />
        </include>
 
-       <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
+       <node pkg="robot_localization" type="ekf_localization_node" name="ekf" clear_params="true" output="screen">
                <rosparam command="load" file="$(find wild_thumper)/config/robot_localization.yaml"/>
                <remap from="odometry/filtered" to="odom_combined"/>
        </node>
                <rosparam command="load" file="$(find wild_thumper)/config/robot_localization.yaml"/>
                <remap from="odometry/filtered" to="odom_combined"/>
        </node>
+
+       <node pkg="gpsd_client" type="gpsd_client"  name="gpsd_client" output="screen">
+               <param name="use_gps_time" value="false"/>
+               <param name="frame_id" value="base_footprint"/>
+       </node>
+
+       <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" output="screen">
+               <param name="magnetic_declination_radians" value="0.047"/>
+               <param name="yaw_offset" value="0.0"/>
+               <param name="zero_altitude" value="true"/>
+               <param name="broadcast_utm_transform" value="true"/>
+
+               <remap from="/imu/data" to="imu"/>
+               <remap from="/gps/fix" to="fix" />
+               <remap from="/odometry/filtered" to="odom_combined"/>
+               <remap from="/odometry/gps" to="gps"/>
+       </node>
 </launch>
 </launch>