Skip to content

Commit

Permalink
static tf broadcaster: workaround transient local durability
Browse files Browse the repository at this point in the history
Transient local durability will be supported in Eloquent. This patch
workaround this issue till adaption to Eloquent.

ros2/geometry2#183

Signed-off-by: Sharron LIU <sharron.liu@intel.com>
  • Loading branch information
sharronliu committed Oct 24, 2019
1 parent 88f1bdd commit 81dc962
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 4 deletions.
4 changes: 2 additions & 2 deletions realsense_ros/include/realsense/rs_base.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ class RealSenseBase
std::string base_frame_id_;

std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;

rclcpp::TimerBase::SharedPtr timer_;
std::map<stream_index_pair, bool> enable_ = {{COLOR, false}, {DEPTH, false},
{INFRA1, false}, {INFRA2, false},
{ACCEL, false}, {GYRO, false},
Expand All @@ -119,4 +119,4 @@ class RealSenseBase
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
};
} // namespace realsense
#endif // REALSENSE__RS_BASE_HPP_
#endif // REALSENSE__RS_BASE_HPP_
8 changes: 6 additions & 2 deletions realsense_ros/src/rs_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,11 @@ void RealSenseBase::startPipeline()

if (enable_[DEPTH] == true) {
auto base_profile = p_profile.get_stream(RS2_STREAM_DEPTH, 0);
publishStaticTransforms(base_profile, active_profiles);
auto pub_tf = [this, base_profile, active_profiles]() -> void {
this->publishStaticTransforms(base_profile, active_profiles);
};

timer_ = node_.create_wall_timer(std::chrono::seconds(1), pub_tf);
} else if (enable_[POSE] == true) {
auto base_profile = p_profile.get_stream(RS2_STREAM_POSE, 0);
publishStaticTransforms(base_profile, active_profiles);
Expand Down Expand Up @@ -270,7 +274,7 @@ void RealSenseBase::composeTFMsgAndPublish(const rclcpp::Time & t, const Float3
const std::string & to)
{
geometry_msgs::msg::TransformStamped msg;
RCLCPP_INFO(node_.get_logger(), "Publish Static TF from %s to %s", from.c_str(), to.c_str());
RCLCPP_DEBUG(node_.get_logger(), "Publish Static TF from %s to %s", from.c_str(), to.c_str());
msg.header.stamp = t;
msg.header.frame_id = from;
msg.child_frame_id = to;
Expand Down

0 comments on commit 81dc962

Please sign in to comment.