fix sbg translation and odom estimation

src/hardware/sbg_translator/src/node_sbg_translator.cpp

@@ -13,6 +13,9 @@ SBGTranslate::SBGTranslate() : Node("sbg_translator_node") {
  this->ekf_odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
  "/imu/odometry", 1, std::bind(&SBGTranslate::ekf_odom_callback, this, _1));
 
+ // this->imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
+ // "/imu/data", 1, std::bind(&SBGTranslate::imu_data_callback, this, _1));
+
  // Odometry
  this->odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/sbg_translated/odometry", 1);
 
@@ -38,6 +41,18 @@ SBGTranslate::SBGTranslate() : Node("sbg_translator_node") {
  RCLCPP_INFO(this->get_logger(), "---SBG Odom Converter Node Initialised---");
 }
 
+// void SBGTranslate::imu_data_callback(sensor_msgs::msg::Imu imu_data_msg) {
+// sensor_msgs::msg::Imu imu_msg = imu_data_msg;
+
+// // orient the position delta by the last yaw
+// double yaw = quat_to_euler(imu_msg.orientation.z);
+
+// imu_msg.orientation = euler_to_quat(0.0, 0.0, -yaw);
+// imu_msg.angular_velocity.z = -imu_msg.angular_velocity.z;
+
+// imu_pub_->publish(imu_msg);
+// }
+
 void SBGTranslate::update_odom() {
  // only update if all messages have been received
  if (!received_imu_ || !received_nav_ || !received_euler_) {
@@ -233,7 +248,7 @@ void SBGTranslate::ekf_odom_callback(const nav_msgs::msg::Odometry::SharedPtr ms
  // initialize yaw
  // convert quat to euler
  double yaw = quat_to_euler(msg->pose.pose.orientation);
- init_yaw_ = yaw;
+ init_yaw_ = -yaw;
 
  // initialize position
  last_x_ = msg->pose.pose.position.x;
@@ -259,7 +274,7 @@ void SBGTranslate::ekf_odom_callback(const nav_msgs::msg::Odometry::SharedPtr ms
 
  double magnitude = sqrt(update_x * update_x + update_y * update_y);
 
- double delta_yaw = yaw - init_yaw_ - state_[2];
+ double delta_yaw = -yaw - init_yaw_ - state_[2];
  double delta_x = magnitude * cos(state_[2]);
  double delta_y = magnitude * sin(state_[2]);
 
@@ -290,12 +305,13 @@ void SBGTranslate::ekf_odom_callback(const nav_msgs::msg::Odometry::SharedPtr ms
  odom_msg.twist.twist.linear.x = vel_magnitude * cos(state_[2]);
  odom_msg.twist.twist.linear.y = vel_magnitude * sin(state_[2]);
  odom_msg.twist.twist.linear.z = 0.0;
+ odom_msg.twist.twist.angular.z = msg->twist.twist.angular.z;
 
  // publish the odom msg
  odom_pub_->publish(odom_msg);
 
  // publish imu message
- imu_pub_->publish(make_imu_msg(odom_msg));
+ // imu_pub_->publish(make_imu_msg(odom_msg));
 
  // publish pose message
  pose_pub_->publish(make_pose_msg(odom_msg));

src/hardware/sensors/config/ellipse_D.yaml

@@ -75,7 +75,7 @@
  # 3 MARINE Used in marine and underwater applications
  # 4 AIRPLANE For fixed wings aircraft
  # 5 HELICOPTER For rotary wing aircraft
- motionProfile: 5
+ motionProfile: 1
 
  # IMU_ALIGNMENT_LEVER_ARM
  imuAlignementLeverArm:
@@ -204,11 +204,11 @@
  # 0 Measurement is not taken into account
  # 1 Measurement is rejected if inconsistent with current estimate (depending on error model)
  # 2 Measurement is always accepted
- posRejectMode: 1
+ posRejectMode: 2
  # Rejection mode for velocity (see posRejectMode values)
- velRejectMode: 1
+ velRejectMode: 2
  # Rejection mode for true heading (see posRejectMode values)
- hdtRejectMode: 1
+ hdtRejectMode: 2
 
  # Odometer configuration
  odom:

src/navigation/nav_bringup/config/localisation_params.yaml

@@ -22,6 +22,19 @@ odom_filter_node:
  false, false, false, # roll_dot, pitch_dot, yaw_dot
  false, false, false] # x_ddot, y_ddot, z_ddot
 
+ # imu0: example/imu
+ # imu0_config: [false, false, false,
+ # false, false, true,
+ # false, false, false,
+ # false, false, true,
+ # false, false, false]
+ # imu0_differential: false
+ # imu0_relative: true
+ # imu0_queue_size: 5
+ # 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 #
+
  # twist0: vehicle/wheel_twist
  # twist0_config: [false, false, false, # x, y, z
  # false, false, false, # roll, pitch, yaw
@@ -33,7 +46,7 @@ odom_filter_node:
  odom1_config: [true, true, false, # x, y, z
  false, false, true, # roll, pitch, yaw
  true, true, false, # x_dot, y_dot, z_dot
- false, false, true, # roll_dot, pitch_dot, yaw_dot
+ false, false, false, # roll_dot, pitch_dot, yaw_dot
  false, false, false] # x_ddot, y_ddot, z_ddot
 
  # odom2: odometry/gps

src/navigation/nav_bringup/config/slam_toolbox_params.yaml

@@ -25,10 +25,10 @@ slam_toolbox_node:
  transform_publish_period: 0.01 # The map to odom transform publish period. 0 will not publish transforms
  map_update_interval: 3.0 # Interval to update the 2D occupancy map for other applications / visualization
  enable_interactive_mode: false # Interactive mode will retain a cache of laser scans mapped to their ID for visualization in interactive mode
- position_covariance_scale: 1.5 # This can be used to tune the influence of the pose position in a downstream localization filter
- yaw_covariance_scale: 1.0 # Pose yaw
+ position_covariance_scale: 2.0 # This can be used to tune the influence of the pose position in a downstream localization filter
+ yaw_covariance_scale: 1.5 # Pose yaw
  resolution: 0.15 # Resolution of the 2D occupancy map to generate
- max_laser_range: 35.0 # Maximum laser range to use for 2D occupancy map rastering
+ max_laser_range: 25.0 # Maximum laser range to use for 2D occupancy map rastering
  minimum_time_interval: 0.5 # The minimum duration of time between scans to be processed in synchronous mode
  transform_timeout: 0.5 # TF timeout for looking up transforms
  tf_buffer_duration: 0.1 # Duration to store TF messages for lookup. Set high if running offline at multiple times speed in synchronous mode
@@ -45,8 +45,8 @@ slam_toolbox_node:
  scan_buffer_maximum_scan_distance: 10.0 # Maximum distance of a scan from the pose before removing the scan from the buffer
 
  link_match_minimum_response_fine: 0.1 # The threshold link matching algorithm response for fine resolution to pass
- link_scan_maximum_distance: 1.5 # Maximum distance between linked scans to be valid
- loop_search_maximum_distance: 3.0 # Maximum threshold of distance for scans to be considered for loop closure
+ link_scan_maximum_distance: 2.5 # Maximum distance between linked scans to be valid
+ loop_search_maximum_distance: 4.0 # Maximum threshold of distance for scans to be considered for loop closure
  do_loop_closing: true # Whether to do loop closure
 
  loop_match_minimum_chain_size: 10 # The minimum chain length of scans to look for loop closure