[Estimation][VO] refactor the throttle process

jsk-ros-pkg · Mar 13, 2023 · 0c58abf · 0c58abf
1 parent 06cbda3
commit 0c58abf
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 6 deletions.
diff --git a/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h b/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h
@@ -75,6 +75,7 @@ namespace sensor_plugin
     ros::Timer  servo_control_timer_;
 
     /* ros param */
+    double throttle_rate_;
     double level_pos_noise_sigma_;
     double z_pos_noise_sigma_;
     double vel_noise_sigma_;

diff --git a/aerial_robot_estimation/src/sensor/vo.cpp b/aerial_robot_estimation/src/sensor/vo.cpp
@@ -44,9 +44,6 @@ namespace
   tf::Transform prev_sensor_tf;
   tf::Vector3 baselink_omega;
   tf::Matrix3x3 baselink_r;
-
-  //debug
-  double sample_interval = 0.02; //50 hz
 }
 
 namespace sensor_plugin
@@ -99,8 +96,6 @@ namespace sensor_plugin
 
   void VisualOdometry::voCallback(const nav_msgs::Odometry::ConstPtr & vo_msg)
   {
-    //if(vo_msg->header.stamp.toSec() - prev_timestamp_ < sample_interval) return;
-
     /* only do egmotion estimate mode */
     if(!getFuserActivate(aerial_robot_estimation::EGOMOTION_ESTIMATE))
       {
@@ -139,9 +134,19 @@ namespace sensor_plugin
     tf::Transform raw_sensor_tf;
     tf::poseMsgToTF(vo_msg->pose.pose, raw_sensor_tf); // motion update
 
-    curr_timestamp_ = vo_msg->header.stamp.toSec() + delay_; //temporal update
+    /* temporal update */
+    curr_timestamp_ = vo_msg->header.stamp.toSec() + delay_;
     reference_timestamp_ = curr_timestamp_;
 
+    /* throttle message */
+    if(throttle_rate_ > 0)
+      {
+        if (curr_timestamp_ - prev_timestamp_ < 1 / throttle_rate_)
+          {
+            return;
+          }
+      }
+
     if(getStatus() == Status::INACTIVE)
       {
         /* for z */
@@ -598,6 +603,7 @@ namespace sensor_plugin
     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);
+    getParam<double>("throttle_rate", throttle_rate_, 0.0);
     getParam<double>("level_pos_noise_sigma", level_pos_noise_sigma_, 0.01 );
     getParam<double>("z_pos_noise_sigma", z_pos_noise_sigma_, 0.01 );
     getParam<double>("vel_noise_sigma", vel_noise_sigma_, 0.05 );