change_notifier.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00032 #include "ros/ros.h"
00033 #include "tf/transform_listener.h"
00034 #include "XmlRpcValue.h"
00035 
00036 class FramePair
00037 {
00038 public:
00039   FramePair(const std::string& source_frame, const std::string& target_frame, double translational_update_distance, double angular_update_distance) : 
00040     source_frame_(source_frame), 
00041     target_frame_(target_frame),
00042     translational_update_distance_(translational_update_distance),
00043     angular_update_distance_(angular_update_distance)
00044   {
00045     pose_in_ = tf::Stamped<tf::Pose>(tf::Pose(tf::createIdentityQuaternion(), tf::Vector3(0, 0, 0)), ros::Time(), source_frame_);
00046   }
00047 
00048 public:
00049   std::string source_frame_;
00050   std::string target_frame_;
00051 
00052   tf::Stamped<tf::Pose> pose_in_;
00053   tf::Stamped<tf::Pose> pose_out_;
00054   tf::Stamped<tf::Pose> last_sent_pose_;
00055   
00056   double translational_update_distance_;
00057   double angular_update_distance_;
00058 };
00059 
00060 bool getFramePairs(const ros::NodeHandle& local_node, std::vector<FramePair>& frame_pairs, double default_translational_update_distance, double default_angular_update_distance)
00061 {
00062   XmlRpc::XmlRpcValue frame_pairs_param;
00063   if (!local_node.getParam("frame_pairs", frame_pairs_param))
00064   {
00065     // No frame_pairs parameter provided. Default to base_link->map.
00066     frame_pairs.push_back(FramePair("base_link", "map", default_translational_update_distance, default_angular_update_distance));
00067     return true;
00068   }
00069 
00070   if (frame_pairs_param.getType() != XmlRpc::XmlRpcValue::TypeArray)
00071   {
00072     ROS_ERROR("Expecting a list for frame_pairs parameter");
00073     return false;
00074   }
00075   for (int i = 0; i < frame_pairs_param.size(); i++)
00076   {
00077     XmlRpc::XmlRpcValue frame_pair_param = frame_pairs_param[i];
00078     if (frame_pair_param.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00079     {
00080       ROS_ERROR("frame_pairs must be specified as maps, but they are XmlRpcType: %d", frame_pair_param.getType());
00081       return false;
00082     }
00083 
00084     // Get the source_frame
00085     if (!frame_pair_param.hasMember("source_frame"))
00086     {
00087       ROS_ERROR("frame_pair does not specified source_frame");
00088       return false;
00089     }
00090     XmlRpc::XmlRpcValue source_frame_param = frame_pair_param["source_frame"];
00091     if (source_frame_param.getType() != XmlRpc::XmlRpcValue::TypeString)
00092     {
00093       ROS_ERROR("source_frame must be a string, but it is XmlRpcType: %d", source_frame_param.getType());
00094       return false;
00095     }
00096     std::string source_frame = source_frame_param;
00097 
00098     // Get the target_frame
00099     if (!frame_pair_param.hasMember("target_frame"))
00100     {
00101       ROS_ERROR("frame_pair does not specified target_frame");
00102       return false;
00103     }
00104     XmlRpc::XmlRpcValue target_frame_param = frame_pair_param["target_frame"];
00105     if (target_frame_param.getType() != XmlRpc::XmlRpcValue::TypeString)
00106     {
00107       ROS_ERROR("target_frame must be a string, but it is XmlRpcType: %d", target_frame_param.getType());
00108       return false;
00109     }
00110     std::string target_frame = target_frame_param;
00111 
00112     // Get the (optional) translational_update_distance
00113     double translational_update_distance = default_translational_update_distance;
00114     if (frame_pair_param.hasMember("translational_update_distance"))
00115     {
00116       XmlRpc::XmlRpcValue translational_update_distance_param = frame_pair_param["translational_update_distance"];
00117       if (translational_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeDouble &&
00118           translational_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeInt)
00119       {
00120         ROS_ERROR("translational_update_distance must be either an integer or a double, but it is XmlRpcType: %d", translational_update_distance_param.getType());
00121         return false;
00122       }
00123       translational_update_distance = translational_update_distance_param;
00124     }
00125 
00126     // Get the (optional) angular_update_distance
00127     double angular_update_distance = default_angular_update_distance;
00128     if (frame_pair_param.hasMember("angular_update_distance"))
00129     {
00130       XmlRpc::XmlRpcValue angular_update_distance_param = frame_pair_param["angular_update_distance"];
00131       if (angular_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeDouble &&
00132           angular_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeInt)
00133       {
00134         ROS_ERROR("angular_update_distance must be either an integer or a double, but it is XmlRpcType: %d", angular_update_distance_param.getType());
00135         return false;
00136       }
00137       angular_update_distance = angular_update_distance_param;
00138     }
00139 
00140     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);
00141 
00142     frame_pairs.push_back(FramePair(source_frame, target_frame, translational_update_distance, angular_update_distance));
00143   }
00144 
00145   return true;
00146 }
00147 
00152 int main(int argc, char** argv)
00153 {
00154   ros::init(argc, argv, "change_notifier", ros::init_options::AnonymousName);
00155   ros::NodeHandle node;
00156   ros::NodeHandle local_node("~");
00157 
00158   double polling_frequency, translational_update_distance, angular_update_distance;
00159   local_node.param(std::string("polling_frequency"),             polling_frequency,             10.0);
00160   local_node.param(std::string("translational_update_distance"), translational_update_distance,  0.10);
00161   local_node.param(std::string("angular_update_distance"),       angular_update_distance,        0.10);
00162 
00163   std::vector<FramePair> frame_pairs;
00164   if (!getFramePairs(local_node, frame_pairs, translational_update_distance, angular_update_distance))
00165     return 1;
00166 
00167   tf::TransformListener tfl(node);
00168 
00169   // Advertise the service
00170   ros::Publisher pub = node.advertise<tf::tfMessage>("tf_changes", 1, true);
00171 
00172   while (node.ok())
00173   {
00174     try
00175     {
00176       tf::tfMessage msg;
00177 
00178       for (std::vector<FramePair>::iterator i = frame_pairs.begin(); i != frame_pairs.end(); i++)
00179       {
00180         FramePair& fp = *i;
00181 
00182         tfl.transformPose(fp.target_frame_, fp.pose_in_, fp.pose_out_);
00183 
00184         const tf::Vector3&    origin   = fp.pose_out_.getOrigin();
00185         const tf::Quaternion& rotation = fp.pose_out_.getRotation();
00186 
00187         if (origin.distance(fp.last_sent_pose_.getOrigin()) > fp.translational_update_distance_ ||
00188             rotation.angle(fp.last_sent_pose_.getRotation()) > fp.angular_update_distance_)
00189         {
00190           fp.last_sent_pose_ = fp.pose_out_;
00191 
00192           tf::StampedTransform stampedTf(tf::Transform(rotation, origin), fp.pose_out_.stamp_, "/" + fp.target_frame_, "/" + fp.source_frame_);
00193           geometry_msgs::TransformStamped msgtf;
00194           transformStampedTFToMsg(stampedTf, msgtf);
00195           msg.transforms.push_back(msgtf);
00196         }
00197       }
00198 
00199       if (msg.transforms.size() > 0)
00200         pub.publish(msg);
00201     }
00202     catch (tf::TransformException& ex)
00203     {
00204       ROS_DEBUG("Exception: %s\n", ex.what());
00205     }
00206     
00207     // Sleep until next polling
00208     if (polling_frequency > 0)
00209       ros::Duration().fromSec(1.0 / polling_frequency).sleep();
00210   }
00211 
00212   return 0;
00213 }


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Aug 11 2017 02:21:55