close_loop_odom param

idlebear · Mar 24, 2021 · a1a45bb · a1a45bb
1 parent f6b1ee2
commit a1a45bb
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 6 deletions.
diff --git a/champ/include/champ b/champ/include/champ
diff --git a/champ_base/include/state_estimation.h b/champ_base/include/state_estimation.h
@@ -93,7 +93,6 @@ class StateEstimation
     std::string odom_frame_;
     std::string base_footprint_frame_;
     std::string base_link_frame_;
-    bool close_loop_odom_;
     bool orientation_from_imu_;
 
     void publishFootprintToOdom_(const ros::TimerEvent& event);

diff --git a/champ_base/src/state_estimation.cpp b/champ_base/src/state_estimation.cpp
@@ -27,8 +27,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <state_estimation.h>
 
+champ::Odometry::Time rosTimeToChampTime(const ros::Time& time)
+{
+  return time.toNSec() / 1000ul;
+}
+
 StateEstimation::StateEstimation(ros::NodeHandle *nh, ros::NodeHandle *pnh):
-    odometry_(base_)
+    odometry_(base_, rosTimeToChampTime(ros::Time::now()))
 {
     joint_states_subscriber_.subscribe(*nh, "joint_states", 1);
     foot_contacts_subscriber_ .subscribe(*nh, "foot_contacts", 1);
@@ -42,7 +47,6 @@ StateEstimation::StateEstimation(ros::NodeHandle *nh, ros::NodeHandle *pnh):
 
     nh->getParam("links_map/base", base_name_);
     nh->getParam("gait/odom_scaler", gait_config_.odom_scaler);
-    pnh->getParam("close_loop_odom", close_loop_odom_);
     pnh->param("orientation_from_imu", orientation_from_imu_, false);
 
     if (orientation_from_imu_)
@@ -105,7 +109,7 @@ void StateEstimation::imu_callback_(const sensor_msgs::ImuConstPtr& msg)
 
 void StateEstimation::publishFootprintToOdom_(const ros::TimerEvent& event)
 {
-    odometry_.getVelocities(current_velocities_, close_loop_odom_);
+    odometry_.getVelocities(current_velocities_, rosTimeToChampTime(ros::Time::now()));
 
     ros::Time current_time = ros::Time::now();
 

diff --git a/champ_bringup/launch/bringup.launch b/champ_bringup/launch/bringup.launch
@@ -59,7 +59,6 @@
 
         <!-- ==================== STATE ESTIMATION ==================== -->
         <node pkg="champ_base" name="state_estimator" type="state_estimation_node" output="screen">
-            <param name="close_loop_odom" value="$(arg close_loop_odom)" />
             <param name="orientation_from_imu" value="$(arg orientation_from_imu)" />
         </node>