[Estimation][VO][WIP] refactor the process for velocity related to th…

…e reference frame (local or global)

aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h

@@ -85,11 +85,13 @@ namespace sensor_plugin
     int fusion_mode_;
     bool vio_mode_; // visual + inertial mode
     bool z_vel_mode_;
+    bool local_vel_mode_;
     bool outdoor_no_vel_time_sync_; // very special flag for stereo cam such as zed mini, which has tricky behavier in outdoor mode
     /* heuristic sepecial flag for fusion */
     bool outdoor_;
     bool z_no_delay_;
 
+
     /* servo */
     std::string joint_name_;
     bool servo_auto_change_flag_;

aerial_robot_estimation/src/sensor/vo.cpp

@@ -353,8 +353,8 @@ namespace sensor_plugin
     tf::quaternionMsgToTF(vo_msg->pose.pose.orientation, raw_q);
 
     // velocity:
-    tf::Vector3 raw_local_vel;
-    tf::vector3MsgToTF(vo_msg->twist.twist.linear, raw_local_vel);
+    tf::Vector3 raw_vel;
+    tf::vector3MsgToTF(vo_msg->twist.twist.linear, raw_vel);
     /* get the latest orientation and omega */
     baselink_r = estimator_->getOrientation(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE);
     baselink_omega = estimator_->getAngularVel(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE);
@@ -365,11 +365,11 @@ namespace sensor_plugin
         // int mode = estimator_->getStateStatus(State::YAW_BASE, aerial_robot_estimation::GROUND_TRUTH)?aerial_robot_estimation::GROUND_TRUTH:aerial_robot_estimation::EGOMOTION_ESTIMATE;
         int mode = aerial_robot_estimation::EGOMOTION_ESTIMATE;
 
-        if (raw_local_vel == tf::Vector3(0.0,0.0,0.0))
+        if (raw_vel == tf::Vector3(0.0,0.0,0.0))
           {
             /* the odometry message does not contain velocity information, we have to calulcate by ourselves. */
             tf::Transform delta_tf = prev_sensor_tf.inverse() * raw_sensor_tf;
-            raw_local_vel = delta_tf.getOrigin() / (curr_timestamp_ - prev_timestamp_);
+            raw_vel = delta_tf.getOrigin() / (curr_timestamp_ - prev_timestamp_);
 
             reference_timestamp_ = (curr_timestamp_ + prev_timestamp_) / 2;
             estimator_->findRotOmega(reference_timestamp_, mode, baselink_r, baselink_omega);
@@ -384,7 +384,16 @@ namespace sensor_plugin
           }
       }
 
-    raw_global_vel_ = baselink_r * ( sensor_tf_.getBasis() * raw_local_vel - baselink_omega.cross(sensor_tf_.getOrigin()));
+    raw_global_vel_ = world_offset_tf_.getBasis() * raw_vel;
+    if (local_vel_mode_)
+      {
+        // if the velocity is described in local frame (i.e., the sensor frame),
+        // we need to convert to global one
+        raw_global_vel_ = baselink_r * sensor_tf_.getBasis() * raw_vel;
+      }
+    // consider the offset between baselink and sensor frames
+    raw_global_vel_ -= baselink_r * baselink_omega.cross(sensor_tf_.getOrigin());
+
 
     if(debug_verbose_)
       {
@@ -600,6 +609,7 @@ namespace sensor_plugin
   {
     getParam<int>("fusion_mode", fusion_mode_, (int)ONLY_POS_MODE);
     getParam<bool>("vio_mode", vio_mode_, false);
+    getParam<bool>("local_vel_mode", local_vel_mode_, true);
     getParam<bool>("z_vel_mode", z_vel_mode_, false);
     getParam<bool>("z_no_delay", z_no_delay_, false);
     getParam<bool>("outdoor_no_vel_time_sync", outdoor_no_vel_time_sync_, false);