diff --git a/rae_camera/config/camera.yaml b/rae_camera/config/camera.yaml index fb2be26..79283ea 100644 --- a/rae_camera/config/camera.yaml +++ b/rae_camera/config/camera.yaml @@ -5,8 +5,16 @@ i_pipeline_type: rae i_enable_ir: false i_enable_imu: true + i_calibration_dump: false i_pipeline_dump: false + imu: + i_acc_cov: 0.1 + i_gyro_cov: 0.001 + i_batch_report_threshold: 5 + i_max_batch_reports: 5 + i_gyro_freq: 400 + i_acc_freq: 65 nn: i_nn_config_path: /workspaces/rae_ws/src/rae-ros/rae_camera/config/yolo.json i_disable_resize: true diff --git a/rae_hw/config/ekf.yaml b/rae_hw/config/ekf.yaml index a5c4d88..840136e 100644 --- a/rae_hw/config/ekf.yaml +++ b/rae_hw/config/ekf.yaml @@ -4,7 +4,7 @@ ekf_filter_node: # 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. - frequency: 25.0 + frequency: 50.0 # 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 @@ -85,9 +85,9 @@ ekf_filter_node: # do not provide some of the state variables estimated by the filter. 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. - odom0_config: [true, true, false, # X , Y , Z - false, false, false, # roll , pitch ,yaw - false, false, false, # dX , dY , dZ + odom0_config: [false, false, false, # X , Y , Z + false, false,false, # roll , pitch ,yaw + true, false, false, # dX , dY , dZ false, false, false, # droll , dpitch ,dyaw false, false, false] # ddX , ddY , ddZ @@ -124,27 +124,27 @@ ekf_filter_node: # required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. # For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying # the thresholds. - odom0_pose_rejection_threshold: 5.0 - odom0_twist_rejection_threshold: 1.0 + #odom0_pose_rejection_threshold: 5.0 + #odom0_twist_rejection_threshold: 1.0 # Further input parameter examples imu0: imu/data imu0_config: [false, false, false, - true, true, true, + false, false, true, false, false, false, false, false, false, false, false, false] imu0_differential: false - imu0_relative: true - imu0_queue_size: 5 + imu0_relative: false + imu0_queue_size: 1 imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names imu0_twist_rejection_threshold: 0.8 # imu0_linear_acceleration_rejection_threshold: 0.8 # # # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set # # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. - imu0_remove_gravitational_acceleration: true + #imu0_remove_gravitational_acceleration: true # [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During @@ -158,7 +158,7 @@ ekf_filter_node: # predicition. Note that if an acceleration measurement for the variable in question is available from one of the # inputs, the control term will be ignored. # Whether or not we use the control input during predicition. Defaults to false. - use_control: true + use_control: false # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to # false. @@ -199,16 +199,16 @@ ekf_filter_node: 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.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.9, 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.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.0, 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.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, 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, 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, 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, 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, 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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + 0.0, 0.0, 0.0, 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 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.1, 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.1] # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in @@ -220,7 +220,7 @@ ekf_filter_node: 0.0, 0.0, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-7, 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, 1e-9, 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, 1e-9, 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, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, diff --git a/rae_hw/launch/control.launch.py b/rae_hw/launch/control.launch.py index 0102e38..fde0ae7 100644 --- a/rae_hw/launch/control.launch.py +++ b/rae_hw/launch/control.launch.py @@ -44,8 +44,9 @@ def launch_setup(context, *args, **kwargs): parameters=[ {'do_bias_estimation': True}, {'do_adaptive_gain': True}, + {'orientation_stddev': 0.001}, {'use_mag': False}, - {'gain_acc': 0.01}, + {'gain_acc': 0.04}, {'gain_mag': 0.01}, ], remappings=[