Skip to content

Commit

Permalink
Changed geometry_msgs calls
Browse files Browse the repository at this point in the history
  • Loading branch information
Facundo Garcia committed Oct 28, 2024
1 parent 332a368 commit 648e00f
Show file tree
Hide file tree
Showing 3 changed files with 6 additions and 6 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ void OdomToTf(const OdometryData& data, geometry_msgs::msg::TransformStamped& tf
* @param[out] static_br_
* @param[out] br_
*/
void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::TransformStamped>>& tf_map, std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_br_, std::shared_ptr<tf2_ros::TransformBroadcaster> br_);
void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::msg::TransformStamped>>& tf_map, std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_br_, std::shared_ptr<tf2_ros::TransformBroadcaster> br_);

/**
* @brief
Expand Down
6 changes: 3 additions & 3 deletions fixposition_driver_ros2/src/data_to_ros2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -619,7 +619,7 @@ void OdomToTf(const OdometryData& data, geometry_msgs::msg::TransformStamped& tf
tf2::toMsg(data.pose.position, tf.transform.translation);
}

void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::TransformStamped>>& tf_map, std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_br_, std::shared_ptr<tf2_ros::TransformBroadcaster> br_) {
void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::msg::TransformStamped>>& tf_map, std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_br_, std::shared_ptr<tf2_ros::TransformBroadcaster> br_) {
if (tf_map.at("ECEFENU0") && tf_map.at("POIPOISH") && tf_map.at("ECEFPOISH") && tf_map.at("ENU0POI")) {
// Publish FP_ECEF -> map
tf_map.at("ECEFENU0")->child_frame_id = "map";
Expand Down Expand Up @@ -657,15 +657,15 @@ void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::Tr
tf2::Transform tf_combined = tf_ENU0POI * tf_ENU0POISH.inverse();

// Create a new TransformStamped message
geometry_msgs::TransformStamped tf_map_odom;
geometry_msgs::msg::TransformStamped tf_map_odom;
tf_map_odom.header.stamp = ros::Time::now();
tf_map_odom.header.frame_id = "map";
tf_map_odom.child_frame_id = "odom";
tf_map_odom.transform = tf2::toMsg(tf_combined);
br_->sendTransform(tf_map_odom);

// Publish odom -> base_link
geometry_msgs::TransformStamped tf_odom_base;
geometry_msgs::msg::TransformStamped tf_odom_base;
tf_odom_base.header.stamp = ros::Time::now();
tf_odom_base.header.frame_id = "odom";
tf_odom_base.child_frame_id = "base_link";
Expand Down
4 changes: 2 additions & 2 deletions fixposition_driver_ros2/src/fixposition_driver_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -224,15 +224,15 @@ void FixpositionDriverNode::RegisterObservers() {
// Append TF if Nav2 mode is selected
if (params_.fp_output.nav2_mode) {
// Get FP_POI -> FP_POISH
tf_map["POIPOISH"] = std::make_shared<geometry_msgs::TransformStamped>(tf);
tf_map["POIPOISH"] = std::make_shared<geometry_msgs::msg::TransformStamped>(tf);
}
} else if (tf.child_frame_id == "FP_ENU0" && tf.header.frame_id == "FP_ECEF") {
static_br_->sendTransform(tf);

// Append TF if Nav2 mode is selected
if (params_.fp_output.nav2_mode) {
// Get FP_ECEF -> FP_ENU0
tf_map["ECEFENU0"] = std::make_shared<geometry_msgs::TransformStamped>(tf);
tf_map["ECEFENU0"] = std::make_shared<geometry_msgs::msg::TransformStamped>(tf);
}
} else {
static_br_->sendTransform(tf);
Expand Down

0 comments on commit 648e00f

Please sign in to comment.