From f8200d8853bff765973dece48a259f2ba133b072 Mon Sep 17 00:00:00 2001 From: kohtake Date: Tue, 2 May 2023 23:57:27 +0900 Subject: [PATCH] fix output value of /odom/raw --- champ_base/src/state_estimation.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/champ_base/src/state_estimation.cpp b/champ_base/src/state_estimation.cpp index 09d1a3bf..0909a240 100644 --- a/champ_base/src/state_estimation.cpp +++ b/champ_base/src/state_estimation.cpp @@ -36,6 +36,9 @@ StateEstimation::StateEstimation(): Node("state_estimation_node",rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true)), + x_pos_(0.0), + y_pos_(0.0), + heading_(0.0), clock_(*this->get_clock()), odometry_(base_, rosTimeToChampTime(clock_.now())) { @@ -151,7 +154,7 @@ void StateEstimation::publishFootprintToOdom_() rclcpp::Time current_time = clock_.now(); - double vel_dt = (current_time - last_vel_time_).nanoseconds()/1e-9; + double vel_dt = (current_time - last_vel_time_).nanoseconds() * 1e-9; //s last_vel_time_ = current_time; //rotate in the z axis //https://en.wikipedia.org/wiki/Rotation_matrix @@ -414,4 +417,4 @@ void StateEstimation::publishBaseToFootprint_() pose_msg.pose.pose.orientation.w = -quaternion.w(); base_to_footprint_publisher_->publish(pose_msg); -} \ No newline at end of file +}