change_notifier.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
32 #include "ros/ros.h"
33 #include "tf/transform_listener.h"
34 #include "xmlrpcpp/XmlRpcValue.h"
35 
36 class FramePair
37 {
38 public:
39  FramePair(const std::string& source_frame, const std::string& target_frame, double translational_update_distance, double angular_update_distance) :
40  source_frame_(source_frame),
41  target_frame_(target_frame),
42  translational_update_distance_(translational_update_distance),
43  angular_update_distance_(angular_update_distance)
44  {
46  }
47 
48 public:
49  std::string source_frame_;
50  std::string target_frame_;
51 
55 
58 };
59 
60 bool getFramePairs(const ros::NodeHandle& local_node, std::vector<FramePair>& frame_pairs, double default_translational_update_distance, double default_angular_update_distance)
61 {
62  XmlRpc::XmlRpcValue frame_pairs_param;
63  if (!local_node.getParam("frame_pairs", frame_pairs_param))
64  {
65  // No frame_pairs parameter provided. Default to base_link->map.
66  frame_pairs.push_back(FramePair("base_link", "map", default_translational_update_distance, default_angular_update_distance));
67  return true;
68  }
69 
70  if (frame_pairs_param.getType() != XmlRpc::XmlRpcValue::TypeArray)
71  {
72  ROS_ERROR("Expecting a list for frame_pairs parameter");
73  return false;
74  }
75  for (int i = 0; i < frame_pairs_param.size(); i++)
76  {
77  XmlRpc::XmlRpcValue frame_pair_param = frame_pairs_param[i];
78  if (frame_pair_param.getType() != XmlRpc::XmlRpcValue::TypeStruct)
79  {
80  ROS_ERROR("frame_pairs must be specified as maps, but they are XmlRpcType: %d", frame_pair_param.getType());
81  return false;
82  }
83 
84  // Get the source_frame
85  if (!frame_pair_param.hasMember("source_frame"))
86  {
87  ROS_ERROR("frame_pair does not specified source_frame");
88  return false;
89  }
90  XmlRpc::XmlRpcValue source_frame_param = frame_pair_param["source_frame"];
91  if (source_frame_param.getType() != XmlRpc::XmlRpcValue::TypeString)
92  {
93  ROS_ERROR("source_frame must be a string, but it is XmlRpcType: %d", source_frame_param.getType());
94  return false;
95  }
96  std::string source_frame = source_frame_param;
97 
98  // Get the target_frame
99  if (!frame_pair_param.hasMember("target_frame"))
100  {
101  ROS_ERROR("frame_pair does not specified target_frame");
102  return false;
103  }
104  XmlRpc::XmlRpcValue target_frame_param = frame_pair_param["target_frame"];
105  if (target_frame_param.getType() != XmlRpc::XmlRpcValue::TypeString)
106  {
107  ROS_ERROR("target_frame must be a string, but it is XmlRpcType: %d", target_frame_param.getType());
108  return false;
109  }
110  std::string target_frame = target_frame_param;
111 
112  // Get the (optional) translational_update_distance
113  double translational_update_distance = default_translational_update_distance;
114  if (frame_pair_param.hasMember("translational_update_distance"))
115  {
116  XmlRpc::XmlRpcValue translational_update_distance_param = frame_pair_param["translational_update_distance"];
117  if (translational_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeDouble &&
118  translational_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeInt)
119  {
120  ROS_ERROR("translational_update_distance must be either an integer or a double, but it is XmlRpcType: %d", translational_update_distance_param.getType());
121  return false;
122  }
123  translational_update_distance = translational_update_distance_param;
124  }
125 
126  // Get the (optional) angular_update_distance
127  double angular_update_distance = default_angular_update_distance;
128  if (frame_pair_param.hasMember("angular_update_distance"))
129  {
130  XmlRpc::XmlRpcValue angular_update_distance_param = frame_pair_param["angular_update_distance"];
131  if (angular_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeDouble &&
132  angular_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeInt)
133  {
134  ROS_ERROR("angular_update_distance must be either an integer or a double, but it is XmlRpcType: %d", angular_update_distance_param.getType());
135  return false;
136  }
137  angular_update_distance = angular_update_distance_param;
138  }
139 
140  ROS_INFO("Notifying change on %s -> %s (translational update distance: %.4f, angular update distance: %.4f)", source_frame.c_str(), target_frame.c_str(), translational_update_distance, angular_update_distance);
141 
142  frame_pairs.push_back(FramePair(source_frame, target_frame, translational_update_distance, angular_update_distance));
143  }
144 
145  return true;
146 }
147 
152 int main(int argc, char** argv)
153 {
154  ros::init(argc, argv, "change_notifier", ros::init_options::AnonymousName);
155  ros::NodeHandle node;
156  ros::NodeHandle local_node("~");
157 
158  double polling_frequency, translational_update_distance, angular_update_distance;
159  local_node.param(std::string("polling_frequency"), polling_frequency, 10.0);
160  local_node.param(std::string("translational_update_distance"), translational_update_distance, 0.10);
161  local_node.param(std::string("angular_update_distance"), angular_update_distance, 0.10);
162 
163  std::vector<FramePair> frame_pairs;
164  if (!getFramePairs(local_node, frame_pairs, translational_update_distance, angular_update_distance))
165  return 1;
166 
167  tf::TransformListener tfl(node);
168 
169  // Advertise the service
170  ros::Publisher pub = node.advertise<tf::tfMessage>("tf_changes", 1, true);
171 
172  while (node.ok())
173  {
174  try
175  {
176  tf::tfMessage msg;
177 
178  for (std::vector<FramePair>::iterator i = frame_pairs.begin(); i != frame_pairs.end(); i++)
179  {
180  FramePair& fp = *i;
181 
183 
184  const tf::Vector3& origin = fp.pose_out_.getOrigin();
185  const tf::Quaternion& rotation = fp.pose_out_.getRotation();
186 
187  if (origin.distance(fp.last_sent_pose_.getOrigin()) > fp.translational_update_distance_ ||
188  rotation.angle(fp.last_sent_pose_.getRotation()) > fp.angular_update_distance_)
189  {
190  fp.last_sent_pose_ = fp.pose_out_;
191 
192  tf::StampedTransform stampedTf(tf::Transform(rotation, origin), fp.pose_out_.stamp_, "/" + fp.target_frame_, "/" + fp.source_frame_);
193  geometry_msgs::TransformStamped msgtf;
194  transformStampedTFToMsg(stampedTf, msgtf);
195  msg.transforms.push_back(msgtf);
196  }
197  }
198 
199  if (msg.transforms.size() > 0)
200  pub.publish(msg);
201  }
202  catch (tf::TransformException& ex)
203  {
204  ROS_DEBUG("Exception: %s\n", ex.what());
205  }
206 
207  // Sleep until next polling
208  if (polling_frequency > 0)
209  ros::Duration().fromSec(1.0 / polling_frequency).sleep();
210  }
211 
212  return 0;
213 }
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
Transform a Stamped Pose Message into the target frame This can throw all that lookupTransform can th...
FramePair(const std::string &source_frame, const std::string &target_frame, double translational_update_distance, double angular_update_distance)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
tf::Stamped< tf::Pose > last_sent_pose_
tf::Stamped< tf::Pose > pose_in_
tfScalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
Definition: Quaternion.h:213
bool getFramePairs(const ros::NodeHandle &local_node, std::vector< FramePair > &frame_pairs, double default_translational_update_distance, double default_angular_update_distance)
static tf::Quaternion createIdentityQuaternion()
construct an Identity Quaternion
tf::Transform Pose
double translational_update_distance_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Type const & getType() const
void publish(const boost::shared_ptr< M > &message) const
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
Duration & fromSec(double t)
int main(int argc, char **argv)
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
double angular_update_distance_
std::string source_frame_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string target_frame_
This class inherits from Transformer and automatically subscribes to ROS transform messages...
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
convert tf::StampedTransform to TransformStamped msg
bool ok() const
#define ROS_ERROR(...)
tf::Stamped< tf::Pose > pose_out_
bool hasMember(const std::string &name) const
The Stamped Transform datatype used by tf.
#define ROS_DEBUG(...)


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Feb 28 2022 22:26:19