Skip to content

Commit

Permalink
Fixed wrong calls
Browse files Browse the repository at this point in the history
  • Loading branch information
Facundo Garcia committed Oct 28, 2024
1 parent 648e00f commit 356524b
Showing 1 changed file with 4 additions and 4 deletions.
8 changes: 4 additions & 4 deletions fixposition_driver_ros2/src/data_to_ros2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -627,15 +627,15 @@ void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::ms

// Compute FP_ENU0 -> FP_POISH
// Extract translation and rotation from ECEFENU0
geometry_msgs::Vector3 trans_ecef_enu0 = tf_map.at("ECEFENU0")->transform.translation;
geometry_msgs::Quaternion rot_ecef_enu0 = tf_map.at("ECEFENU0")->transform.rotation;
geometry_msgs::msg::Vector3 trans_ecef_enu0 = tf_map.at("ECEFENU0")->transform.translation;
geometry_msgs::msg::Quaternion rot_ecef_enu0 = tf_map.at("ECEFENU0")->transform.rotation;
Eigen::Vector3d t_ecef_enu0_;
t_ecef_enu0_ << trans_ecef_enu0.x, trans_ecef_enu0.y, trans_ecef_enu0.z;
Eigen::Quaterniond q_ecef_enu0_(rot_ecef_enu0.w, rot_ecef_enu0.x, rot_ecef_enu0.y, rot_ecef_enu0.z);

// Extract translation and rotation from ECEFPOISH
geometry_msgs::Vector3 trans_ecef_poish = tf_map.at("ECEFPOISH")->transform.translation;
geometry_msgs::Quaternion rot_ecef_poish = tf_map.at("ECEFPOISH")->transform.rotation;
geometry_msgs::msg::Vector3 trans_ecef_poish = tf_map.at("ECEFPOISH")->transform.translation;
geometry_msgs::msg::Quaternion rot_ecef_poish = tf_map.at("ECEFPOISH")->transform.rotation;
Eigen::Vector3d t_ecef_poish;
t_ecef_poish << trans_ecef_poish.x, trans_ecef_poish.y, trans_ecef_poish.z;
Eigen::Quaterniond q_ecef_poish(rot_ecef_poish.w, rot_ecef_poish.x, rot_ecef_poish.y, rot_ecef_poish.z);
Expand Down

0 comments on commit 356524b

Please sign in to comment.