tf_manager.cpp
Go to the documentation of this file.
2 
4 {
6 {
7  if (!getParam())
8  {
9  throw std::runtime_error("ERROR getting TFManager's params");
10  }
11 }
12 
13 bool TFManager::getPoseInFrame(const std::string &frame,
14  const geometry_msgs::PoseStamped &pose,
15  geometry_msgs::PoseStamped &out)
16 {
17  int attempts = 0;
18  bool success = false;
19  std::string error;
20  while (attempts < max_tf_attempts_)
21  {
22  try
23  {
24  listener_.transformPose(frame, pose, out);
25  success = true;
26  break;
27  }
28  catch (tf::TransformException ex)
29  {
30  attempts++;
31  ros::Duration(0.1).sleep();
32  error = ex.what();
33  }
34  }
35 
36  if (!success)
37  {
38  ROS_ERROR_STREAM("TFManager: Failed to get the target pose from frame "
39  << pose.header.frame_id << " in frame " << frame
40  << ". Exception: " << error);
41  }
42 
43  return success;
44 }
45 
47 {
48  if (!nh_.getParam("tf_manager/max_attempts", max_tf_attempts_))
49  {
50  ROS_WARN("TFManager: Missing max_attempts parameter, setting default");
51  max_tf_attempts_ = 5;
52  }
53 
54  return true;
55 }
56 } // namespace generic_control_toolbox
bool getPoseInFrame(const std::string &frame, const geometry_msgs::PoseStamped &pose, geometry_msgs::PoseStamped &out)
Definition: tf_manager.cpp:13
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
tf::TransformListener listener_
Definition: tf_manager.hpp:38
#define ROS_WARN(...)
TFManager(ros::NodeHandle nh=ros::NodeHandle("~"))
Definition: tf_manager.cpp:5
bool getParam(const std::string &key, std::string &s) const
bool sleep() const
#define ROS_ERROR_STREAM(args)


generic_control_toolbox
Author(s): diogo
autogenerated on Mon Feb 28 2022 22:24:37