SMACC2
cb_wait_transform.cpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
21
22namespace cl_nav2z
23{
25 std::string targetFrame, std::string referenceFrame, rclcpp::Duration timeout)
26: targetFrame_(targetFrame), referenceFrame_(referenceFrame), timeout_(timeout)
27{
28}
29
31
33{
34 RCLCPP_INFO(
35 getLogger(), "[CbWaitTransform] ref %s -> target %s", referenceFrame_.c_str(),
36 targetFrame_.c_str());
37
38 tfBuffer_ = std::make_shared<tf2_ros::Buffer>(getNode()->get_clock());
39 tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
40
41 tf2::Stamped<tf2::Transform> transform;
42 try
43 {
44 auto transformstamped =
45 tfBuffer_->lookupTransform(targetFrame_, referenceFrame_, getNode()->now(), timeout_);
46 tf2::fromMsg(transformstamped, transform);
47
48 result_ = transform;
49
50 RCLCPP_INFO(
51 getLogger(), "[CbWaitTransform] Success wait transform ref %s -> target %s",
52 referenceFrame_.c_str(), targetFrame_.c_str());
53 this->postSuccessEvent();
54 }
55 catch (tf2::TransformException & ex)
56 {
57 RCLCPP_ERROR_STREAM(
58 getLogger(), "[CbWaitTransform] Failure waiting transform ( ref "
59 << targetFrame_ << "/ target " << referenceFrame_ << " - " << ex.what());
60 this->postFailureEvent();
61 }
62}
63} // namespace cl_nav2z
std::shared_ptr< tf2_ros::TransformListener > tfListener_
CbWaitTransform(std::string targetFrame, std::string referenceFrame, rclcpp::Duration timeout)
std::shared_ptr< tf2_ros::Buffer > tfBuffer_
std::optional< tf2::Stamped< tf2::Transform > > result_
virtual rclcpp::Node::SharedPtr getNode()