Skip to content

Commit

Permalink
Changed the type of ekf_to_odom and add const
Browse files Browse the repository at this point in the history
Signed-off-by: Shintaro Sakoda <[email protected]>
  • Loading branch information
SakodaShintaro committed Nov 1, 2023
1 parent 048c63f commit 4b2ebe7
Showing 1 changed file with 3 additions and 6 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -127,12 +127,9 @@ void PoseInstabilityDetector::callback_timer()

// compare pose and latest_odometry_
const Pose latest_ekf_pose = latest_odometry_.pose.pose;
PoseStamped ekf_to_odom;
ekf_to_odom.pose = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose);
ekf_to_odom.header.stamp = latest_odometry_.header.stamp;
ekf_to_odom.header.frame_id = "base_link";
const geometry_msgs::msg::Point pos = ekf_to_odom.pose.position;
const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.pose.orientation);
const Pose ekf_to_odom = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose);
const geometry_msgs::msg::Point pos = ekf_to_odom.position;
const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.orientation);
const std::vector<double> values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z};

// publish debug
Expand Down

0 comments on commit 4b2ebe7

Please sign in to comment.